├── .clang-format ├── .github └── workflows │ └── build.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── apps ├── floor_detection_nodelet.cpp ├── hdl_graph_slam_nodelet.cpp ├── prefiltering_nodelet.cpp └── scan_matching_odometry_nodelet.cpp ├── cmake └── FindG2O.cmake ├── docker ├── build.sh ├── howtouse.md ├── kinetic │ └── Dockerfile ├── kinetic_llvm │ └── Dockerfile ├── melodic │ └── Dockerfile ├── melodic_llvm │ └── Dockerfile ├── noetic │ └── Dockerfile ├── noetic_llvm │ └── Dockerfile └── run.sh ├── imgs ├── birds.png ├── ford1.png ├── ford2.png ├── ford3.png ├── hdl_400_graph.png ├── hdl_400_points.png ├── hdl_graph_slam.png ├── nodelets.png ├── nodelets.vsdx ├── packages.png └── top.png ├── include ├── g2o │ ├── edge_plane_identity.hpp │ ├── edge_plane_parallel.hpp │ ├── edge_plane_prior.hpp │ ├── edge_se3_plane.hpp │ ├── edge_se3_priorquat.hpp │ ├── edge_se3_priorvec.hpp │ ├── edge_se3_priorxy.hpp │ ├── edge_se3_priorxyz.hpp │ └── robust_kernel_io.hpp └── hdl_graph_slam │ ├── graph_slam.hpp │ ├── information_matrix_calculator.hpp │ ├── keyframe.hpp │ ├── keyframe_updater.hpp │ ├── loop_detector.hpp │ ├── map_cloud_generator.hpp │ ├── nmea_sentence_parser.hpp │ ├── registrations.hpp │ ├── ros_time_hash.hpp │ └── ros_utils.hpp ├── launch ├── hdl_graph_slam.launch ├── hdl_graph_slam_400.launch ├── hdl_graph_slam_501.launch ├── hdl_graph_slam_imu.launch ├── hdl_graph_slam_kitti.launch └── msf_config.yaml ├── msg ├── FloorCoeffs.msg └── ScanMatchingStatus.msg ├── nodelet_plugins.xml ├── package.xml ├── rviz └── hdl_graph_slam.rviz ├── setup.py ├── src ├── g2o │ └── robust_kernel_io.cpp └── hdl_graph_slam │ ├── __init__.py │ ├── bag_player.py │ ├── ford2bag.py │ ├── graph_slam.cpp │ ├── information_matrix_calculator.cpp │ ├── keyframe.cpp │ ├── map2odom_publisher.py │ ├── map_cloud_generator.cpp │ └── registrations.cpp └── srv ├── DumpGraph.srv ├── LoadGraph.srv └── SaveMap.srv /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: Empty 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: false 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakBeforeTernaryOperators: true 43 | BreakConstructorInitializersBeforeComma: false 44 | BreakConstructorInitializers: BeforeColon 45 | BreakAfterJavaFieldAnnotations: false 46 | BreakStringLiterals: false 47 | ColumnLimit: 256 48 | CommentPragmas: '^ IWYU pragma:' 49 | CompactNamespaces: false 50 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 51 | ConstructorInitializerIndentWidth: 4 52 | ContinuationIndentWidth: 4 53 | Cpp11BracedListStyle: true 54 | DerivePointerAlignment: true 55 | DisableFormat: false 56 | ExperimentalAutoDetectBinPacking: false 57 | FixNamespaceComments: true 58 | ForEachMacros: 59 | - foreach 60 | - Q_FOREACH 61 | - BOOST_FOREACH 62 | IncludeBlocks: Preserve 63 | IncludeCategories: 64 | - Regex: '^' 65 | Priority: 2 66 | - Regex: '^<.*\.h>' 67 | Priority: 1 68 | - Regex: '^<.*' 69 | Priority: 2 70 | - Regex: '.*' 71 | Priority: 3 72 | IncludeIsMainRegex: '([-_](test|unittest))?$' 73 | IndentCaseLabels: true 74 | IndentPPDirectives: None 75 | IndentWidth: 2 76 | IndentWrappedFunctionNames: false 77 | JavaScriptQuotes: Leave 78 | JavaScriptWrapImports: true 79 | KeepEmptyLinesAtTheStartOfBlocks: false 80 | MacroBlockBegin: '' 81 | MacroBlockEnd: '' 82 | MaxEmptyLinesToKeep: 1 83 | NamespaceIndentation: None 84 | ObjCBlockIndentWidth: 2 85 | ObjCSpaceAfterProperty: false 86 | ObjCSpaceBeforeProtocolList: false 87 | PenaltyBreakAssignment: 2 88 | PenaltyBreakBeforeFirstCallParameter: 1 89 | PenaltyBreakComment: 300 90 | PenaltyBreakFirstLessLess: 120 91 | PenaltyBreakString: 1000 92 | PenaltyExcessCharacter: 0 93 | PenaltyReturnTypeOnItsOwnLine: 200 94 | PointerAlignment: Left 95 | RawStringFormats: 96 | - Delimiter: pb 97 | Language: TextProto 98 | BasedOnStyle: google 99 | ReflowComments: true 100 | SortIncludes: false 101 | SortUsingDeclarations: false 102 | SpaceAfterCStyleCast: false 103 | SpaceAfterTemplateKeyword: false 104 | SpaceBeforeAssignmentOperators: true 105 | SpaceBeforeParens: Never 106 | SpaceInEmptyParentheses: false 107 | SpacesBeforeTrailingComments: 2 108 | SpacesInAngles: false 109 | SpacesInContainerLiterals: true 110 | SpacesInCStyleCastParentheses: false 111 | SpacesInParentheses: false 112 | SpacesInSquareBrackets: false 113 | Standard: Auto 114 | TabWidth: 8 115 | UseTab: Never 116 | ... 117 | 118 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | # Allows you to run this workflow manually from the Actions tab 10 | workflow_dispatch: 11 | 12 | jobs: 13 | build: 14 | runs-on: ubuntu-latest 15 | strategy: 16 | matrix: 17 | ROS_DISTRO: [melodic, melodic_llvm, noetic, noetic_llvm] 18 | 19 | steps: 20 | - uses: actions/checkout@v2 21 | 22 | - name: Docker login 23 | continue-on-error: true 24 | uses: docker/login-action@v1 25 | with: 26 | username: ${{ secrets.DOCKER_USERNAME }} 27 | password: ${{ secrets.DOCKER_TOKEN }} 28 | 29 | - name: Docker build 30 | uses: docker/build-push-action@v2 31 | with: 32 | file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile 33 | context: . 34 | push: false 35 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | imgui.ini 3 | rviz/* 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: BSD-2-Clause 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(hdl_graph_slam) 4 | 5 | # Can we use C++17 in indigo? 6 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") 7 | add_definitions(-std=c++11) 8 | set(CMAKE_CXX_FLAGS "-std=c++11") 9 | else() 10 | add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 11 | set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 12 | endif() 13 | 14 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 15 | 16 | # pcl 1.7 causes a segfault when it is built with debug mode 17 | set(CMAKE_BUILD_TYPE "RELEASE") 18 | 19 | find_package(catkin REQUIRED COMPONENTS 20 | fast_gicp 21 | geodesy 22 | geometry_msgs 23 | interactive_markers 24 | message_generation 25 | ndt_omp 26 | nmea_msgs 27 | pcl_ros 28 | roscpp 29 | rospy 30 | sensor_msgs 31 | std_msgs 32 | tf_conversions 33 | ) 34 | catkin_python_setup() 35 | 36 | find_package(PCL REQUIRED) 37 | include_directories(${PCL_INCLUDE_DIRS}) 38 | link_directories(${PCL_LIBRARY_DIRS}) 39 | add_definitions(${PCL_DEFINITIONS}) 40 | 41 | message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) 42 | message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) 43 | message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) 44 | 45 | find_package(G2O REQUIRED) 46 | include_directories(SYSTEM ${G2O_INCLUDE_DIR} ${G2O_INCLUDE_DIRS}) 47 | link_directories(${G2O_LIBRARY_DIRS}) 48 | # link_libraries(${G2O_LIBRARIES}) 49 | 50 | find_package(OpenMP) 51 | if (OPENMP_FOUND) 52 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 53 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 54 | endif() 55 | 56 | find_library(VGICP_CUDA_FOUND NAMES fast_vgicp_cuda) 57 | message(STATUS "VGICP_CUDA_FOUND:" ${VGICP_CUDA_FOUND}) 58 | if(VGICP_CUDA_FOUND) 59 | add_definitions(-DUSE_VGICP_CUDA) 60 | endif() 61 | 62 | ######################## 63 | ## message generation ## 64 | ######################## 65 | add_message_files(FILES 66 | FloorCoeffs.msg 67 | ScanMatchingStatus.msg 68 | ) 69 | 70 | add_service_files(FILES 71 | SaveMap.srv 72 | LoadGraph.srv 73 | DumpGraph.srv 74 | ) 75 | 76 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 77 | 78 | ################################### 79 | ## catkin specific configuration ## 80 | ################################### 81 | catkin_package( 82 | INCLUDE_DIRS include 83 | LIBRARIES hdl_graph_slam_nodelet 84 | CATKIN_DEPENDS 85 | geometry_msgs 86 | message_runtime 87 | nmea_msgs 88 | roscpp 89 | sensor_msgs 90 | std_msgs 91 | tf_conversions 92 | # DEPENDS system_lib 93 | ) 94 | 95 | ########### 96 | ## Build ## 97 | ########### 98 | include_directories(include) 99 | include_directories( 100 | ${PCL_INCLUDE_DIRS} 101 | ${catkin_INCLUDE_DIRS} 102 | ) 103 | 104 | # nodelets 105 | add_library(prefiltering_nodelet apps/prefiltering_nodelet.cpp) 106 | target_link_libraries(prefiltering_nodelet 107 | ${catkin_LIBRARIES} 108 | ${PCL_LIBRARIES} 109 | ) 110 | 111 | 112 | add_library(floor_detection_nodelet apps/floor_detection_nodelet.cpp) 113 | target_link_libraries(floor_detection_nodelet 114 | ${catkin_LIBRARIES} 115 | ${PCL_LIBRARIES} 116 | ) 117 | add_dependencies(floor_detection_nodelet ${PROJECT_NAME}_gencpp) 118 | 119 | 120 | add_library(scan_matching_odometry_nodelet 121 | apps/scan_matching_odometry_nodelet.cpp 122 | src/hdl_graph_slam/registrations.cpp 123 | ) 124 | target_link_libraries(scan_matching_odometry_nodelet 125 | ${catkin_LIBRARIES} 126 | ${PCL_LIBRARIES} 127 | ) 128 | add_dependencies(scan_matching_odometry_nodelet ${PROJECT_NAME}_gencpp) 129 | 130 | 131 | add_library(hdl_graph_slam_nodelet 132 | apps/hdl_graph_slam_nodelet.cpp 133 | src/hdl_graph_slam/graph_slam.cpp 134 | src/hdl_graph_slam/keyframe.cpp 135 | src/hdl_graph_slam/map_cloud_generator.cpp 136 | src/hdl_graph_slam/registrations.cpp 137 | src/hdl_graph_slam/information_matrix_calculator.cpp 138 | src/g2o/robust_kernel_io.cpp 139 | ) 140 | target_link_libraries(hdl_graph_slam_nodelet 141 | ${catkin_LIBRARIES} 142 | ${PCL_LIBRARIES} 143 | ${G2O_TYPES_DATA} 144 | ${G2O_CORE_LIBRARY} 145 | ${G2O_STUFF_LIBRARY} 146 | ${G2O_SOLVER_PCG} 147 | ${G2O_SOLVER_CSPARSE} # be aware of that CSPARSE is released under LGPL 148 | ${G2O_SOLVER_CHOLMOD} # be aware of that cholmod is released under GPL 149 | ${G2O_TYPES_SLAM3D} 150 | ${G2O_TYPES_SLAM3D_ADDONS} 151 | ) 152 | add_dependencies(hdl_graph_slam_nodelet ${PROJECT_NAME}_gencpp) 153 | 154 | catkin_install_python( 155 | PROGRAMS 156 | src/${PROJECT_NAME}/bag_player.py 157 | src/${PROJECT_NAME}/ford2bag.py 158 | src/${PROJECT_NAME}/map2odom_publisher.py 159 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | ) 161 | 162 | install(FILES nodelet_plugins.xml 163 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 164 | ) 165 | 166 | install(TARGETS 167 | prefiltering_nodelet 168 | floor_detection_nodelet 169 | scan_matching_odometry_nodelet 170 | hdl_graph_slam_nodelet 171 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 172 | 173 | install(DIRECTORY include/ 174 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 175 | ) 176 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, k.koide 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # New SLAM package is released 2 | 3 | A new 3D SLAM package is released: https://github.com/koide3/glim. 4 | 5 | # hdl_graph_slam 6 | ***hdl_graph_slam*** is an open source ROS package for real-time 6DOF SLAM using a 3D LIDAR. It is based on 3D Graph SLAM with NDT scan matching-based odometry estimation and loop detection. It also supports several graph constraints, such as GPS, IMU acceleration (gravity vector), IMU orientation (magnetic sensor), and floor plane (detected in a point cloud). We have tested this package with Velodyne (HDL32e, VLP16) and RoboSense (16 channels) sensors in indoor and outdoor environments. 7 | 8 | 9 | 10 | [![Build](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml) on melodic & noetic 11 | 12 | ## Third-party extensions 13 | 14 | See also the following nice works built upon hdl_graph_slam. Feel free to request to include your work in the list :) 15 | 16 | - Multi-Robot Mapping (ROS2) developed by [Andreas Serov](https://github.com/aserbremen) : [aserbremen/Multi-Robot-Graph-SLAM](https://github.com/aserbremen/Multi-Robot-Graph-SLAM) 17 | - [CURB-SG: Collaborative Dynamic 3D Scene Graphs](http://curb.cs.uni-freiburg.de/) developed by Elias Greve : [robot-learning-freiburg/CURB-SG](https://github.com/robot-learning-freiburg/CURB-SG) 18 | 19 | ## Nodelets 20 | ***hdl_graph_slam*** consists of four nodelets. 21 | 22 | - *prefiltering_nodelet* 23 | - *scan_matching_odometry_nodelet* 24 | - *floor_detection_nodelet* 25 | - *hdl_graph_slam_nodelet* 26 | 27 | The input point cloud is first downsampled by *prefiltering_nodelet*, and then passed to the next nodelets. While *scan_matching_odometry_nodelet* estimates the sensor pose by iteratively applying a scan matching between consecutive frames (i.e., odometry estimation), *floor_detection_nodelet* detects floor planes by RANSAC. The estimated odometry and the detected floor planes are sent to *hdl_graph_slam*. To compensate the accumulated error of the scan matching, it performs loop detection and optimizes a pose graph which takes various constraints into account. 28 | 29 | 30 | 31 | ## Constraints (Edges) 32 | 33 | You can enable/disable each constraint by changing params in the launch file, and you can also change the weight (\*_stddev) and the robust kernel (\*_robust_kernel) of each constraint. 34 | 35 | - ***Odometry*** 36 | 37 | - ***Loop closure*** 38 | 39 | - ***GPS*** 40 | - */gps/geopoint* (geographic_msgs/GeoPointStamped) 41 | - */gps/navsat* (sensor_msgs/NavSatFix) 42 | - */gpsimu_driver/nmea_sentence* (nmea_msgs/Sentence) 43 | 44 | hdl_graph_slam supports several GPS message types. All the supported types contain (latitude, longitude, and altitude). hdl_graph_slam converts them into [the UTM coordinate](http://wiki.ros.org/geodesy), and adds them into the graph as 3D position constraints. If altitude is set to NaN, the GPS data is treated as a 2D constrait. GeoPoint is the most basic one, which consists of only (lat, lon, alt). Although NavSatFix provides many information, we use only (lat, lon, alt) and ignore all other data. If you're using HDL32e, you can directly connect *hdl_graph_slam* with *velodyne_driver* via */gpsimu_driver/nmea_sentence*. 45 | 46 | - ***IMU acceleration (gravity vector)*** 47 | - */gpsimu_driver/imu_data* (sensor_msgs/Imu) 48 | 49 | This constraint rotates each pose node so that the acceleration vector associated with the node becomes vertical (as the gravity vector). This is useful to compensate for accumulated tilt rotation errors of the scan matching. Since we ignore acceleration by sensor motion, you should not give a big weight for this constraint. 50 | 51 | - ***IMU orientation (magnetic sensor)*** 52 | - */gpsimu_driver/imu_data* (sensor_msgs/Imu) 53 | 54 | If your IMU has a reliable magnetic orientation sensor, you can add orientation data to the graph as 3D rotation constraints. Note that, magnetic orientation sensors can be affected by external magnetic disturbances. In such cases, this constraint should be disabled. 55 | 56 | - ***Floor plane*** 57 | - */floor_detection/floor_coeffs* (hdl_graph_slam/FloorCoeffs) 58 | 59 | This constraint optimizes the graph so that the floor planes (detected by RANSAC) of the pose nodes becomes the same. This is designed to compensate the accumulated rotation error of the scan matching in large flat indoor environments. 60 | 61 | 62 | ## Parameters 63 | All the configurable parameters are listed in *launch/hdl_graph_slam.launch* as ros params. 64 | 65 | ## Services 66 | - */hdl_graph_slam/dump* (hdl_graph_slam/DumpGraph) 67 | - save all the internal data (point clouds, floor coeffs, odoms, and pose graph) to a directory. 68 | - */hdl_graph_slam/save_map* (hdl_graph_slam/SaveMap) 69 | - save the generated map as a PCD file. 70 | 71 | ## Requirements 72 | ***hdl_graph_slam*** requires the following libraries: 73 | 74 | - OpenMP 75 | - PCL 76 | - g2o 77 | - suitesparse 78 | 79 | The following ROS packages are required: 80 | 81 | - geodesy 82 | - nmea_msgs 83 | - pcl_ros 84 | - [ndt_omp](https://github.com/koide3/ndt_omp) 85 | - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) 86 | 87 | ```bash 88 | # for melodic 89 | sudo apt-get install ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-libg2o 90 | cd catkin_ws/src 91 | git clone https://github.com/koide3/ndt_omp.git -b melodic 92 | git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 93 | git clone https://github.com/koide3/hdl_graph_slam 94 | 95 | cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release 96 | 97 | # for noetic 98 | sudo apt-get install ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs ros-noetic-libg2o 99 | 100 | cd catkin_ws/src 101 | git clone https://github.com/koide3/ndt_omp.git 102 | git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 103 | git clone https://github.com/koide3/hdl_graph_slam 104 | 105 | cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release 106 | ``` 107 | 108 | **[optional]** *bag_player.py* script requires ProgressBar2. 109 | ```bash 110 | sudo pip install ProgressBar2 111 | ``` 112 | 113 | ## Example1 (Indoor) 114 | 115 | Bag file (recorded in a small room): 116 | 117 | - [hdl_501.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501.bag.tar.gz) (raw data, 344MB) 118 | - [hdl_501_filtered.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501_filtered.bag.tar.gz) (downsampled data, 57MB, **Recommended!**) 119 | - [Mirror link](https://zenodo.org/record/6960371) 120 | 121 | ```bash 122 | rosparam set use_sim_time true 123 | roslaunch hdl_graph_slam hdl_graph_slam_501.launch 124 | ``` 125 | 126 | ```bash 127 | roscd hdl_graph_slam/rviz 128 | rviz -d hdl_graph_slam.rviz 129 | ``` 130 | 131 | ```bash 132 | rosbag play --clock hdl_501_filtered.bag 133 | ``` 134 | 135 | We also provide bag_player.py which automatically adjusts the playback speed and processes data as fast as possible. 136 | 137 | ```bash 138 | rosrun hdl_graph_slam bag_player.py hdl_501_filtered.bag 139 | ``` 140 | 141 | You'll see a point cloud like: 142 | 143 | 144 | 145 | You can save the generated map by: 146 | ```bash 147 | rosservice call /hdl_graph_slam/save_map "resolution: 0.05 148 | destination: '/full_path_directory/map.pcd'" 149 | ``` 150 | 151 | ## Example2 (Outdoor) 152 | 153 | Bag file (recorded in an outdoor environment): 154 | - [hdl_400.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz) (raw data, about 900MB) 155 | - [Mirror link](https://zenodo.org/record/6960371) 156 | 157 | ```bash 158 | rosparam set use_sim_time true 159 | roslaunch hdl_graph_slam hdl_graph_slam_400.launch 160 | ``` 161 | 162 | ```bash 163 | roscd hdl_graph_slam/rviz 164 | rviz -d hdl_graph_slam.rviz 165 | ``` 166 | 167 | ```bash 168 | rosbag play --clock hdl_400.bag 169 | ``` 170 | 171 | 172 | 173 | ## Example with GPS 174 | Ford Campus Vision and Lidar Data Set [\[URL\]](http://robots.engin.umich.edu/SoftwareData/Ford) 175 | 176 | The following script converts the Ford Lidar Dataset to a rosbag and plays it. In this example, ***hdl_graph_slam*** utilizes the GPS data to correct the pose graph. 177 | 178 | ```bash 179 | cd IJRR-Dataset-2 180 | rosrun hdl_graph_slam ford2bag.py dataset-2.bag 181 | rosrun hdl_graph_slam bag_player.py dataset-2.bag 182 | ``` 183 | 184 | 185 | 186 | ## Use hdl_graph_slam in your system 187 | 188 | 1. Define the transformation between your sensors (LIDAR, IMU, GPS) and base_link of your system using static_transform_publisher (see line #11, hdl_graph_slam.launch). All the sensor data will be transformed into the common base_link frame, and then fed to the SLAM algorithm. 189 | 190 | 2. Remap the point cloud topic of ***prefiltering_nodelet***. Like: 191 | 192 | ```bash 193 | 195 | ... 196 | ``` 197 | 198 | ## Common Problems 199 | 200 | ### Parameter tuning guide 201 | 202 | The mapping quality largely depends on the parameter setting. In particular, scan matching parameters have a big impact on the result. Tune the parameters accoding to the following instructions: 203 | 204 | - ***registration_method*** 205 | **[updated] In short, use FAST_GICP for most cases and FAST_VGICP or NDT_OMP if the processing speed matters** This parameter allows to change the registration method to be used for odometry estimation and loop detection. Note that GICP in PCL1.7 (ROS kinetic) or earlier has a bug in the initial guess handling. **If you are on ROS kinectic or earlier, do not use GICP**. 206 | 207 | - ***ndt_resolution*** 208 | This parameter decides the voxel size of NDT. Typically larger values are good for outdoor environements (0.5 - 2.0 [m] for indoor, 2.0 - 10.0 [m] for outdoor). If you chose NDT or NDT_OMP, tweak this parameter so you can obtain a good odometry estimation result. 209 | 210 | - ***other parameters*** 211 | All the configurable parameters are available in the launch file. Copy a template launch file (hdl_graph_slam_501.launch for indoor, hdl_graph_slam_400.launch for outdoor) and tweak parameters in the launch file to adapt it to your application. 212 | 213 | ## License 214 | 215 | This package is released under the BSD-2-Clause License. 216 | 217 | 218 | Note that the cholmod solver in g2o is licensed under GPL. You may need to build g2o without cholmod dependency to avoid the GPL. 219 | 220 | ## Related packages 221 | 222 | - [interactive_slam](https://github.com/koide3/interactive_slam) 223 | - [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam) 224 | - [hdl_localization](https://github.com/koide3/hdl_localization) 225 | - [hdl_people_tracking](https://github.com/koide3/hdl_people_tracking) 226 | 227 | 228 | 229 | ## Papers 230 | Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019 [[link]](https://www.researchgate.net/publication/331283709_A_Portable_3D_LIDAR-based_System_for_Long-term_and_Wide-area_People_Behavior_Measurement). 231 | 232 | ## Contact 233 | Kenji Koide, k.koide@aist.go.jp, https://staff.aist.go.jp/k.koide 234 | 235 | Active Intelligent Systems Laboratory, Toyohashi University of Technology, Japan [\[URL\]](http://www.aisl.cs.tut.ac.jp) 236 | Mobile Robotics Research Team, National Institute of Advanced Industrial Science and Technology (AIST), Japan [\[URL\]](https://unit.aist.go.jp/hcmrc/mr-rt/contact.html) 237 | -------------------------------------------------------------------------------- /apps/floor_detection_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace hdl_graph_slam { 28 | 29 | class FloorDetectionNodelet : public nodelet::Nodelet { 30 | public: 31 | typedef pcl::PointXYZI PointT; 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | 34 | FloorDetectionNodelet() {} 35 | virtual ~FloorDetectionNodelet() {} 36 | 37 | virtual void onInit() { 38 | NODELET_DEBUG("initializing floor_detection_nodelet..."); 39 | nh = getNodeHandle(); 40 | private_nh = getPrivateNodeHandle(); 41 | 42 | initialize_params(); 43 | 44 | points_sub = nh.subscribe("/filtered_points", 256, &FloorDetectionNodelet::cloud_callback, this); 45 | floor_pub = nh.advertise("/floor_detection/floor_coeffs", 32); 46 | 47 | read_until_pub = nh.advertise("/floor_detection/read_until", 32); 48 | floor_filtered_pub = nh.advertise("/floor_detection/floor_filtered_points", 32); 49 | floor_points_pub = nh.advertise("/floor_detection/floor_points", 32); 50 | } 51 | 52 | private: 53 | /** 54 | * @brief initialize parameters 55 | */ 56 | void initialize_params() { 57 | tilt_deg = private_nh.param("tilt_deg", 0.0); // approximate sensor tilt angle [deg] 58 | sensor_height = private_nh.param("sensor_height", 2.0); // approximate sensor height [m] 59 | height_clip_range = private_nh.param("height_clip_range", 1.0); // points with heights in [sensor_height - height_clip_range, sensor_height + height_clip_range] will be used for floor detection 60 | floor_pts_thresh = private_nh.param("floor_pts_thresh", 512); // minimum number of support points of RANSAC to accept a detected floor plane 61 | floor_normal_thresh = private_nh.param("floor_normal_thresh", 10.0); // verticality check thresold for the detected floor plane [deg] 62 | use_normal_filtering = private_nh.param("use_normal_filtering", true); // if true, points with "non-"vertical normals will be filtered before RANSAC 63 | normal_filter_thresh = private_nh.param("normal_filter_thresh", 20.0); // "non-"verticality check threshold [deg] 64 | 65 | points_topic = private_nh.param("points_topic", "/velodyne_points"); 66 | } 67 | 68 | /** 69 | * @brief callback for point clouds 70 | * @param cloud_msg point cloud msg 71 | */ 72 | void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { 73 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 74 | pcl::fromROSMsg(*cloud_msg, *cloud); 75 | 76 | if(cloud->empty()) { 77 | return; 78 | } 79 | 80 | // floor detection 81 | boost::optional floor = detect(cloud); 82 | 83 | // publish the detected floor coefficients 84 | hdl_graph_slam::FloorCoeffs coeffs; 85 | coeffs.header = cloud_msg->header; 86 | if(floor) { 87 | coeffs.coeffs.resize(4); 88 | for(int i = 0; i < 4; i++) { 89 | coeffs.coeffs[i] = (*floor)[i]; 90 | } 91 | } 92 | 93 | floor_pub.publish(coeffs); 94 | 95 | // for offline estimation 96 | std_msgs::HeaderPtr read_until(new std_msgs::Header()); 97 | read_until->frame_id = points_topic; 98 | read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0); 99 | read_until_pub.publish(read_until); 100 | 101 | read_until->frame_id = "/filtered_points"; 102 | read_until_pub.publish(read_until); 103 | } 104 | 105 | /** 106 | * @brief detect the floor plane from a point cloud 107 | * @param cloud input cloud 108 | * @return detected floor plane coefficients 109 | */ 110 | boost::optional detect(const pcl::PointCloud::Ptr& cloud) const { 111 | // compensate the tilt rotation 112 | Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity(); 113 | tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix(); 114 | 115 | // filtering before RANSAC (height and normal filtering) 116 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud); 117 | pcl::transformPointCloud(*cloud, *filtered, tilt_matrix); 118 | filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false); 119 | filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true); 120 | 121 | if(use_normal_filtering) { 122 | filtered = normal_filtering(filtered); 123 | } 124 | 125 | pcl::transformPointCloud(*filtered, *filtered, static_cast(tilt_matrix.inverse())); 126 | 127 | if(floor_filtered_pub.getNumSubscribers()) { 128 | filtered->header = cloud->header; 129 | floor_filtered_pub.publish(*filtered); 130 | } 131 | 132 | // too few points for RANSAC 133 | if(filtered->size() < floor_pts_thresh) { 134 | return boost::none; 135 | } 136 | 137 | // RANSAC 138 | pcl::SampleConsensusModelPlane::Ptr model_p(new pcl::SampleConsensusModelPlane(filtered)); 139 | pcl::RandomSampleConsensus ransac(model_p); 140 | ransac.setDistanceThreshold(0.1); 141 | ransac.computeModel(); 142 | 143 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 144 | ransac.getInliers(inliers->indices); 145 | 146 | // too few inliers 147 | if(inliers->indices.size() < floor_pts_thresh) { 148 | return boost::none; 149 | } 150 | 151 | // verticality check of the detected floor's normal 152 | Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ(); 153 | 154 | Eigen::VectorXf coeffs; 155 | ransac.getModelCoefficients(coeffs); 156 | 157 | double dot = coeffs.head<3>().dot(reference.head<3>()); 158 | if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) { 159 | // the normal is not vertical 160 | return boost::none; 161 | } 162 | 163 | // make the normal upward 164 | if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) { 165 | coeffs *= -1.0f; 166 | } 167 | 168 | if(floor_points_pub.getNumSubscribers()) { 169 | pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud); 170 | pcl::ExtractIndices extract; 171 | extract.setInputCloud(filtered); 172 | extract.setIndices(inliers); 173 | extract.filter(*inlier_cloud); 174 | inlier_cloud->header = cloud->header; 175 | 176 | floor_points_pub.publish(*inlier_cloud); 177 | } 178 | 179 | return Eigen::Vector4f(coeffs); 180 | } 181 | 182 | /** 183 | * @brief plane_clip 184 | * @param src_cloud 185 | * @param plane 186 | * @param negative 187 | * @return 188 | */ 189 | pcl::PointCloud::Ptr plane_clip(const pcl::PointCloud::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative) const { 190 | pcl::PlaneClipper3D clipper(plane); 191 | pcl::PointIndices::Ptr indices(new pcl::PointIndices); 192 | 193 | clipper.clipPointCloud3D(*src_cloud, indices->indices); 194 | 195 | pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud); 196 | 197 | pcl::ExtractIndices extract; 198 | extract.setInputCloud(src_cloud); 199 | extract.setIndices(indices); 200 | extract.setNegative(negative); 201 | extract.filter(*dst_cloud); 202 | 203 | return dst_cloud; 204 | } 205 | 206 | /** 207 | * @brief filter points with non-vertical normals 208 | * @param cloud input cloud 209 | * @return filtered cloud 210 | */ 211 | pcl::PointCloud::Ptr normal_filtering(const pcl::PointCloud::Ptr& cloud) const { 212 | pcl::NormalEstimation ne; 213 | ne.setInputCloud(cloud); 214 | 215 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); 216 | ne.setSearchMethod(tree); 217 | 218 | pcl::PointCloud::Ptr normals(new pcl::PointCloud); 219 | ne.setKSearch(10); 220 | ne.setViewPoint(0.0f, 0.0f, sensor_height); 221 | ne.compute(*normals); 222 | 223 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud); 224 | filtered->reserve(cloud->size()); 225 | 226 | for(int i = 0; i < cloud->size(); i++) { 227 | float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ()); 228 | if(std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) { 229 | filtered->push_back(cloud->at(i)); 230 | } 231 | } 232 | 233 | filtered->width = filtered->size(); 234 | filtered->height = 1; 235 | filtered->is_dense = false; 236 | 237 | return filtered; 238 | } 239 | 240 | private: 241 | ros::NodeHandle nh; 242 | ros::NodeHandle private_nh; 243 | 244 | // ROS topics 245 | ros::Subscriber points_sub; 246 | 247 | ros::Publisher floor_pub; 248 | ros::Publisher floor_points_pub; 249 | ros::Publisher floor_filtered_pub; 250 | 251 | std::string points_topic; 252 | ros::Publisher read_until_pub; 253 | 254 | // floor detection parameters 255 | // see initialize_params() for the details 256 | double tilt_deg; 257 | double sensor_height; 258 | double height_clip_range; 259 | 260 | int floor_pts_thresh; 261 | double floor_normal_thresh; 262 | 263 | bool use_normal_filtering; 264 | double normal_filter_thresh; 265 | }; 266 | 267 | } // namespace hdl_graph_slam 268 | 269 | PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::FloorDetectionNodelet, nodelet::Nodelet) 270 | -------------------------------------------------------------------------------- /apps/prefiltering_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace hdl_graph_slam { 26 | 27 | class PrefilteringNodelet : public nodelet::Nodelet { 28 | public: 29 | typedef pcl::PointXYZI PointT; 30 | 31 | PrefilteringNodelet() {} 32 | virtual ~PrefilteringNodelet() {} 33 | 34 | virtual void onInit() { 35 | nh = getNodeHandle(); 36 | private_nh = getPrivateNodeHandle(); 37 | 38 | initialize_params(); 39 | 40 | if(private_nh.param("deskewing", false)) { 41 | imu_sub = nh.subscribe("/imu/data", 1, &PrefilteringNodelet::imu_callback, this); 42 | } 43 | 44 | points_sub = nh.subscribe("/velodyne_points", 64, &PrefilteringNodelet::cloud_callback, this); 45 | points_pub = nh.advertise("/filtered_points", 32); 46 | colored_pub = nh.advertise("/colored_points", 32); 47 | } 48 | 49 | private: 50 | void initialize_params() { 51 | std::string downsample_method = private_nh.param("downsample_method", "VOXELGRID"); 52 | double downsample_resolution = private_nh.param("downsample_resolution", 0.1); 53 | 54 | if(downsample_method == "VOXELGRID") { 55 | std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; 56 | auto voxelgrid = new pcl::VoxelGrid(); 57 | voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 58 | downsample_filter.reset(voxelgrid); 59 | } else if(downsample_method == "APPROX_VOXELGRID") { 60 | std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; 61 | pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid()); 62 | approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 63 | downsample_filter = approx_voxelgrid; 64 | } else { 65 | if(downsample_method != "NONE") { 66 | std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl; 67 | std::cerr << " : use passthrough filter" << std::endl; 68 | } 69 | std::cout << "downsample: NONE" << std::endl; 70 | } 71 | 72 | std::string outlier_removal_method = private_nh.param("outlier_removal_method", "STATISTICAL"); 73 | if(outlier_removal_method == "STATISTICAL") { 74 | int mean_k = private_nh.param("statistical_mean_k", 20); 75 | double stddev_mul_thresh = private_nh.param("statistical_stddev", 1.0); 76 | std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl; 77 | 78 | pcl::StatisticalOutlierRemoval::Ptr sor(new pcl::StatisticalOutlierRemoval()); 79 | sor->setMeanK(mean_k); 80 | sor->setStddevMulThresh(stddev_mul_thresh); 81 | outlier_removal_filter = sor; 82 | } else if(outlier_removal_method == "RADIUS") { 83 | double radius = private_nh.param("radius_radius", 0.8); 84 | int min_neighbors = private_nh.param("radius_min_neighbors", 2); 85 | std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl; 86 | 87 | pcl::RadiusOutlierRemoval::Ptr rad(new pcl::RadiusOutlierRemoval()); 88 | rad->setRadiusSearch(radius); 89 | rad->setMinNeighborsInRadius(min_neighbors); 90 | outlier_removal_filter = rad; 91 | } else { 92 | std::cout << "outlier_removal: NONE" << std::endl; 93 | } 94 | 95 | use_distance_filter = private_nh.param("use_distance_filter", true); 96 | distance_near_thresh = private_nh.param("distance_near_thresh", 1.0); 97 | distance_far_thresh = private_nh.param("distance_far_thresh", 100.0); 98 | 99 | base_link_frame = private_nh.param("base_link_frame", ""); 100 | } 101 | 102 | void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) { 103 | imu_queue.push_back(imu_msg); 104 | } 105 | 106 | void cloud_callback(const pcl::PointCloud& src_cloud_r) { 107 | pcl::PointCloud::ConstPtr src_cloud = src_cloud_r.makeShared(); 108 | if(src_cloud->empty()) { 109 | return; 110 | } 111 | 112 | src_cloud = deskewing(src_cloud); 113 | 114 | // if base_link_frame is defined, transform the input cloud to the frame 115 | if(!base_link_frame.empty()) { 116 | if(!tf_listener.canTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0))) { 117 | std::cerr << "failed to find transform between " << base_link_frame << " and " << src_cloud->header.frame_id << std::endl; 118 | } 119 | 120 | tf::StampedTransform transform; 121 | tf_listener.waitForTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0), ros::Duration(2.0)); 122 | tf_listener.lookupTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0), transform); 123 | 124 | pcl::PointCloud::Ptr transformed(new pcl::PointCloud()); 125 | pcl_ros::transformPointCloud(*src_cloud, *transformed, transform); 126 | transformed->header.frame_id = base_link_frame; 127 | transformed->header.stamp = src_cloud->header.stamp; 128 | src_cloud = transformed; 129 | } 130 | 131 | pcl::PointCloud::ConstPtr filtered = distance_filter(src_cloud); 132 | filtered = downsample(filtered); 133 | filtered = outlier_removal(filtered); 134 | 135 | points_pub.publish(*filtered); 136 | } 137 | 138 | pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { 139 | if(!downsample_filter) { 140 | return cloud; 141 | } 142 | 143 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 144 | downsample_filter->setInputCloud(cloud); 145 | downsample_filter->filter(*filtered); 146 | filtered->header = cloud->header; 147 | 148 | return filtered; 149 | } 150 | 151 | pcl::PointCloud::ConstPtr outlier_removal(const pcl::PointCloud::ConstPtr& cloud) const { 152 | if(!outlier_removal_filter) { 153 | return cloud; 154 | } 155 | 156 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 157 | outlier_removal_filter->setInputCloud(cloud); 158 | outlier_removal_filter->filter(*filtered); 159 | filtered->header = cloud->header; 160 | 161 | return filtered; 162 | } 163 | 164 | pcl::PointCloud::ConstPtr distance_filter(const pcl::PointCloud::ConstPtr& cloud) const { 165 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 166 | filtered->reserve(cloud->size()); 167 | 168 | std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), [&](const PointT& p) { 169 | double d = p.getVector3fMap().norm(); 170 | return d > distance_near_thresh && d < distance_far_thresh; 171 | }); 172 | 173 | filtered->width = filtered->size(); 174 | filtered->height = 1; 175 | filtered->is_dense = false; 176 | 177 | filtered->header = cloud->header; 178 | 179 | return filtered; 180 | } 181 | 182 | pcl::PointCloud::ConstPtr deskewing(const pcl::PointCloud::ConstPtr& cloud) { 183 | ros::Time stamp = pcl_conversions::fromPCL(cloud->header.stamp); 184 | if(imu_queue.empty()) { 185 | return cloud; 186 | } 187 | 188 | // the color encodes the point number in the point sequence 189 | if(colored_pub.getNumSubscribers()) { 190 | pcl::PointCloud::Ptr colored(new pcl::PointCloud()); 191 | colored->header = cloud->header; 192 | colored->is_dense = cloud->is_dense; 193 | colored->width = cloud->width; 194 | colored->height = cloud->height; 195 | colored->resize(cloud->size()); 196 | 197 | for(int i = 0; i < cloud->size(); i++) { 198 | double t = static_cast(i) / cloud->size(); 199 | colored->at(i).getVector4fMap() = cloud->at(i).getVector4fMap(); 200 | colored->at(i).r = 255 * t; 201 | colored->at(i).g = 128; 202 | colored->at(i).b = 255 * (1 - t); 203 | } 204 | colored_pub.publish(*colored); 205 | } 206 | 207 | sensor_msgs::ImuConstPtr imu_msg = imu_queue.front(); 208 | 209 | auto loc = imu_queue.begin(); 210 | for(; loc != imu_queue.end(); loc++) { 211 | imu_msg = (*loc); 212 | if((*loc)->header.stamp > stamp) { 213 | break; 214 | } 215 | } 216 | 217 | imu_queue.erase(imu_queue.begin(), loc); 218 | 219 | Eigen::Vector3f ang_v(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); 220 | ang_v *= -1; 221 | 222 | pcl::PointCloud::Ptr deskewed(new pcl::PointCloud()); 223 | deskewed->header = cloud->header; 224 | deskewed->is_dense = cloud->is_dense; 225 | deskewed->width = cloud->width; 226 | deskewed->height = cloud->height; 227 | deskewed->resize(cloud->size()); 228 | 229 | double scan_period = private_nh.param("scan_period", 0.1); 230 | for(int i = 0; i < cloud->size(); i++) { 231 | const auto& pt = cloud->at(i); 232 | 233 | // TODO: transform IMU data into the LIDAR frame 234 | double delta_t = scan_period * static_cast(i) / cloud->size(); 235 | Eigen::Quaternionf delta_q(1, delta_t / 2.0 * ang_v[0], delta_t / 2.0 * ang_v[1], delta_t / 2.0 * ang_v[2]); 236 | Eigen::Vector3f pt_ = delta_q.inverse() * pt.getVector3fMap(); 237 | 238 | deskewed->at(i) = cloud->at(i); 239 | deskewed->at(i).getVector3fMap() = pt_; 240 | } 241 | 242 | return deskewed; 243 | } 244 | 245 | private: 246 | ros::NodeHandle nh; 247 | ros::NodeHandle private_nh; 248 | 249 | ros::Subscriber imu_sub; 250 | std::vector imu_queue; 251 | 252 | ros::Subscriber points_sub; 253 | ros::Publisher points_pub; 254 | 255 | ros::Publisher colored_pub; 256 | 257 | tf::TransformListener tf_listener; 258 | 259 | std::string base_link_frame; 260 | 261 | bool use_distance_filter; 262 | double distance_near_thresh; 263 | double distance_far_thresh; 264 | 265 | pcl::Filter::Ptr downsample_filter; 266 | pcl::Filter::Ptr outlier_removal_filter; 267 | }; 268 | 269 | } // namespace hdl_graph_slam 270 | 271 | PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::PrefilteringNodelet, nodelet::Nodelet) 272 | -------------------------------------------------------------------------------- /cmake/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | /opt/ros/$ENV{ROS_DISTRO}/include 13 | NO_DEFAULT_PATH 14 | ) 15 | 16 | # Macro to unify finding both the debug and release versions of the 17 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 18 | # macro. 19 | 20 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 21 | 22 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 23 | NAMES "g2o_${MYLIBRARYNAME}_d" 24 | PATHS 25 | ${G2O_ROOT}/lib/Debug 26 | ${G2O_ROOT}/lib 27 | $ENV{G2O_ROOT}/lib/Debug 28 | $ENV{G2O_ROOT}/lib 29 | /opt/ros/$ENV{ROS_DISTRO}/lib 30 | NO_DEFAULT_PATH 31 | ) 32 | 33 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 34 | NAMES "g2o_${MYLIBRARYNAME}_d" 35 | PATHS 36 | ~/Library/Frameworks 37 | /Library/Frameworks 38 | /usr/local/lib 39 | /usr/local/lib64 40 | /usr/lib 41 | /usr/lib64 42 | /opt/local/lib 43 | /sw/local/lib 44 | /sw/lib 45 | ) 46 | 47 | FIND_LIBRARY(${MYLIBRARY} 48 | NAMES "g2o_${MYLIBRARYNAME}" 49 | PATHS 50 | ${G2O_ROOT}/lib/Release 51 | ${G2O_ROOT}/lib 52 | $ENV{G2O_ROOT}/lib/Release 53 | $ENV{G2O_ROOT}/lib 54 | /opt/ros/$ENV{ROS_DISTRO}/lib 55 | NO_DEFAULT_PATH 56 | ) 57 | 58 | FIND_LIBRARY(${MYLIBRARY} 59 | NAMES "g2o_${MYLIBRARYNAME}" 60 | PATHS 61 | ~/Library/Frameworks 62 | /Library/Frameworks 63 | /usr/local/lib 64 | /usr/local/lib64 65 | /usr/lib 66 | /usr/lib64 67 | /opt/local/lib 68 | /sw/local/lib 69 | /sw/lib 70 | ) 71 | 72 | IF(NOT ${MYLIBRARY}_DEBUG) 73 | IF(MYLIBRARY) 74 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 75 | ENDIF(MYLIBRARY) 76 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 77 | 78 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 79 | 80 | # Find the core elements 81 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 82 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 83 | 84 | # Find the CLI library 85 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 86 | 87 | # Find the pluggable solvers 88 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 93 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 94 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 95 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 96 | 97 | # Find the predefined types 98 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 99 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 102 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 103 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 104 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 105 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons) 106 | 107 | # G2O solvers declared found if we found at least one solver 108 | SET(G2O_SOLVERS_FOUND "NO") 109 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 110 | SET(G2O_SOLVERS_FOUND "YES") 111 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 112 | 113 | # G2O itself declared found if we found the core libraries and at least one solver 114 | SET(G2O_FOUND "NO") 115 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 116 | SET(G2O_FOUND "YES") 117 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 118 | -------------------------------------------------------------------------------- /docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | docker build --tag hdl_graph_slam -f noetic/Dockerfile .. 3 | -------------------------------------------------------------------------------- /docker/howtouse.md: -------------------------------------------------------------------------------- 1 | # hdl_graph_slam 2 | 3 | Original repository: https://github.com/koide3/hdl_graph_slam 4 | 5 | 6 | ## Build 7 | ```bash 8 | cd hdl_graph_slam/docker 9 | ./build.sh 10 | ``` 11 | 12 | ## Run 13 | 14 | ### On host: 15 | ```bash 16 | roscore 17 | ``` 18 | 19 | ```bash 20 | rosparam set use_sim_time true 21 | 22 | cd hdl_graph_slam/rviz 23 | rviz -d hdl_graph_slam.rviz 24 | ``` 25 | 26 | ```bash 27 | rosbag play --clock hdl_400.bag 28 | ``` 29 | http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz 30 | 31 | ### On docker image: 32 | ```bash 33 | cd hdl_graph_slam/docker 34 | ./run.sh 35 | 36 | roslaunch hdl_graph_slam hdl_graph_slam_400.launch 37 | ``` 38 | 39 | 40 | ![hdl_graph_slam](https://user-images.githubusercontent.com/31344317/98347836-4fed5a00-205b-11eb-931c-158f6cd056bf.gif) 41 | -------------------------------------------------------------------------------- /docker/kinetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y && apt-get install --no-install-recommends -y wget nano build-essential ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs ros-kinetic-rviz ros-kinetic-tf-conversions ros-kinetic-libg2o \ 4 | && apt-get clean \ 5 | && rm -rf /var/lib/apt/lists/* 6 | 7 | 8 | RUN mkdir -p /root/catkin_ws/src 9 | WORKDIR /root/catkin_ws/src 10 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace' 11 | RUN git clone https://github.com/koide3/ndt_omp.git 12 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 13 | 14 | COPY . /root/catkin_ws/src/hdl_graph_slam/ 15 | 16 | WORKDIR /root/catkin_ws 17 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_make' 18 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 19 | 20 | RUN apt-get clean && rm -rf /var/lib/apt/lists/* 21 | 22 | WORKDIR / 23 | 24 | ENTRYPOINT ["/ros_entrypoint.sh"] 25 | CMD ["bash"] 26 | -------------------------------------------------------------------------------- /docker/kinetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y \ 5 | wget nano build-essential libomp-dev clang lld-6.0 \ 6 | ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs \ 7 | ros-kinetic-rviz ros-kinetic-tf-conversions ros-kinetic-libg2o \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-6.0 50 12 | 13 | RUN mkdir -p /root/catkin_ws/src 14 | WORKDIR /root/catkin_ws/src 15 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace' 16 | RUN git clone https://github.com/koide3/ndt_omp.git 17 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 18 | 19 | COPY . /root/catkin_ws/src/hdl_graph_slam/ 20 | 21 | WORKDIR /root/catkin_ws 22 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; CC=clang CXX=clang++ catkin_make' 23 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 24 | 25 | RUN apt-get clean && rm -rf /var/lib/apt/lists/* 26 | 27 | WORKDIR / 28 | 29 | ENTRYPOINT ["/ros_entrypoint.sh"] 30 | CMD ["bash"] 31 | -------------------------------------------------------------------------------- /docker/melodic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends \ 5 | wget nano build-essential libomp-dev clang lld\ 6 | ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \ 7 | ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 14 | RUN git clone https://github.com/koide3/ndt_omp.git 15 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 16 | 17 | COPY . /root/catkin_ws/src/hdl_graph_slam/ 18 | 19 | WORKDIR /root/catkin_ws 20 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make' 21 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 22 | 23 | WORKDIR / 24 | 25 | ENTRYPOINT ["/ros_entrypoint.sh"] 26 | CMD ["bash"] 27 | -------------------------------------------------------------------------------- /docker/melodic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends \ 5 | wget nano build-essential libomp-dev clang lld\ 6 | ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \ 7 | ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 14 | RUN git clone -b melodic https://github.com/koide3/ndt_omp.git 15 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 16 | 17 | COPY . /root/catkin_ws/src/hdl_graph_slam/ 18 | 19 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 20 | 21 | WORKDIR /root/catkin_ws 22 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make' 23 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 24 | 25 | WORKDIR / 26 | 27 | ENTRYPOINT ["/ros_entrypoint.sh"] 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /docker/noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends \ 5 | wget nano build-essential libomp-dev clang lld git\ 6 | ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs \ 7 | ros-noetic-rviz ros-noetic-tf-conversions ros-noetic-libg2o \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 14 | RUN git clone https://github.com/koide3/ndt_omp.git 15 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 16 | 17 | COPY . /root/catkin_ws/src/hdl_graph_slam/ 18 | 19 | WORKDIR /root/catkin_ws 20 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make' 21 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 22 | 23 | WORKDIR / 24 | 25 | ENTRYPOINT ["/ros_entrypoint.sh"] 26 | CMD ["bash"] 27 | -------------------------------------------------------------------------------- /docker/noetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends \ 5 | wget nano build-essential libomp-dev clang lld git\ 6 | ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs \ 7 | ros-noetic-rviz ros-noetic-tf-conversions ros-noetic-libg2o \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 14 | RUN git clone https://github.com/koide3/ndt_omp.git 15 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 16 | 17 | COPY . /root/catkin_ws/src/hdl_graph_slam/ 18 | 19 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 20 | 21 | WORKDIR /root/catkin_ws 22 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make' 23 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 24 | 25 | WORKDIR / 26 | 27 | ENTRYPOINT ["/ros_entrypoint.sh"] 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | docker run --net=host -it --rm \ 3 | -v $(realpath ..):/root/catkin_ws/src/hdl_graph_slam \ 4 | -w /root/catkin_ws/src/hdl_graph_slam \ 5 | $@ \ 6 | hdl_graph_slam 7 | -------------------------------------------------------------------------------- /imgs/birds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/birds.png -------------------------------------------------------------------------------- /imgs/ford1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/ford1.png -------------------------------------------------------------------------------- /imgs/ford2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/ford2.png -------------------------------------------------------------------------------- /imgs/ford3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/ford3.png -------------------------------------------------------------------------------- /imgs/hdl_400_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/hdl_400_graph.png -------------------------------------------------------------------------------- /imgs/hdl_400_points.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/hdl_400_points.png -------------------------------------------------------------------------------- /imgs/hdl_graph_slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/hdl_graph_slam.png -------------------------------------------------------------------------------- /imgs/nodelets.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/nodelets.png -------------------------------------------------------------------------------- /imgs/nodelets.vsdx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/nodelets.vsdx -------------------------------------------------------------------------------- /imgs/packages.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/packages.png -------------------------------------------------------------------------------- /imgs/top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/imgs/top.png -------------------------------------------------------------------------------- /include/g2o/edge_plane_identity.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef EDGE_PLANE_IDENTITY_HPP 28 | #define EDGE_PLANE_IDENTITY_HPP 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane normals. 38 | * 39 | */ 40 | class EdgePlaneIdentity : public BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane> { 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | EdgePlaneIdentity() : BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane>() { 44 | _information.setIdentity(); 45 | _error.setZero(); 46 | } 47 | void computeError() { 48 | const VertexPlane* v1 = static_cast(_vertices[0]); 49 | const VertexPlane* v2 = static_cast(_vertices[1]); 50 | 51 | Eigen::Vector4d p1 = v1->estimate().toVector(); 52 | Eigen::Vector4d p2 = v2->estimate().toVector(); 53 | 54 | if(p1.dot(p2) < 0.0) { 55 | p2 = -p2; 56 | } 57 | 58 | _error = (p2 - p1) - _measurement; 59 | } 60 | virtual bool read(std::istream& is) override { 61 | Eigen::Vector4d v; 62 | for(int i = 0; i < 4; ++i) { 63 | is >> v[i]; 64 | } 65 | 66 | setMeasurement(v); 67 | for(int i = 0; i < information().rows(); ++i) { 68 | for(int j = i; j < information().cols(); ++j) { 69 | is >> information()(i, j); 70 | if(i != j) { 71 | information()(j, i) = information()(i, j); 72 | } 73 | } 74 | } 75 | 76 | return true; 77 | } 78 | 79 | virtual bool write(std::ostream& os) const override { 80 | for(int i = 0; i < 4; ++i) { 81 | os << _measurement[i] << " "; 82 | } 83 | 84 | for(int i = 0; i < information().rows(); ++i) { 85 | for(int j = i; j < information().cols(); ++j) { 86 | os << " " << information()(i, j); 87 | }; 88 | } 89 | return os.good(); 90 | } 91 | 92 | virtual void setMeasurement(const Eigen::Vector4d& m) override { 93 | _measurement = m; 94 | } 95 | 96 | virtual int measurementDimension() const override { 97 | return 4; 98 | } 99 | }; 100 | 101 | } // namespace g2o 102 | 103 | #endif // EDGE_PLANE_PARALLEL_HPP 104 | -------------------------------------------------------------------------------- /include/g2o/edge_plane_parallel.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef EDGE_PLANE_PARALLEL_HPP 28 | #define EDGE_PLANE_PARALLEL_HPP 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | EdgePlaneParallel() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() { 40 | _information.setIdentity(); 41 | _error.setZero(); 42 | } 43 | 44 | void computeError() override { 45 | const VertexPlane* v1 = static_cast(_vertices[0]); 46 | const VertexPlane* v2 = static_cast(_vertices[1]); 47 | 48 | Eigen::Vector3d normal1 = v1->estimate().normal(); 49 | Eigen::Vector3d normal2 = v2->estimate().normal(); 50 | 51 | if(normal1.dot(normal2) < 0.0) { 52 | normal2 = -normal2; 53 | } 54 | 55 | _error = (normal2 - normal1) - _measurement; 56 | } 57 | virtual bool read(std::istream& is) override { 58 | Eigen::Vector3d v; 59 | for(int i = 0; i < 3; ++i) { 60 | is >> v[i]; 61 | } 62 | 63 | setMeasurement(v); 64 | for(int i = 0; i < information().rows(); ++i) { 65 | for(int j = i; j < information().cols(); ++j) { 66 | is >> information()(i, j); 67 | if(i != j) { 68 | information()(j, i) = information()(i, j); 69 | } 70 | } 71 | } 72 | 73 | return true; 74 | } 75 | 76 | virtual bool write(std::ostream& os) const override { 77 | for(int i = 0; i < 3; ++i) { 78 | os << _measurement[i] << " "; 79 | } 80 | 81 | for(int i = 0; i < information().rows(); ++i) { 82 | for(int j = i; j < information().cols(); ++j) { 83 | os << " " << information()(i, j); 84 | }; 85 | } 86 | return os.good(); 87 | } 88 | 89 | virtual void setMeasurement(const Eigen::Vector3d& m) override { 90 | _measurement = m; 91 | } 92 | 93 | virtual int measurementDimension() const override { 94 | return 3; 95 | } 96 | }; 97 | 98 | class EdgePlanePerpendicular : public BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane> { 99 | public: 100 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 101 | EdgePlanePerpendicular() : BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane>() { 102 | _information.setIdentity(); 103 | _error.setZero(); 104 | } 105 | 106 | void computeError() override { 107 | const VertexPlane* v1 = static_cast(_vertices[0]); 108 | const VertexPlane* v2 = static_cast(_vertices[1]); 109 | 110 | Eigen::Vector3d normal1 = v1->estimate().normal().normalized(); 111 | Eigen::Vector3d normal2 = v2->estimate().normal().normalized(); 112 | 113 | _error[0] = normal1.dot(normal2); 114 | } 115 | 116 | virtual bool read(std::istream& is) override { 117 | Eigen::Vector3d v; 118 | for(int i = 0; i < 3; ++i) { 119 | is >> v[i]; 120 | } 121 | 122 | setMeasurement(v); 123 | for(int i = 0; i < information().rows(); ++i) { 124 | for(int j = i; j < information().cols(); ++j) { 125 | is >> information()(i, j); 126 | if(i != j) { 127 | information()(j, i) = information()(i, j); 128 | } 129 | } 130 | } 131 | 132 | return true; 133 | } 134 | 135 | virtual bool write(std::ostream& os) const override { 136 | for(int i = 0; i < 3; ++i) { 137 | os << _measurement[i] << " "; 138 | } 139 | 140 | for(int i = 0; i < information().rows(); ++i) { 141 | for(int j = i; j < information().cols(); ++j) { 142 | os << " " << information()(i, j); 143 | }; 144 | } 145 | return os.good(); 146 | } 147 | 148 | virtual void setMeasurement(const Eigen::Vector3d& m) override { 149 | _measurement = m; 150 | } 151 | 152 | virtual int measurementDimension() const override { 153 | return 3; 154 | } 155 | }; 156 | 157 | } // namespace g2o 158 | 159 | #endif // EDGE_PLANE_PARALLEL_HPP 160 | -------------------------------------------------------------------------------- /include/g2o/edge_plane_prior.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef EDGE_PLANE_PRIOR_HPP 28 | #define EDGE_PLANE_PRIOR_HPP 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | class EdgePlanePriorNormal : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane> { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | EdgePlanePriorNormal() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane>() {} 39 | 40 | void computeError() override { 41 | const g2o::VertexPlane* v1 = static_cast(_vertices[0]); 42 | Eigen::Vector3d normal = v1->estimate().normal(); 43 | 44 | if(normal.dot(_measurement) < 0.0) { 45 | normal = -normal; 46 | } 47 | 48 | _error = normal - _measurement; 49 | } 50 | 51 | void setMeasurement(const Eigen::Vector3d& m) override { 52 | _measurement = m; 53 | } 54 | 55 | virtual bool read(std::istream& is) override { 56 | Eigen::Vector3d v; 57 | is >> v(0) >> v(1) >> v(2); 58 | setMeasurement(v); 59 | for(int i = 0; i < information().rows(); ++i) 60 | for(int j = i; j < information().cols(); ++j) { 61 | is >> information()(i, j); 62 | if(i != j) information()(j, i) = information()(i, j); 63 | } 64 | return true; 65 | } 66 | virtual bool write(std::ostream& os) const override { 67 | Eigen::Vector3d v = _measurement; 68 | os << v(0) << " " << v(1) << " " << v(2) << " "; 69 | for(int i = 0; i < information().rows(); ++i) 70 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 71 | return os.good(); 72 | } 73 | }; 74 | 75 | class EdgePlanePriorDistance : public g2o::BaseUnaryEdge<1, double, g2o::VertexPlane> { 76 | public: 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | EdgePlanePriorDistance() : g2o::BaseUnaryEdge<1, double, g2o::VertexPlane>() {} 79 | 80 | void computeError() override { 81 | const g2o::VertexPlane* v1 = static_cast(_vertices[0]); 82 | _error[0] = _measurement - v1->estimate().distance(); 83 | } 84 | 85 | void setMeasurement(const double& m) override { 86 | _measurement = m; 87 | } 88 | 89 | virtual bool read(std::istream& is) override { 90 | is >> _measurement; 91 | for(int i = 0; i < information().rows(); ++i) 92 | for(int j = i; j < information().cols(); ++j) { 93 | is >> information()(i, j); 94 | if(i != j) information()(j, i) = information()(i, j); 95 | } 96 | return true; 97 | } 98 | virtual bool write(std::ostream& os) const override { 99 | os << _measurement; 100 | for(int i = 0; i < information().rows(); ++i) 101 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 102 | return os.good(); 103 | } 104 | }; 105 | } // namespace g2o 106 | 107 | #endif // EDGE_SE3_PRIORXY_HPP 108 | -------------------------------------------------------------------------------- /include/g2o/edge_se3_plane.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef KKL_G2O_EDGE_SE3_PLANE_HPP 28 | #define KKL_G2O_EDGE_SE3_PLANE_HPP 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | class EdgeSE3Plane : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane> { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | EdgeSE3Plane() : BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() {} 39 | 40 | void computeError() override { 41 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 42 | const g2o::VertexPlane* v2 = static_cast(_vertices[1]); 43 | 44 | Eigen::Isometry3d w2n = v1->estimate().inverse(); 45 | Plane3D local_plane = w2n * v2->estimate(); 46 | _error = local_plane.ominus(_measurement); 47 | } 48 | 49 | void setMeasurement(const g2o::Plane3D& m) override { 50 | _measurement = m; 51 | } 52 | 53 | virtual bool read(std::istream& is) override { 54 | Eigen::Vector4d v; 55 | is >> v(0) >> v(1) >> v(2) >> v(3); 56 | setMeasurement(Plane3D(v)); 57 | for(int i = 0; i < information().rows(); ++i) 58 | for(int j = i; j < information().cols(); ++j) { 59 | is >> information()(i, j); 60 | if(i != j) information()(j, i) = information()(i, j); 61 | } 62 | return true; 63 | } 64 | virtual bool write(std::ostream& os) const override { 65 | Eigen::Vector4d v = _measurement.toVector(); 66 | os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " "; 67 | for(int i = 0; i < information().rows(); ++i) 68 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 69 | return os.good(); 70 | } 71 | }; 72 | } // namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /include/g2o/edge_se3_priorquat.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef KKL_G2O_EDGE_SE3_PRIORQUAT_HPP 28 | #define KKL_G2O_EDGE_SE3_PRIORQUAT_HPP 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | EdgeSE3PriorQuat() : g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() {} 38 | 39 | void computeError() override { 40 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 41 | 42 | Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear()); 43 | 44 | if(_measurement.coeffs().dot(estimate.coeffs()) < 0.0) { 45 | estimate.coeffs() = -estimate.coeffs(); 46 | } 47 | _error = estimate.vec() - _measurement.vec(); 48 | } 49 | 50 | void setMeasurement(const Eigen::Quaterniond& m) override { 51 | _measurement = m; 52 | if(m.w() < 0.0) { 53 | _measurement.coeffs() = -m.coeffs(); 54 | } 55 | } 56 | 57 | virtual bool read(std::istream& is) override { 58 | Eigen::Quaterniond q; 59 | is >> q.w() >> q.x() >> q.y() >> q.z(); 60 | setMeasurement(q); 61 | for(int i = 0; i < information().rows(); ++i) 62 | for(int j = i; j < information().cols(); ++j) { 63 | is >> information()(i, j); 64 | if(i != j) information()(j, i) = information()(i, j); 65 | } 66 | return true; 67 | } 68 | virtual bool write(std::ostream& os) const override { 69 | Eigen::Quaterniond q = _measurement; 70 | os << q.w() << " " << q.x() << " " << q.y() << " " << q.z(); 71 | for(int i = 0; i < information().rows(); ++i) 72 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 73 | return os.good(); 74 | } 75 | }; 76 | } // namespace g2o 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /include/g2o/edge_se3_priorvec.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef KKL_G2O_EDGE_SE3_PRIORVEC_HPP 28 | #define KKL_G2O_EDGE_SE3_PRIORVEC_HPP 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | class EdgeSE3PriorVec : public g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexSE3> { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | EdgeSE3PriorVec() : g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexSE3>() {} 38 | 39 | void computeError() override { 40 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 41 | 42 | Eigen::Vector3d direction = _measurement.head<3>(); 43 | Eigen::Vector3d measurement = _measurement.tail<3>(); 44 | 45 | Eigen::Vector3d estimate = (v1->estimate().linear().inverse() * direction); 46 | 47 | _error = estimate - measurement; 48 | } 49 | 50 | void setMeasurement(const Eigen::Matrix& m) override { 51 | _measurement.head<3>() = m.head<3>().normalized(); 52 | _measurement.tail<3>() = m.tail<3>().normalized(); 53 | } 54 | 55 | virtual bool read(std::istream& is) override { 56 | Eigen::Matrix v; 57 | is >> v[0] >> v[1] >> v[2] >> v[3] >> v[4] >> v[5]; 58 | setMeasurement(v); 59 | for(int i = 0; i < information().rows(); ++i) 60 | for(int j = i; j < information().cols(); ++j) { 61 | is >> information()(i, j); 62 | if(i != j) information()(j, i) = information()(i, j); 63 | } 64 | return true; 65 | } 66 | virtual bool write(std::ostream& os) const override { 67 | Eigen::Matrix v = _measurement; 68 | os << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5]; 69 | for(int i = 0; i < information().rows(); ++i) 70 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 71 | return os.good(); 72 | } 73 | }; 74 | } // namespace g2o 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /include/g2o/edge_se3_priorxy.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef EDGE_SE3_PRIORXY_HPP 28 | #define EDGE_SE3_PRIORXY_HPP 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | class EdgeSE3PriorXY : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3> { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | EdgeSE3PriorXY() : g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3>() {} 38 | 39 | void computeError() override { 40 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 41 | 42 | Eigen::Vector2d estimate = v1->estimate().translation().head<2>(); 43 | _error = estimate - _measurement; 44 | } 45 | 46 | void setMeasurement(const Eigen::Vector2d& m) override { 47 | _measurement = m; 48 | } 49 | 50 | virtual bool read(std::istream& is) override { 51 | Eigen::Vector2d v; 52 | is >> v(0) >> v(1); 53 | setMeasurement(v); 54 | for(int i = 0; i < information().rows(); ++i) 55 | for(int j = i; j < information().cols(); ++j) { 56 | is >> information()(i, j); 57 | if(i != j) information()(j, i) = information()(i, j); 58 | } 59 | return true; 60 | } 61 | virtual bool write(std::ostream& os) const override { 62 | Eigen::Vector2d v = _measurement; 63 | os << v(0) << " " << v(1) << " "; 64 | for(int i = 0; i < information().rows(); ++i) 65 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 66 | return os.good(); 67 | } 68 | }; 69 | } // namespace g2o 70 | 71 | #endif // EDGE_SE3_PRIORXY_HPP 72 | -------------------------------------------------------------------------------- /include/g2o/edge_se3_priorxyz.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef KKL_G2O_EDGE_SE3_PRIORXYZ_HPP 28 | #define KKL_G2O_EDGE_SE3_PRIORXYZ_HPP 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | EdgeSE3PriorXYZ() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() {} 38 | 39 | void computeError() override { 40 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 41 | 42 | Eigen::Vector3d estimate = v1->estimate().translation(); 43 | _error = estimate - _measurement; 44 | } 45 | 46 | void setMeasurement(const Eigen::Vector3d& m) override { 47 | _measurement = m; 48 | } 49 | 50 | virtual bool read(std::istream& is) override { 51 | Eigen::Vector3d v; 52 | is >> v(0) >> v(1) >> v(2); 53 | setMeasurement(v); 54 | for(int i = 0; i < information().rows(); ++i) 55 | for(int j = i; j < information().cols(); ++j) { 56 | is >> information()(i, j); 57 | if(i != j) information()(j, i) = information()(i, j); 58 | } 59 | return true; 60 | } 61 | virtual bool write(std::ostream& os) const override { 62 | Eigen::Vector3d v = _measurement; 63 | os << v(0) << " " << v(1) << " " << v(2) << " "; 64 | for(int i = 0; i < information().rows(); ++i) 65 | for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 66 | return os.good(); 67 | } 68 | }; 69 | } // namespace g2o 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /include/g2o/robust_kernel_io.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef ROBUST_KERNEL_IO_HPP 4 | #define ROBUST_KERNEL_IO_HPP 5 | 6 | #include 7 | #include 8 | 9 | namespace g2o { 10 | 11 | std::string kernel_type(g2o::RobustKernel* kernel); 12 | 13 | bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph); 14 | 15 | bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph); 16 | 17 | } // namespace g2o 18 | 19 | #endif // ROBUST_KERNEL_IO_HPP 20 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/graph_slam.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef GRAPH_SLAM_HPP 4 | #define GRAPH_SLAM_HPP 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace g2o { 13 | class VertexSE3; 14 | class VertexPlane; 15 | class VertexPointXYZ; 16 | class EdgeSE3; 17 | class EdgeSE3Plane; 18 | class EdgeSE3PointXYZ; 19 | class EdgeSE3PriorXY; 20 | class EdgeSE3PriorXYZ; 21 | class EdgeSE3PriorVec; 22 | class EdgeSE3PriorQuat; 23 | class EdgePlane; 24 | class EdgePlaneIdentity; 25 | class EdgePlaneParallel; 26 | class EdgePlanePerpendicular; 27 | class EdgePlanePriorNormal; 28 | class EdgePlanePriorDistance; 29 | class RobustKernelFactory; 30 | } // namespace g2o 31 | 32 | namespace hdl_graph_slam { 33 | 34 | class GraphSLAM { 35 | public: 36 | GraphSLAM(const std::string& solver_type = "lm_var"); 37 | virtual ~GraphSLAM(); 38 | 39 | int num_vertices() const; 40 | int num_edges() const; 41 | 42 | void set_solver(const std::string& solver_type); 43 | 44 | /** 45 | * @brief add a SE3 node to the graph 46 | * @param pose 47 | * @return registered node 48 | */ 49 | g2o::VertexSE3* add_se3_node(const Eigen::Isometry3d& pose); 50 | 51 | /** 52 | * @brief add a plane node to the graph 53 | * @param plane_coeffs 54 | * @return registered node 55 | */ 56 | g2o::VertexPlane* add_plane_node(const Eigen::Vector4d& plane_coeffs); 57 | 58 | /** 59 | * @brief add a point_xyz node to the graph 60 | * @param xyz 61 | * @return registered node 62 | */ 63 | g2o::VertexPointXYZ* add_point_xyz_node(const Eigen::Vector3d& xyz); 64 | 65 | /** 66 | * @brief add an edge between SE3 nodes 67 | * @param v1 node1 68 | * @param v2 node2 69 | * @param relative_pose relative pose between node1 and node2 70 | * @param information_matrix information matrix (it must be 6x6) 71 | * @return registered edge 72 | */ 73 | g2o::EdgeSE3* add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix); 74 | 75 | /** 76 | * @brief add an edge between an SE3 node and a plane node 77 | * @param v_se3 SE3 node 78 | * @param v_plane plane node 79 | * @param plane_coeffs plane coefficients w.r.t. v_se3 80 | * @param information_matrix information matrix (it must be 3x3) 81 | * @return registered edge 82 | */ 83 | g2o::EdgeSE3Plane* add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix); 84 | 85 | /** 86 | * @brief add an edge between an SE3 node and a point_xyz node 87 | * @param v_se3 SE3 node 88 | * @param v_xyz point_xyz node 89 | * @param xyz xyz coordinate 90 | * @param information information_matrix (it must be 3x3) 91 | * @return registered edge 92 | */ 93 | g2o::EdgeSE3PointXYZ* add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); 94 | 95 | /** 96 | * @brief add a prior edge to an SE3 node 97 | * @param v_se3 98 | * @param xy 99 | * @param information_matrix 100 | * @return 101 | */ 102 | g2o::EdgePlanePriorNormal* add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix); 103 | 104 | g2o::EdgePlanePriorDistance* add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix); 105 | 106 | g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix); 107 | 108 | g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); 109 | 110 | g2o::EdgeSE3PriorQuat* add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix); 111 | 112 | g2o::EdgeSE3PriorVec* add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix); 113 | 114 | g2o::EdgePlane* add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information); 115 | 116 | g2o::EdgePlaneIdentity* add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information); 117 | 118 | g2o::EdgePlaneParallel* add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information); 119 | 120 | g2o::EdgePlanePerpendicular* add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information); 121 | 122 | void add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size); 123 | 124 | /** 125 | * @brief perform graph optimization 126 | */ 127 | int optimize(int num_iterations); 128 | 129 | /** 130 | * @brief save the pose graph to a file 131 | * @param filename output filename 132 | */ 133 | void save(const std::string& filename); 134 | 135 | /** 136 | * @brief load the pose graph from file 137 | * @param filename output filename 138 | */ 139 | bool load(const std::string& filename); 140 | 141 | public: 142 | g2o::RobustKernelFactory* robust_kernel_factory; 143 | std::unique_ptr graph; // g2o graph 144 | }; 145 | 146 | } // namespace hdl_graph_slam 147 | 148 | #endif // GRAPH_SLAM_HPP 149 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/information_matrix_calculator.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef INFORMATION_MATRIX_CALCULATOR_HPP 4 | #define INFORMATION_MATRIX_CALCULATOR_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace hdl_graph_slam { 11 | 12 | class InformationMatrixCalculator { 13 | public: 14 | using PointT = pcl::PointXYZI; 15 | 16 | InformationMatrixCalculator() {} 17 | InformationMatrixCalculator(ros::NodeHandle& nh); 18 | ~InformationMatrixCalculator(); 19 | 20 | template 21 | void load(ParamServer& params) { 22 | use_const_inf_matrix = params.template param("use_const_inf_matrix", false); 23 | const_stddev_x = params.template param("const_stddev_x", 0.5); 24 | const_stddev_q = params.template param("const_stddev_q", 0.1); 25 | 26 | var_gain_a = params.template param("var_gain_a", 20.0); 27 | min_stddev_x = params.template param("min_stddev_x", 0.1); 28 | max_stddev_x = params.template param("max_stddev_x", 5.0); 29 | min_stddev_q = params.template param("min_stddev_q", 0.05); 30 | max_stddev_q = params.template param("max_stddev_q", 0.2); 31 | fitness_score_thresh = params.template param("fitness_score_thresh", 2.5); 32 | } 33 | 34 | static double calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range = std::numeric_limits::max()); 35 | 36 | Eigen::MatrixXd calc_information_matrix(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const; 37 | 38 | private: 39 | double weight(double a, double max_x, double min_y, double max_y, double x) const { 40 | double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x)); 41 | return min_y + (max_y - min_y) * y; 42 | } 43 | 44 | private: 45 | bool use_const_inf_matrix; 46 | double const_stddev_x; 47 | double const_stddev_q; 48 | 49 | double var_gain_a; 50 | double min_stddev_x; 51 | double max_stddev_x; 52 | double min_stddev_q; 53 | double max_stddev_q; 54 | double fitness_score_thresh; 55 | }; 56 | 57 | } // namespace hdl_graph_slam 58 | 59 | #endif // INFORMATION_MATRIX_CALCULATOR_HPP 60 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/keyframe.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef KEYFRAME_HPP 4 | #define KEYFRAME_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace g2o { 12 | class VertexSE3; 13 | class HyperGraph; 14 | class SparseOptimizer; 15 | } // namespace g2o 16 | 17 | namespace hdl_graph_slam { 18 | 19 | /** 20 | * @brief KeyFrame (pose node) 21 | */ 22 | struct KeyFrame { 23 | public: 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | using PointT = pcl::PointXYZI; 26 | using Ptr = std::shared_ptr; 27 | 28 | KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud::ConstPtr& cloud); 29 | KeyFrame(const std::string& directory, g2o::HyperGraph* graph); 30 | virtual ~KeyFrame(); 31 | 32 | void save(const std::string& directory); 33 | bool load(const std::string& directory, g2o::HyperGraph* graph); 34 | 35 | long id() const; 36 | Eigen::Isometry3d estimate() const; 37 | 38 | public: 39 | ros::Time stamp; // timestamp 40 | Eigen::Isometry3d odom; // odometry (estimated by scan_matching_odometry) 41 | double accum_distance; // accumulated distance from the first node (by scan_matching_odometry) 42 | pcl::PointCloud::ConstPtr cloud; // point cloud 43 | boost::optional floor_coeffs; // detected floor's coefficients 44 | boost::optional utm_coord; // UTM coord obtained by GPS 45 | 46 | boost::optional acceleration; // 47 | boost::optional orientation; // 48 | 49 | g2o::VertexSE3* node; // node instance 50 | }; 51 | 52 | /** 53 | * @brief KeyFramesnapshot for map cloud generation 54 | */ 55 | struct KeyFrameSnapshot { 56 | public: 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | 59 | using PointT = KeyFrame::PointT; 60 | using Ptr = std::shared_ptr; 61 | 62 | KeyFrameSnapshot(const KeyFrame::Ptr& key); 63 | KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud::ConstPtr& cloud); 64 | 65 | ~KeyFrameSnapshot(); 66 | 67 | public: 68 | Eigen::Isometry3d pose; // pose estimated by graph optimization 69 | pcl::PointCloud::ConstPtr cloud; // point cloud 70 | }; 71 | 72 | } // namespace hdl_graph_slam 73 | 74 | #endif // KEYFRAME_HPP 75 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/keyframe_updater.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef KEYFRAME_UPDATER_HPP 4 | #define KEYFRAME_UPDATER_HPP 5 | 6 | #include 7 | #include 8 | 9 | namespace hdl_graph_slam { 10 | 11 | /** 12 | * @brief this class decides if a new frame should be registered to the pose graph as a keyframe 13 | */ 14 | class KeyframeUpdater { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | /** 19 | * @brief constructor 20 | * @param pnh 21 | */ 22 | KeyframeUpdater(ros::NodeHandle& pnh) : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()) { 23 | keyframe_delta_trans = pnh.param("keyframe_delta_trans", 2.0); 24 | keyframe_delta_angle = pnh.param("keyframe_delta_angle", 2.0); 25 | 26 | accum_distance = 0.0; 27 | } 28 | 29 | /** 30 | * @brief decide if a new frame should be registered to the graph 31 | * @param pose pose of the frame 32 | * @return if true, the frame should be registered 33 | */ 34 | bool update(const Eigen::Isometry3d& pose) { 35 | // first frame is always registered to the graph 36 | if(is_first) { 37 | is_first = false; 38 | prev_keypose = pose; 39 | return true; 40 | } 41 | 42 | // calculate the delta transformation from the previous keyframe 43 | Eigen::Isometry3d delta = prev_keypose.inverse() * pose; 44 | double dx = delta.translation().norm(); 45 | double da = Eigen::AngleAxisd(delta.linear()).angle(); 46 | 47 | // too close to the previous frame 48 | if(dx < keyframe_delta_trans && da < keyframe_delta_angle) { 49 | return false; 50 | } 51 | 52 | accum_distance += dx; 53 | prev_keypose = pose; 54 | return true; 55 | } 56 | 57 | /** 58 | * @brief the last keyframe's accumulated distance from the first keyframe 59 | * @return accumulated distance 60 | */ 61 | double get_accum_distance() const { 62 | return accum_distance; 63 | } 64 | 65 | private: 66 | // parameters 67 | double keyframe_delta_trans; // 68 | double keyframe_delta_angle; // 69 | 70 | bool is_first; 71 | double accum_distance; 72 | Eigen::Isometry3d prev_keypose; 73 | }; 74 | 75 | } // namespace hdl_graph_slam 76 | 77 | #endif // KEYFRAME_UPDATOR_HPP 78 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/loop_detector.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef LOOP_DETECTOR_HPP 4 | #define LOOP_DETECTOR_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace hdl_graph_slam { 14 | 15 | struct Loop { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | using Ptr = std::shared_ptr; 19 | 20 | Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose) : key1(key1), key2(key2), relative_pose(relpose) {} 21 | 22 | public: 23 | KeyFrame::Ptr key1; 24 | KeyFrame::Ptr key2; 25 | Eigen::Matrix4f relative_pose; 26 | }; 27 | 28 | /** 29 | * @brief this class finds loops by scam matching and adds them to the pose graph 30 | */ 31 | class LoopDetector { 32 | public: 33 | typedef pcl::PointXYZI PointT; 34 | 35 | /** 36 | * @brief constructor 37 | * @param pnh 38 | */ 39 | LoopDetector(ros::NodeHandle& pnh) { 40 | distance_thresh = pnh.param("distance_thresh", 5.0); 41 | accum_distance_thresh = pnh.param("accum_distance_thresh", 8.0); 42 | distance_from_last_edge_thresh = pnh.param("min_edge_interval", 5.0); 43 | 44 | fitness_score_max_range = pnh.param("fitness_score_max_range", std::numeric_limits::max()); 45 | fitness_score_thresh = pnh.param("fitness_score_thresh", 0.5); 46 | 47 | registration = select_registration_method(pnh); 48 | last_edge_accum_distance = 0.0; 49 | } 50 | 51 | /** 52 | * @brief detect loops and add them to the pose graph 53 | * @param keyframes keyframes 54 | * @param new_keyframes newly registered keyframes 55 | * @param graph_slam pose graph 56 | */ 57 | std::vector detect(const std::vector& keyframes, const std::deque& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam) { 58 | std::vector detected_loops; 59 | for(const auto& new_keyframe : new_keyframes) { 60 | auto candidates = find_candidates(keyframes, new_keyframe); 61 | auto loop = matching(candidates, new_keyframe, graph_slam); 62 | if(loop) { 63 | detected_loops.push_back(loop); 64 | } 65 | } 66 | 67 | return detected_loops; 68 | } 69 | 70 | double get_distance_thresh() const { 71 | return distance_thresh; 72 | } 73 | 74 | private: 75 | /** 76 | * @brief find loop candidates. A detected loop begins at one of #keyframes and ends at #new_keyframe 77 | * @param keyframes candidate keyframes of loop start 78 | * @param new_keyframe loop end keyframe 79 | * @return loop candidates 80 | */ 81 | std::vector find_candidates(const std::vector& keyframes, const KeyFrame::Ptr& new_keyframe) const { 82 | // too close to the last registered loop edge 83 | if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) { 84 | return std::vector(); 85 | } 86 | 87 | std::vector candidates; 88 | candidates.reserve(32); 89 | 90 | for(const auto& k : keyframes) { 91 | // traveled distance between keyframes is too small 92 | if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) { 93 | continue; 94 | } 95 | 96 | const auto& pos1 = k->node->estimate().translation(); 97 | const auto& pos2 = new_keyframe->node->estimate().translation(); 98 | 99 | // estimated distance between keyframes is too small 100 | double dist = (pos1.head<2>() - pos2.head<2>()).norm(); 101 | if(dist > distance_thresh) { 102 | continue; 103 | } 104 | 105 | candidates.push_back(k); 106 | } 107 | 108 | return candidates; 109 | } 110 | 111 | /** 112 | * @brief To validate a loop candidate this function applies a scan matching between keyframes consisting the loop. If they are matched well, the loop is added to the pose graph 113 | * @param candidate_keyframes candidate keyframes of loop start 114 | * @param new_keyframe loop end keyframe 115 | * @param graph_slam graph slam 116 | */ 117 | Loop::Ptr matching(const std::vector& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) { 118 | if(candidate_keyframes.empty()) { 119 | return nullptr; 120 | } 121 | 122 | registration->setInputTarget(new_keyframe->cloud); 123 | 124 | double best_score = std::numeric_limits::max(); 125 | KeyFrame::Ptr best_matched; 126 | Eigen::Matrix4f relative_pose; 127 | 128 | std::cout << std::endl; 129 | std::cout << "--- loop detection ---" << std::endl; 130 | std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl; 131 | std::cout << "matching" << std::flush; 132 | auto t1 = ros::Time::now(); 133 | 134 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 135 | for(const auto& candidate : candidate_keyframes) { 136 | registration->setInputSource(candidate->cloud); 137 | Eigen::Isometry3d new_keyframe_estimate = new_keyframe->node->estimate(); 138 | new_keyframe_estimate.linear() = Eigen::Quaterniond(new_keyframe_estimate.linear()).normalized().toRotationMatrix(); 139 | Eigen::Isometry3d candidate_estimate = candidate->node->estimate(); 140 | candidate_estimate.linear() = Eigen::Quaterniond(candidate_estimate.linear()).normalized().toRotationMatrix(); 141 | Eigen::Matrix4f guess = (new_keyframe_estimate.inverse() * candidate_estimate).matrix().cast(); 142 | guess(2, 3) = 0.0; 143 | registration->align(*aligned, guess); 144 | std::cout << "." << std::flush; 145 | 146 | double score = registration->getFitnessScore(fitness_score_max_range); 147 | if(!registration->hasConverged() || score > best_score) { 148 | continue; 149 | } 150 | 151 | best_score = score; 152 | best_matched = candidate; 153 | relative_pose = registration->getFinalTransformation(); 154 | } 155 | 156 | auto t2 = ros::Time::now(); 157 | std::cout << " done" << std::endl; 158 | std::cout << "best_score: " << boost::format("%.3f") % best_score << " time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl; 159 | 160 | if(best_score > fitness_score_thresh) { 161 | std::cout << "loop not found..." << std::endl; 162 | return nullptr; 163 | } 164 | 165 | std::cout << "loop found!!" << std::endl; 166 | std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl; 167 | 168 | last_edge_accum_distance = new_keyframe->accum_distance; 169 | 170 | return std::make_shared(new_keyframe, best_matched, relative_pose); 171 | } 172 | 173 | private: 174 | double distance_thresh; // estimated distance between keyframes consisting a loop must be less than this distance 175 | double accum_distance_thresh; // traveled distance between ... 176 | double distance_from_last_edge_thresh; // a new loop edge must far from the last one at least this distance 177 | 178 | double fitness_score_max_range; // maximum allowable distance between corresponding points 179 | double fitness_score_thresh; // threshold for scan matching 180 | 181 | double last_edge_accum_distance; 182 | 183 | pcl::Registration::Ptr registration; 184 | }; 185 | 186 | } // namespace hdl_graph_slam 187 | 188 | #endif // LOOP_DETECTOR_HPP 189 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/map_cloud_generator.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef MAP_CLOUD_GENERATOR_HPP 4 | #define MAP_CLOUD_GENERATOR_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hdl_graph_slam { 12 | 13 | /** 14 | * @brief this class generates a map point cloud from registered keyframes 15 | */ 16 | class MapCloudGenerator { 17 | public: 18 | using PointT = pcl::PointXYZI; 19 | 20 | MapCloudGenerator(); 21 | ~MapCloudGenerator(); 22 | 23 | /** 24 | * @brief generates a map point cloud 25 | * @param keyframes snapshots of keyframes 26 | * @param resolution resolution of generated map 27 | * @return generated map point cloud 28 | */ 29 | pcl::PointCloud::Ptr generate(const std::vector& keyframes, double resolution) const; 30 | }; 31 | 32 | } // namespace hdl_graph_slam 33 | 34 | #endif // MAP_POINTCLOUD_GENERATOR_HPP 35 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/nmea_sentence_parser.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef NMEA_SENTENCE_PARSER_HPP 4 | #define NMEA_SENTENCE_PARSER_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace hdl_graph_slam { 13 | 14 | struct GPRMC { 15 | public: 16 | GPRMC() { 17 | status = 'V'; 18 | } 19 | 20 | GPRMC(const std::vector& tokens) { 21 | if(tokens[0] != "$GPRMC" || tokens.size() < 12) { 22 | status = 'V'; 23 | return; 24 | } 25 | 26 | long time = std::stol(tokens[1]); 27 | hour = time / 10000; 28 | minute = (time % 10000) / 100; 29 | second = time % 100; 30 | 31 | status = tokens[2][0]; 32 | 33 | latitude = degmin2deg(std::stod(tokens[3])); 34 | latitude = tokens[4] == "N" ? latitude : -latitude; 35 | 36 | longitude = degmin2deg(std::stod(tokens[5])); 37 | longitude = tokens[6] == "E" ? longitude : -longitude; 38 | 39 | speed_knots = std::stod(tokens[7]); 40 | track_angle_degree = std::stod(tokens[8]); 41 | 42 | long date = std::stol(tokens[9]); 43 | year = date % 100; 44 | month = (date / 100) % 100; 45 | day = (date / 10000) % 100; 46 | 47 | magnetic_variation = std::stod(tokens[10]); 48 | magnetic_variation = tokens[11][0] == 'E' ? magnetic_variation : -magnetic_variation; 49 | } 50 | 51 | double degmin2deg(double degmin) { 52 | double d = std::floor(degmin / 100.0); 53 | double m = (degmin - d * 100.0) / 60.0; 54 | return d + m; 55 | } 56 | 57 | public: 58 | char status; // Status A=active or V=Void. 59 | 60 | int hour; // Fix taken at 12:35:19 UTC 61 | int minute; 62 | int second; 63 | 64 | double latitude; // 65 | double longitude; 66 | 67 | double speed_knots; // Speed over the ground in knots 68 | double track_angle_degree; // Track angle in degrees True 69 | 70 | int year; 71 | int month; 72 | int day; 73 | 74 | double magnetic_variation; 75 | }; 76 | 77 | class NmeaSentenceParser { 78 | public: 79 | NmeaSentenceParser() {} 80 | ~NmeaSentenceParser() {} 81 | 82 | GPRMC parse(const std::string& sentence) const { 83 | int checksum_loc = sentence.find('*'); 84 | if(checksum_loc == std::string::npos) { 85 | return GPRMC(); 86 | } 87 | 88 | int checksum = std::stoul(sentence.substr(checksum_loc + 1), nullptr, 16); 89 | 90 | std::string substr = sentence.substr(1, checksum_loc - 1); 91 | int sum = std::accumulate(substr.begin(), substr.end(), static_cast(0), [=](unsigned char n, unsigned char c) { return n ^ c; }); 92 | 93 | if(checksum != (sum & 0xf)) { 94 | std::cerr << "checksum doesn't match!!" << std::endl; 95 | std::cerr << sentence << " " << sum << std::endl; 96 | return GPRMC(); 97 | } 98 | 99 | std::vector tokens; 100 | boost::split(tokens, sentence, boost::is_any_of(",")); 101 | 102 | return GPRMC(tokens); 103 | } 104 | }; 105 | 106 | } // namespace hdl_graph_slam 107 | 108 | #endif // NMEA_SENTENCE_PARSER_HPP 109 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/registrations.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef HDL_GRAPH_SLAM_REGISTRATIONS_HPP 4 | #define HDL_GRAPH_SLAM_REGISTRATIONS_HPP 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace hdl_graph_slam { 11 | 12 | /** 13 | * @brief select a scan matching algorithm according to rosparams 14 | * @param pnh 15 | * @return selected scan matching 16 | */ 17 | pcl::Registration::Ptr select_registration_method(ros::NodeHandle& pnh); 18 | 19 | } // namespace hdl_graph_slam 20 | 21 | #endif // 22 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/ros_time_hash.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef ROS_TIME_HASH_HPP 4 | #define ROS_TIME_HASH_HPP 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | /** 12 | * @brief Hash calculation for ros::Time 13 | */ 14 | class RosTimeHash { 15 | public: 16 | size_t operator()(const ros::Time& val) const { 17 | size_t seed = 0; 18 | boost::hash_combine(seed, val.sec); 19 | boost::hash_combine(seed, val.nsec); 20 | return seed; 21 | } 22 | }; 23 | 24 | #endif // ROS_TIME_HASHER_HPP 25 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/ros_utils.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #ifndef ROS_UTILS_HPP 4 | #define ROS_UTILS_HPP 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace hdl_graph_slam { 14 | 15 | /** 16 | * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped 17 | * @param stamp timestamp 18 | * @param pose Eigen::Matrix to be converted 19 | * @param frame_id tf frame_id 20 | * @param child_frame_id tf child frame_id 21 | * @return converted TransformStamped 22 | */ 23 | static geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) { 24 | Eigen::Quaternionf quat(pose.block<3, 3>(0, 0)); 25 | quat.normalize(); 26 | geometry_msgs::Quaternion odom_quat; 27 | odom_quat.w = quat.w(); 28 | odom_quat.x = quat.x(); 29 | odom_quat.y = quat.y(); 30 | odom_quat.z = quat.z(); 31 | 32 | geometry_msgs::TransformStamped odom_trans; 33 | odom_trans.header.stamp = stamp; 34 | odom_trans.header.frame_id = frame_id; 35 | odom_trans.child_frame_id = child_frame_id; 36 | 37 | odom_trans.transform.translation.x = pose(0, 3); 38 | odom_trans.transform.translation.y = pose(1, 3); 39 | odom_trans.transform.translation.z = pose(2, 3); 40 | odom_trans.transform.rotation = odom_quat; 41 | 42 | return odom_trans; 43 | } 44 | 45 | static Eigen::Isometry3d pose2isometry(const geometry_msgs::Pose& pose) { 46 | Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); 47 | mat.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); 48 | mat.linear() = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z).toRotationMatrix(); 49 | return mat; 50 | } 51 | 52 | static Eigen::Isometry3d tf2isometry(const tf::StampedTransform& trans) { 53 | Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); 54 | mat.translation() = Eigen::Vector3d(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z()); 55 | mat.linear() = Eigen::Quaterniond(trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()).toRotationMatrix(); 56 | return mat; 57 | } 58 | 59 | static geometry_msgs::Pose isometry2pose(const Eigen::Isometry3d& mat) { 60 | Eigen::Quaterniond quat(mat.linear()); 61 | Eigen::Vector3d trans = mat.translation(); 62 | 63 | geometry_msgs::Pose pose; 64 | pose.position.x = trans.x(); 65 | pose.position.y = trans.y(); 66 | pose.position.z = trans.z(); 67 | pose.orientation.w = quat.w(); 68 | pose.orientation.x = quat.x(); 69 | pose.orientation.y = quat.y(); 70 | pose.orientation.z = quat.z(); 71 | 72 | return pose; 73 | } 74 | 75 | static Eigen::Isometry3d odom2isometry(const nav_msgs::OdometryConstPtr& odom_msg) { 76 | const auto& orientation = odom_msg->pose.pose.orientation; 77 | const auto& position = odom_msg->pose.pose.position; 78 | 79 | Eigen::Quaterniond quat; 80 | quat.w() = orientation.w; 81 | quat.x() = orientation.x; 82 | quat.y() = orientation.y; 83 | quat.z() = orientation.z; 84 | 85 | Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity(); 86 | isometry.linear() = quat.toRotationMatrix(); 87 | isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z); 88 | return isometry; 89 | } 90 | 91 | } // namespace hdl_graph_slam 92 | 93 | #endif // ROS_UTILS_HPP 94 | -------------------------------------------------------------------------------- /launch/hdl_graph_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | -------------------------------------------------------------------------------- /launch/hdl_graph_slam_400.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /launch/hdl_graph_slam_501.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /launch/hdl_graph_slam_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /launch/hdl_graph_slam_kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | -------------------------------------------------------------------------------- /launch/msf_config.yaml: -------------------------------------------------------------------------------- 1 | /msf_lidar_scan_matching/core/data_playback: true 2 | /msf_lidar_scan_matching/core/fixed_bias: 0 3 | 4 | #########IMU PARAMETERS####### 5 | ######## Asctec Firefly IMU 6 | /msf_lidar_scan_matching/core/core_noise_acc: 0.083 7 | /msf_lidar_scan_matching/core/core_noise_accbias: 0.0083 8 | /msf_lidar_scan_matching/core/core_noise_gyr: 0.0013 9 | /msf_lidar_scan_matching/core/core_noise_gyrbias: 0.00013 10 | 11 | /msf_lidar_scan_matching/pose_sensor/fixed_scale: 1 12 | /msf_lidar_scan_matching/pose_sensor/pose_noise_scale: 0.0 13 | /msf_lidar_scan_matching/pose_sensor/pose_noise_p_wv: 0.0 14 | /msf_lidar_scan_matching/pose_sensor/pose_noise_q_wv: 0.0 15 | /msf_lidar_scan_matching/pose_sensor/pose_noise_q_ic: 0.0 16 | /msf_lidar_scan_matching/pose_sensor/pose_noise_p_ic: 0.0 17 | /msf_lidar_scan_matching/pose_sensor/pose_delay: 0.02 18 | /msf_lidar_scan_matching/pose_sensor/pose_noise_meas_p: 0.05 19 | /msf_lidar_scan_matching/pose_sensor/pose_noise_meas_q: 0.05 20 | 21 | /msf_lidar_scan_matching/pose_sensor/init/q_ic/w: 1.0 22 | /msf_lidar_scan_matching/pose_sensor/init/q_ic/x: 0.0 23 | /msf_lidar_scan_matching/pose_sensor/init/q_ic/y: 0.0 24 | /msf_lidar_scan_matching/pose_sensor/init/q_ic/z: 0.0 25 | /msf_lidar_scan_matching/pose_sensor/init/p_ic/x: 0.0 26 | /msf_lidar_scan_matching/pose_sensor/init/p_ic/y: 0.0 27 | /msf_lidar_scan_matching/pose_sensor/init/p_ic/z: 0.0 28 | 29 | /msf_lidar_scan_matching/pose_sensor/pose_fixed_scale: true 30 | /msf_lidar_scan_matching/pose_sensor/pose_fixed_p_ic: false 31 | /msf_lidar_scan_matching/pose_sensor/pose_fixed_q_ic: false 32 | /msf_lidar_scan_matching/pose_sensor/pose_fixed_p_wv: false 33 | /msf_lidar_scan_matching/pose_sensor/pose_fixed_q_wv: false 34 | 35 | /msf_lidar_scan_matching/pose_sensor/pose_absolute_measurements: false 36 | /msf_lidar_scan_matching/pose_sensor/pose_use_fixed_covariance: true 37 | /msf_lidar_scan_matching/pose_sensor/pose_measurement_world_sensor: true # selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam 38 | 39 | /msf_lidar_scan_matching/pose_sensor/pose_measurement_minimum_dt: 0.05 # Sets the minimum time in seconds between two pose measurements. 40 | -------------------------------------------------------------------------------- /msg/FloorCoeffs.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float32[] coeffs 4 | -------------------------------------------------------------------------------- /msg/ScanMatchingStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | bool has_converged 4 | float32 matching_error 5 | float32 inlier_fraction 6 | geometry_msgs/Pose relative_pose 7 | 8 | std_msgs/String[] prediction_labels 9 | geometry_msgs/Pose[] prediction_errors -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hdl_graph_slam 4 | 0.0.0 5 | The hdl_graph_slam package 6 | 7 | koide 8 | 9 | BSD 10 | 11 | catkin 12 | libg2o 13 | message_generation 14 | eigen 15 | fast_gicp 16 | geodesy 17 | geometry_msgs 18 | interactive_markers 19 | ndt_omp 20 | nmea_msgs 21 | pcl_ros 22 | roscpp 23 | rospy 24 | sensor_msgs 25 | std_msgs 26 | tf_conversions 27 | message_runtime 28 | msf_updates 29 | nodelet 30 | tf 31 | 32 | python-scipy 33 | python3-scipy 34 | python-progressbar 35 | python3-progressbar 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /rviz/hdl_graph_slam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 125 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /MarkerArray1/Namespaces1 10 | Splitter Ratio: 0.5 11 | Tree Height: 670 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: PointCloud2 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 10 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 50 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/TF 57 | Enabled: true 58 | Frame Timeout: 50 59 | Frames: 60 | All Enabled: true 61 | base_link: 62 | Value: true 63 | keyframe: 64 | Value: true 65 | map: 66 | Value: true 67 | odom: 68 | Value: true 69 | velodyne: 70 | Value: true 71 | Marker Scale: 1 72 | Name: TF 73 | Show Arrows: true 74 | Show Axes: true 75 | Show Names: true 76 | Tree: 77 | map: 78 | odom: 79 | base_link: 80 | velodyne: 81 | {} 82 | keyframe: 83 | {} 84 | Update Interval: 0 85 | Value: true 86 | - Alpha: 1 87 | Autocompute Intensity Bounds: true 88 | Autocompute Value Bounds: 89 | Max Value: 10 90 | Min Value: -10 91 | Value: true 92 | Axis: Z 93 | Channel Name: intensity 94 | Class: rviz/PointCloud2 95 | Color: 255; 255; 255 96 | Color Transformer: Intensity 97 | Decay Time: 0 98 | Enabled: true 99 | Invert Rainbow: false 100 | Max Color: 255; 255; 255 101 | Max Intensity: 251 102 | Min Color: 0; 0; 0 103 | Min Intensity: 0 104 | Name: PointCloud2 105 | Position Transformer: XYZ 106 | Queue Size: 10 107 | Selectable: true 108 | Size (Pixels): 3 109 | Size (m): 0.009999999776482582 110 | Style: Flat Squares 111 | Topic: /velodyne_points 112 | Unreliable: false 113 | Use Fixed Frame: true 114 | Use rainbow: true 115 | Value: true 116 | - Alpha: 1 117 | Autocompute Intensity Bounds: true 118 | Autocompute Value Bounds: 119 | Max Value: 10 120 | Min Value: -10 121 | Value: true 122 | Axis: Z 123 | Channel Name: intensity 124 | Class: rviz/PointCloud2 125 | Color: 59; 0; 255 126 | Color Transformer: FlatColor 127 | Decay Time: 0 128 | Enabled: true 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Max Intensity: 104 132 | Min Color: 0; 0; 0 133 | Min Intensity: 1 134 | Name: PointCloud2 135 | Position Transformer: XYZ 136 | Queue Size: 10 137 | Selectable: true 138 | Size (Pixels): 3 139 | Size (m): 0.15000000596046448 140 | Style: Flat Squares 141 | Topic: /floor_detection/floor_points 142 | Unreliable: false 143 | Use Fixed Frame: true 144 | Use rainbow: true 145 | Value: true 146 | - Alpha: 1 147 | Autocompute Intensity Bounds: false 148 | Autocompute Value Bounds: 149 | Max Value: 10 150 | Min Value: -10 151 | Value: true 152 | Axis: Z 153 | Channel Name: z 154 | Class: rviz/PointCloud2 155 | Color: 255; 255; 255 156 | Color Transformer: Intensity 157 | Decay Time: 0 158 | Enabled: true 159 | Invert Rainbow: false 160 | Max Color: 255; 255; 255 161 | Max Intensity: 2 162 | Min Color: 0; 0; 0 163 | Min Intensity: -1 164 | Name: PointCloud2 165 | Position Transformer: XYZ 166 | Queue Size: 1 167 | Selectable: true 168 | Size (Pixels): 3 169 | Size (m): 0.10000000149011612 170 | Style: Flat Squares 171 | Topic: /hdl_graph_slam/map_points 172 | Unreliable: false 173 | Use Fixed Frame: true 174 | Use rainbow: true 175 | Value: true 176 | - Alpha: 1 177 | Autocompute Intensity Bounds: true 178 | Autocompute Value Bounds: 179 | Max Value: 10 180 | Min Value: -10 181 | Value: true 182 | Axis: Z 183 | Channel Name: intensity 184 | Class: rviz/PointCloud2 185 | Color: 255; 255; 255 186 | Color Transformer: Intensity 187 | Decay Time: 0 188 | Enabled: true 189 | Invert Rainbow: false 190 | Max Color: 255; 255; 255 191 | Max Intensity: 251 192 | Min Color: 0; 0; 0 193 | Min Intensity: 0 194 | Name: PointCloud2 195 | Position Transformer: XYZ 196 | Queue Size: 10 197 | Selectable: true 198 | Size (Pixels): 3 199 | Size (m): 0.009999999776482582 200 | Style: Flat Squares 201 | Topic: /filtered_points 202 | Unreliable: false 203 | Use Fixed Frame: true 204 | Use rainbow: true 205 | Value: true 206 | - Class: rviz/MarkerArray 207 | Enabled: true 208 | Marker Topic: /hdl_graph_slam/markers 209 | Name: MarkerArray 210 | Namespaces: 211 | edges: true 212 | imu: true 213 | loop_close_radius: true 214 | nodes: true 215 | Queue Size: 100 216 | Value: true 217 | Enabled: true 218 | Global Options: 219 | Background Color: 48; 48; 48 220 | Default Light: true 221 | Fixed Frame: map 222 | Frame Rate: 30 223 | Name: root 224 | Tools: 225 | - Class: rviz/Interact 226 | Hide Inactive Objects: true 227 | - Class: rviz/MoveCamera 228 | - Class: rviz/Select 229 | - Class: rviz/FocusCamera 230 | - Class: rviz/Measure 231 | - Class: rviz/SetInitialPose 232 | Theta std deviation: 0.2617993950843811 233 | Topic: /initialpose 234 | X std deviation: 0.5 235 | Y std deviation: 0.5 236 | - Class: rviz/SetGoal 237 | Topic: /move_base_simple/goal 238 | - Class: rviz/PublishPoint 239 | Single click: true 240 | Topic: /clicked_point 241 | Value: true 242 | Views: 243 | Current: 244 | Class: rviz/Orbit 245 | Distance: 48.87110137939453 246 | Enable Stereo Rendering: 247 | Stereo Eye Separation: 0.05999999865889549 248 | Stereo Focal Distance: 1 249 | Swap Stereo Eyes: false 250 | Value: false 251 | Focal Point: 252 | X: 0 253 | Y: 0 254 | Z: 0 255 | Focal Shape Fixed Size: true 256 | Focal Shape Size: 0.05000000074505806 257 | Invert Z Axis: false 258 | Name: Current View 259 | Near Clip Distance: 0.009999999776482582 260 | Pitch: 0.8453980088233948 261 | Target Frame: 262 | Value: Orbit (rviz) 263 | Yaw: 0.7703980207443237 264 | Saved: ~ 265 | Window Geometry: 266 | "&Displays": 267 | collapsed: false 268 | "&Time": 269 | collapsed: false 270 | "&Views": 271 | collapsed: false 272 | Height: 1056 273 | Hide Left Dock: false 274 | Hide Right Dock: false 275 | QMainWindow State: 000000ff00000000fd00000004000000000000019d00000368fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007601000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000368000000f701000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000368fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000368000000d001000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000004cfc0100000002fb0000000800540069006d006501000000000000073f000002ad01000003fb0000000800540069006d00650100000000000004500000000000000000000004900000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 276 | Selection: 277 | collapsed: false 278 | Tool Properties: 279 | collapsed: false 280 | Width: 1855 281 | X: 62 282 | Y: 0 283 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['hdl_graph_slam'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | 13 | -------------------------------------------------------------------------------- /src/g2o/robust_kernel_io.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace g2o { 13 | 14 | std::string kernel_type(g2o::RobustKernel* kernel) { 15 | if(dynamic_cast(kernel)) { 16 | return "Huber"; 17 | } 18 | if(dynamic_cast(kernel)) { 19 | return "Cauchy"; 20 | } 21 | if(dynamic_cast(kernel)) { 22 | return "DCS"; 23 | } 24 | if(dynamic_cast(kernel)) { 25 | return "Fair"; 26 | } 27 | if(dynamic_cast(kernel)) { 28 | return "GemanMcClure"; 29 | } 30 | if(dynamic_cast(kernel)) { 31 | return "PseudoHuber"; 32 | } 33 | if(dynamic_cast(kernel)) { 34 | return "Saturated"; 35 | } 36 | if(dynamic_cast(kernel)) { 37 | return "Tukey"; 38 | } 39 | if(dynamic_cast(kernel)) { 40 | return "Welsch"; 41 | } 42 | return ""; 43 | } 44 | 45 | bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { 46 | std::ofstream ofs(filename); 47 | if(!ofs) { 48 | std::cerr << "failed to open output stream!!" << std::endl; 49 | return false; 50 | } 51 | 52 | for(const auto& edge_ : graph->edges()) { 53 | g2o::OptimizableGraph::Edge* edge = static_cast(edge_); 54 | g2o::RobustKernel* kernel = edge->robustKernel(); 55 | if(!kernel) { 56 | continue; 57 | } 58 | 59 | std::string type = kernel_type(kernel); 60 | if(type.empty()) { 61 | std::cerr << "unknown kernel type!!" << std::endl; 62 | continue; 63 | } 64 | 65 | ofs << edge->vertices().size() << " "; 66 | for(size_t i = 0; i < edge->vertices().size(); i++) { 67 | ofs << edge->vertices()[i]->id() << " "; 68 | } 69 | ofs << type << " " << kernel->delta() << std::endl; 70 | } 71 | 72 | return true; 73 | } 74 | 75 | class KernelData { 76 | public: 77 | KernelData(const std::string& line) { 78 | std::stringstream sst(line); 79 | size_t num_vertices; 80 | sst >> num_vertices; 81 | 82 | vertex_indices.resize(num_vertices); 83 | for(size_t i = 0; i < num_vertices; i++) { 84 | sst >> vertex_indices[i]; 85 | } 86 | 87 | sst >> type >> delta; 88 | } 89 | 90 | bool match(g2o::OptimizableGraph::Edge* edge) const { 91 | if(edge->vertices().size() != vertex_indices.size()) { 92 | return false; 93 | } 94 | 95 | for(size_t i = 0; i < edge->vertices().size(); i++) { 96 | if(edge->vertices()[i]->id() != vertex_indices[i]) { 97 | return false; 98 | } 99 | } 100 | 101 | return true; 102 | } 103 | 104 | g2o::RobustKernel* create() const { 105 | auto factory = g2o::RobustKernelFactory::instance(); 106 | g2o::RobustKernel* kernel = factory->construct(type); 107 | kernel->setDelta(delta); 108 | 109 | return kernel; 110 | } 111 | 112 | public: 113 | std::vector vertex_indices; 114 | std::string type; 115 | double delta; 116 | }; 117 | 118 | bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { 119 | std::ifstream ifs(filename); 120 | if(!ifs) { 121 | std::cerr << "warning: failed to open input stream!!" << std::endl; 122 | return true; 123 | } 124 | 125 | std::vector kernels; 126 | 127 | while(!ifs.eof()) { 128 | std::string line; 129 | std::getline(ifs, line); 130 | if(line.empty()) { 131 | continue; 132 | } 133 | 134 | kernels.push_back(KernelData(line)); 135 | } 136 | std::cout << "kernels: " << kernels.size() << std::endl; 137 | 138 | for(auto& edge_ : graph->edges()) { 139 | g2o::OptimizableGraph::Edge* edge = static_cast(edge_); 140 | 141 | for(auto itr = kernels.begin(); itr != kernels.end(); itr++) { 142 | if(itr->match(edge)) { 143 | edge->setRobustKernel(itr->create()); 144 | kernels.erase(itr); 145 | break; 146 | } 147 | } 148 | } 149 | 150 | if(kernels.size() != 0) { 151 | std::cerr << "warning: there is non-associated kernels!!" << std::endl; 152 | } 153 | return true; 154 | } 155 | 156 | } // namespace g2o 157 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_graph_slam/95b8dce41c10667dcf0e3e9801dc22754b3bd525/src/hdl_graph_slam/__init__.py -------------------------------------------------------------------------------- /src/hdl_graph_slam/bag_player.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # SPDX-License-Identifier: BSD-2-Clause 3 | import sys 4 | import yaml 5 | import time 6 | import curses 7 | import argparse 8 | 9 | try: 10 | from StringIO import StringIO ## for Python 2 11 | except ImportError: 12 | from io import StringIO ## for Python 3 13 | 14 | 15 | import rospy 16 | import rosbag 17 | import roslib 18 | from std_msgs.msg import * 19 | from sensor_msgs.msg import * 20 | from rosgraph_msgs.msg import * 21 | from progressbar import ProgressBar 22 | 23 | 24 | class BagPlayer: 25 | def __init__(self, bagfile, start, duration): 26 | print('opening...',) 27 | self.bag = rosbag.Bag(bagfile, 'r') 28 | print('done') 29 | 30 | self.message_generator = self.bag.read_messages() 31 | 32 | info_dict = yaml.safe_load(self.bag._get_yaml_info()) 33 | self.duration = float(info_dict['duration']) 34 | self.endtime = float(info_dict['end']) 35 | 36 | self.progress = ProgressBar(0, self.duration, term_width=80, fd=StringIO()) 37 | 38 | self.publishers = {} 39 | for con in self.bag._get_connections(): 40 | msg_class = roslib.message.get_message_class(con.datatype) 41 | self.publishers[con.topic] = rospy.Publisher(con.topic, msg_class, queue_size=256) 42 | self.clock_pub = rospy.Publisher('/clock', Clock, queue_size=256) 43 | 44 | self.init_time = None 45 | self.start = start 46 | self.duration = duration 47 | self.fail_count = 0 48 | self.time_subs = {} 49 | self.target_times = {} 50 | self.latest_stamps = {} 51 | 52 | self.play() 53 | 54 | def update_time_subs(self): 55 | for topic_name, msg_type in rospy.get_published_topics(): 56 | if 'read_until' in topic_name and 'std_msgs/Header' in msg_type: 57 | if topic_name not in self.time_subs: 58 | print('ADD', topic_name) 59 | self.time_subs[topic_name] = rospy.Subscriber(topic_name, Header, self.time_callback, topic_name) 60 | 61 | def time_callback(self, header_msg, topic_name): 62 | if header_msg.frame_id not in self.target_times: 63 | self.target_times[header_msg.frame_id] = {} 64 | 65 | if topic_name not in self.target_times[header_msg.frame_id] or self.target_times[header_msg.frame_id][topic_name] < header_msg.stamp: 66 | self.target_times[header_msg.frame_id][topic_name] = header_msg.stamp 67 | 68 | def play_realtime(self, duration): 69 | topic, msg, stamp = next(self.message_generator) 70 | stamp_wall = time.time() 71 | 72 | start_stamp = stamp 73 | start_stamp_wall = stamp_wall 74 | 75 | self.start_stamp = start_stamp 76 | 77 | while not rospy.is_shutdown() and (stamp - start_stamp) < duration: 78 | stamp_wall = time.time() 79 | elapsed_stamp = stamp - start_stamp 80 | if (stamp_wall - start_stamp_wall) < (elapsed_stamp.secs + elapsed_stamp.nsecs * 1e-9): 81 | time.sleep(1e-6) 82 | self.update_time_subs() 83 | continue 84 | 85 | clock_msg = Clock() 86 | clock_msg.clock = stamp 87 | 88 | if self.init_time is None: 89 | self.init_time = stamp 90 | 91 | if self.start and (stamp - self.init_time) < rospy.Duration(float(self.start)): 92 | start_stamp = stamp 93 | else: 94 | self.clock_pub.publish(clock_msg) 95 | self.publishers[topic].publish(msg) 96 | 97 | topic, msg, stamp = next(self.message_generator) 98 | 99 | return topic, msg, stamp 100 | 101 | def print_progress(self, stamp): 102 | self.stdscr.clear() 103 | self.stdscr.addstr(0, 0, 'topic') 104 | self.stdscr.addstr(0, 25, 'time') 105 | 106 | line = 1 107 | for target in self.target_times: 108 | if target not in self.publishers: 109 | continue 110 | 111 | for sub_name in self.target_times[target]: 112 | target_time = self.target_times[target][sub_name] 113 | self.stdscr.addstr(line, 0, sub_name[:-11]) 114 | self.stdscr.addstr(line, 25, '%.6f' % (target_time.secs + target_time.nsecs * 1e-9)) 115 | 116 | residual = target_time - self.latest_stamps[target].stamp 117 | 118 | color = 1 if residual.to_sec() > 0.0 else 2 119 | self.stdscr.addstr(line, 50, '%.5f' % residual.to_sec(), curses.color_pair(color)) 120 | line += 1 121 | 122 | if not hasattr(self, 'prev_stamp'): 123 | self.prev_stamp = stamp 124 | self.prev_stamp_wall = time.time() 125 | self.processing_speed = 1.0 126 | elif time.time() - self.prev_stamp_wall > 1.0: 127 | sim_duration = (stamp - self.prev_stamp).to_sec() 128 | wall_duration = time.time() - self.prev_stamp_wall 129 | self.processing_speed = sim_duration / wall_duration 130 | 131 | self.stdscr.addstr(line, 0, 'current_stamp') 132 | self.stdscr.addstr(line, 25, '%.6f' % stamp.to_sec()) 133 | self.stdscr.addstr(line, 50, '(x%.2f)' % self.processing_speed) 134 | 135 | elapsed = (stamp - self.start_stamp).to_sec() 136 | self.progress.fd = StringIO() 137 | try: 138 | self.progress.update(elapsed) 139 | except: 140 | # nothing to do 141 | pass 142 | self.stdscr.addstr(line + 1, 0, '----------') 143 | self.stdscr.addstr(line + 2, 0, self.progress.fd.getvalue()) 144 | 145 | self.stdscr.refresh() 146 | 147 | def check_stamp(self, topic, msg): 148 | if topic not in self.target_times: 149 | return True 150 | 151 | if self.fail_count > 10: 152 | self.fail_count = 0 153 | return True 154 | 155 | target_time_map = self.target_times[topic] 156 | for sub_name in target_time_map: 157 | self.latest_stamps[topic] = msg.header 158 | if msg.header.stamp > target_time_map[sub_name]: 159 | self.fail_count += 1 160 | return False 161 | 162 | self.fail_count = 0 163 | return True 164 | 165 | def play(self): 166 | print('play realtime for 3.0[sec]') 167 | topic, msg, stamp = self.play_realtime(rospy.Duration(5.0)) 168 | self.update_time_subs() 169 | 170 | print('play as fast as possible') 171 | self.stdscr = curses.initscr() 172 | curses.start_color() 173 | curses.noecho() 174 | 175 | curses.init_pair(1, curses.COLOR_BLUE, curses.COLOR_WHITE) 176 | curses.init_pair(2, curses.COLOR_RED, curses.COLOR_WHITE) 177 | 178 | try: 179 | while not rospy.is_shutdown(): 180 | if not self.check_stamp(topic, msg): 181 | self.update_time_subs() 182 | self.print_progress(stamp) 183 | time.sleep(0.1) 184 | continue 185 | 186 | clock_msg = Clock() 187 | clock_msg.clock = stamp 188 | 189 | if self.duration: 190 | if (stamp - self.init_time) > rospy.Duration(float(self.duration)): 191 | break 192 | 193 | self.clock_pub.publish(clock_msg) 194 | self.publishers[topic].publish(msg) 195 | topic, msg, stamp = next(self.message_generator) 196 | except: 197 | print(sys.exc_info()[0]) 198 | clock_msg = Clock() 199 | clock_msg.clock = stamp + rospy.Duration(30.0) 200 | self.clock_pub.publish(clock_msg) 201 | time.sleep(0.5) 202 | 203 | curses.echo() 204 | curses.endwin() 205 | 206 | 207 | def main(): 208 | myargv = rospy.myargv(sys.argv) 209 | parser = argparse.ArgumentParser() 210 | parser.add_argument('input_bag', help='bag file to be played') 211 | parser.add_argument('-s', '--start', help='start sec seconds into the bag') 212 | parser.add_argument('-u', '--duration', help='play only sec seconds into the bag') 213 | args = parser.parse_args(myargv[1:]) 214 | 215 | if len(sys.argv) < 2: 216 | print('usage bag_player src_bagname') 217 | return 218 | 219 | rospy.init_node('bag_player') 220 | BagPlayer(args.input_bag, args.start, args.duration) 221 | 222 | if __name__ == '__main__': 223 | main() 224 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/ford2bag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # SPDX-License-Identifier: BSD-2-Clause 3 | import re 4 | import os 5 | import sys 6 | import struct 7 | import numpy 8 | import scipy.io 9 | 10 | import rospy 11 | import rosbag 12 | import progressbar 13 | import sensor_msgs.point_cloud2 as pc2 14 | 15 | from sensor_msgs.msg import NavSatFix, NavSatStatus, PointCloud2 16 | from geographic_msgs.msg import GeoPointStamped 17 | 18 | 19 | def gps2navsat(filename, bag): 20 | with open(filename, 'r') as file: 21 | try: 22 | while True: 23 | data = struct.unpack('qddd', file.read(8*4)) 24 | time = data[0] 25 | local = data[1:] 26 | lat_lon_el_theta = struct.unpack('dddd', file.read(8*4)) 27 | gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16)) 28 | 29 | if abs(lat_lon_el_theta[0]) < 1e-1: 30 | continue 31 | 32 | navsat = NavSatFix() 33 | navsat.header.frame_id = 'gps' 34 | navsat.header.stamp = rospy.Time.from_sec(time * 1e-6) 35 | navsat.status.status = NavSatStatus.STATUS_FIX 36 | navsat.status.service = NavSatStatus.SERVICE_GPS 37 | 38 | navsat.latitude = lat_lon_el_theta[0] 39 | navsat.longitude = lat_lon_el_theta[1] 40 | navsat.altitude = lat_lon_el_theta[2] 41 | 42 | navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist() 43 | navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN 44 | 45 | bag.write('/gps/fix', navsat, navsat.header.stamp) 46 | 47 | geopoint = GeoPointStamped() 48 | geopoint.header = navsat.header 49 | geopoint.position.latitude = lat_lon_el_theta[0] 50 | geopoint.position.longitude = lat_lon_el_theta[1] 51 | geopoint.position.altitude = lat_lon_el_theta[2] 52 | 53 | bag.write('/gps/geopoint', geopoint, geopoint.header.stamp) 54 | 55 | except: 56 | print 'done' 57 | 58 | 59 | def mat2pointcloud(filename): 60 | m = scipy.io.loadmat(filename) 61 | scan = numpy.transpose(m['SCAN']['XYZ'][0][0]).astype(numpy.float32) 62 | stamp = m['SCAN']['timestamp_laser'][0][0][0][0] * 1e-6 63 | 64 | cloud = PointCloud2() 65 | cloud.header.stamp = rospy.Time.from_sec(stamp) 66 | cloud.header.frame_id = 'velodyne' 67 | cloud = pc2.create_cloud_xyz32(cloud.header, scan) 68 | return cloud 69 | 70 | 71 | def main(): 72 | if len(sys.argv) < 2: 73 | print 'usage: ford2bag.py src_dirname output_filename' 74 | 75 | output_filename = sys.argv[1] 76 | 77 | rospy.init_node('bag') 78 | filenames = sorted(['SCANS/' + x for x in os.listdir('SCANS') if re.match('Scan[0-9]*\.mat', x)]) 79 | print filenames 80 | 81 | progress = progressbar.ProgressBar(max_value=len(filenames)) 82 | pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=32) 83 | with rosbag.Bag(output_filename, 'w') as bag: 84 | gps2navsat('GPS.log', bag) 85 | for i, filename in enumerate(filenames): 86 | if rospy.is_shutdown(): 87 | break 88 | progress.update(i) 89 | cloud = mat2pointcloud(filename) 90 | if pub.get_num_connections(): 91 | pub.publish(cloud) 92 | bag.write('/velodyne_points', cloud, cloud.header.stamp) 93 | 94 | 95 | if __name__ == '__main__': 96 | main() 97 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/graph_slam.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | G2O_USE_OPTIMIZATION_LIBRARY(pcg) 29 | G2O_USE_OPTIMIZATION_LIBRARY(cholmod) // be aware of that cholmod brings GPL dependency 30 | G2O_USE_OPTIMIZATION_LIBRARY(csparse) // be aware of that csparse brings LGPL unless it is dynamically linked 31 | 32 | namespace g2o { 33 | G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane) 34 | G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY) 35 | G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ) 36 | G2O_REGISTER_TYPE(EDGE_SE3_PRIORVEC, EdgeSE3PriorVec) 37 | G2O_REGISTER_TYPE(EDGE_SE3_PRIORQUAT, EdgeSE3PriorQuat) 38 | G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_NORMAL, EdgePlanePriorNormal) 39 | G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_DISTANCE, EdgePlanePriorDistance) 40 | G2O_REGISTER_TYPE(EDGE_PLANE_IDENTITY, EdgePlaneIdentity) 41 | G2O_REGISTER_TYPE(EDGE_PLANE_PARALLEL, EdgePlaneParallel) 42 | G2O_REGISTER_TYPE(EDGE_PLANE_PAERPENDICULAR, EdgePlanePerpendicular) 43 | } // namespace g2o 44 | 45 | namespace hdl_graph_slam { 46 | 47 | /** 48 | * @brief constructor 49 | */ 50 | GraphSLAM::GraphSLAM(const std::string& solver_type) { 51 | graph.reset(new g2o::SparseOptimizer()); 52 | g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); 53 | 54 | std::cout << "construct solver: " << solver_type << std::endl; 55 | g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance(); 56 | g2o::OptimizationAlgorithmProperty solver_property; 57 | g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property); 58 | graph->setAlgorithm(solver); 59 | 60 | if(!graph->solver()) { 61 | std::cerr << std::endl; 62 | std::cerr << "error : failed to allocate solver!!" << std::endl; 63 | solver_factory->listSolvers(std::cerr); 64 | std::cerr << "-------------" << std::endl; 65 | std::cin.ignore(1); 66 | return; 67 | } 68 | std::cout << "done" << std::endl; 69 | 70 | robust_kernel_factory = g2o::RobustKernelFactory::instance(); 71 | } 72 | 73 | /** 74 | * @brief destructor 75 | */ 76 | GraphSLAM::~GraphSLAM() { 77 | graph.reset(); 78 | } 79 | 80 | void GraphSLAM::set_solver(const std::string& solver_type) { 81 | g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); 82 | 83 | std::cout << "construct solver: " << solver_type << std::endl; 84 | g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance(); 85 | g2o::OptimizationAlgorithmProperty solver_property; 86 | g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property); 87 | graph->setAlgorithm(solver); 88 | 89 | if(!graph->solver()) { 90 | std::cerr << std::endl; 91 | std::cerr << "error : failed to allocate solver!!" << std::endl; 92 | solver_factory->listSolvers(std::cerr); 93 | std::cerr << "-------------" << std::endl; 94 | std::cin.ignore(1); 95 | return; 96 | } 97 | std::cout << "done" << std::endl; 98 | } 99 | 100 | int GraphSLAM::num_vertices() const { 101 | return graph->vertices().size(); 102 | } 103 | int GraphSLAM::num_edges() const { 104 | return graph->edges().size(); 105 | } 106 | 107 | g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) { 108 | g2o::VertexSE3* vertex(new g2o::VertexSE3()); 109 | vertex->setId(static_cast(graph->vertices().size())); 110 | vertex->setEstimate(pose); 111 | graph->addVertex(vertex); 112 | 113 | return vertex; 114 | } 115 | 116 | g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) { 117 | g2o::VertexPlane* vertex(new g2o::VertexPlane()); 118 | vertex->setId(static_cast(graph->vertices().size())); 119 | vertex->setEstimate(plane_coeffs); 120 | graph->addVertex(vertex); 121 | 122 | return vertex; 123 | } 124 | 125 | g2o::VertexPointXYZ* GraphSLAM::add_point_xyz_node(const Eigen::Vector3d& xyz) { 126 | g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ()); 127 | vertex->setId(static_cast(graph->vertices().size())); 128 | vertex->setEstimate(xyz); 129 | graph->addVertex(vertex); 130 | 131 | return vertex; 132 | } 133 | 134 | g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) { 135 | g2o::EdgeSE3* edge(new g2o::EdgeSE3()); 136 | edge->setMeasurement(relative_pose); 137 | edge->setInformation(information_matrix); 138 | edge->vertices()[0] = v1; 139 | edge->vertices()[1] = v2; 140 | graph->addEdge(edge); 141 | 142 | return edge; 143 | } 144 | 145 | g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) { 146 | g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane()); 147 | edge->setMeasurement(plane_coeffs); 148 | edge->setInformation(information_matrix); 149 | edge->vertices()[0] = v_se3; 150 | edge->vertices()[1] = v_plane; 151 | graph->addEdge(edge); 152 | 153 | return edge; 154 | } 155 | 156 | g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) { 157 | g2o::EdgeSE3PointXYZ* edge(new g2o::EdgeSE3PointXYZ()); 158 | edge->setMeasurement(xyz); 159 | edge->setInformation(information_matrix); 160 | edge->vertices()[0] = v_se3; 161 | edge->vertices()[1] = v_xyz; 162 | graph->addEdge(edge); 163 | 164 | return edge; 165 | } 166 | 167 | g2o::EdgePlanePriorNormal* GraphSLAM::add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix) { 168 | g2o::EdgePlanePriorNormal* edge(new g2o::EdgePlanePriorNormal()); 169 | edge->setMeasurement(normal); 170 | edge->setInformation(information_matrix); 171 | edge->vertices()[0] = v; 172 | graph->addEdge(edge); 173 | 174 | return edge; 175 | } 176 | 177 | g2o::EdgePlanePriorDistance* GraphSLAM::add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix) { 178 | g2o::EdgePlanePriorDistance* edge(new g2o::EdgePlanePriorDistance()); 179 | edge->setMeasurement(distance); 180 | edge->setInformation(information_matrix); 181 | edge->vertices()[0] = v; 182 | graph->addEdge(edge); 183 | 184 | return edge; 185 | } 186 | 187 | g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) { 188 | g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY()); 189 | edge->setMeasurement(xy); 190 | edge->setInformation(information_matrix); 191 | edge->vertices()[0] = v_se3; 192 | graph->addEdge(edge); 193 | 194 | return edge; 195 | } 196 | 197 | g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) { 198 | g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ()); 199 | edge->setMeasurement(xyz); 200 | edge->setInformation(information_matrix); 201 | edge->vertices()[0] = v_se3; 202 | graph->addEdge(edge); 203 | 204 | return edge; 205 | } 206 | 207 | g2o::EdgeSE3PriorVec* GraphSLAM::add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix) { 208 | Eigen::Matrix m; 209 | m.head<3>() = direction; 210 | m.tail<3>() = measurement; 211 | 212 | g2o::EdgeSE3PriorVec* edge(new g2o::EdgeSE3PriorVec()); 213 | edge->setMeasurement(m); 214 | edge->setInformation(information_matrix); 215 | edge->vertices()[0] = v_se3; 216 | graph->addEdge(edge); 217 | 218 | return edge; 219 | } 220 | 221 | g2o::EdgeSE3PriorQuat* GraphSLAM::add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix) { 222 | g2o::EdgeSE3PriorQuat* edge(new g2o::EdgeSE3PriorQuat()); 223 | edge->setMeasurement(quat); 224 | edge->setInformation(information_matrix); 225 | edge->vertices()[0] = v_se3; 226 | graph->addEdge(edge); 227 | 228 | return edge; 229 | } 230 | 231 | g2o::EdgePlane* GraphSLAM::add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) { 232 | g2o::EdgePlane* edge(new g2o::EdgePlane()); 233 | edge->setMeasurement(measurement); 234 | edge->setInformation(information); 235 | edge->vertices()[0] = v_plane1; 236 | edge->vertices()[1] = v_plane2; 237 | graph->addEdge(edge); 238 | 239 | return edge; 240 | } 241 | 242 | g2o::EdgePlaneIdentity* GraphSLAM::add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) { 243 | g2o::EdgePlaneIdentity* edge(new g2o::EdgePlaneIdentity()); 244 | edge->setMeasurement(measurement); 245 | edge->setInformation(information); 246 | edge->vertices()[0] = v_plane1; 247 | edge->vertices()[1] = v_plane2; 248 | graph->addEdge(edge); 249 | 250 | return edge; 251 | } 252 | 253 | g2o::EdgePlaneParallel* GraphSLAM::add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information) { 254 | g2o::EdgePlaneParallel* edge(new g2o::EdgePlaneParallel()); 255 | edge->setMeasurement(measurement); 256 | edge->setInformation(information); 257 | edge->vertices()[0] = v_plane1; 258 | edge->vertices()[1] = v_plane2; 259 | graph->addEdge(edge); 260 | 261 | return edge; 262 | } 263 | 264 | g2o::EdgePlanePerpendicular* GraphSLAM::add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information) { 265 | g2o::EdgePlanePerpendicular* edge(new g2o::EdgePlanePerpendicular()); 266 | edge->setMeasurement(measurement); 267 | edge->setInformation(information); 268 | edge->vertices()[0] = v_plane1; 269 | edge->vertices()[1] = v_plane2; 270 | graph->addEdge(edge); 271 | 272 | return edge; 273 | } 274 | 275 | void GraphSLAM::add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size) { 276 | if(kernel_type == "NONE") { 277 | return; 278 | } 279 | 280 | g2o::RobustKernel* kernel = robust_kernel_factory->construct(kernel_type); 281 | if(kernel == nullptr) { 282 | std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl; 283 | return; 284 | } 285 | 286 | kernel->setDelta(kernel_size); 287 | 288 | g2o::OptimizableGraph::Edge* edge_ = dynamic_cast(edge); 289 | edge_->setRobustKernel(kernel); 290 | } 291 | 292 | int GraphSLAM::optimize(int num_iterations) { 293 | g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); 294 | if(graph->edges().size() < 10) { 295 | return -1; 296 | } 297 | 298 | std::cout << std::endl; 299 | std::cout << "--- pose graph optimization ---" << std::endl; 300 | std::cout << "nodes: " << graph->vertices().size() << " edges: " << graph->edges().size() << std::endl; 301 | std::cout << "optimizing... " << std::flush; 302 | 303 | std::cout << "init" << std::endl; 304 | graph->initializeOptimization(); 305 | graph->setVerbose(true); 306 | 307 | std::cout << "chi2" << std::endl; 308 | double chi2 = graph->chi2(); 309 | 310 | std::cout << "optimize!!" << std::endl; 311 | auto t1 = ros::WallTime::now(); 312 | int iterations = graph->optimize(num_iterations); 313 | 314 | auto t2 = ros::WallTime::now(); 315 | std::cout << "done" << std::endl; 316 | std::cout << "iterations: " << iterations << " / " << num_iterations << std::endl; 317 | std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl; 318 | std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl; 319 | 320 | return iterations; 321 | } 322 | 323 | void GraphSLAM::save(const std::string& filename) { 324 | g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); 325 | 326 | std::ofstream ofs(filename); 327 | graph->save(ofs); 328 | 329 | g2o::save_robust_kernels(filename + ".kernels", graph); 330 | } 331 | 332 | bool GraphSLAM::load(const std::string& filename) { 333 | std::cout << "loading pose graph..." << std::endl; 334 | g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); 335 | 336 | std::ifstream ifs(filename); 337 | if(!graph->load(ifs)) { 338 | return false; 339 | } 340 | 341 | std::cout << "nodes : " << graph->vertices().size() << std::endl; 342 | std::cout << "edges : " << graph->edges().size() << std::endl; 343 | 344 | if(!g2o::load_robust_kernels(filename + ".kernels", graph)) { 345 | return false; 346 | } 347 | 348 | return true; 349 | } 350 | 351 | } // namespace hdl_graph_slam 352 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/information_matrix_calculator.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | InformationMatrixCalculator::InformationMatrixCalculator(ros::NodeHandle& nh) { 11 | use_const_inf_matrix = nh.param("use_const_inf_matrix", false); 12 | const_stddev_x = nh.param("const_stddev_x", 0.5); 13 | const_stddev_q = nh.param("const_stddev_q", 0.1); 14 | 15 | var_gain_a = nh.param("var_gain_a", 20.0); 16 | min_stddev_x = nh.param("min_stddev_x", 0.1); 17 | max_stddev_x = nh.param("max_stddev_x", 5.0); 18 | min_stddev_q = nh.param("min_stddev_q", 0.05); 19 | max_stddev_q = nh.param("max_stddev_q", 0.2); 20 | fitness_score_thresh = nh.param("fitness_score_thresh", 0.5); 21 | } 22 | 23 | InformationMatrixCalculator::~InformationMatrixCalculator() {} 24 | 25 | Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const { 26 | if(use_const_inf_matrix) { 27 | Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); 28 | inf.topLeftCorner(3, 3).array() /= const_stddev_x; 29 | inf.bottomRightCorner(3, 3).array() /= const_stddev_q; 30 | return inf; 31 | } 32 | 33 | double fitness_score = calc_fitness_score(cloud1, cloud2, relpose); 34 | 35 | double min_var_x = std::pow(min_stddev_x, 2); 36 | double max_var_x = std::pow(max_stddev_x, 2); 37 | double min_var_q = std::pow(min_stddev_q, 2); 38 | double max_var_q = std::pow(max_stddev_q, 2); 39 | 40 | float w_x = weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score); 41 | float w_q = weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score); 42 | 43 | Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); 44 | inf.topLeftCorner(3, 3).array() /= w_x; 45 | inf.bottomRightCorner(3, 3).array() /= w_q; 46 | return inf; 47 | } 48 | 49 | double InformationMatrixCalculator::calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range) { 50 | pcl::search::KdTree::Ptr tree_(new pcl::search::KdTree()); 51 | tree_->setInputCloud(cloud1); 52 | 53 | double fitness_score = 0.0; 54 | 55 | // Transform the input dataset using the final transformation 56 | pcl::PointCloud input_transformed; 57 | pcl::transformPointCloud(*cloud2, input_transformed, relpose.cast()); 58 | 59 | std::vector nn_indices(1); 60 | std::vector nn_dists(1); 61 | 62 | // For each point in the source dataset 63 | int nr = 0; 64 | for(size_t i = 0; i < input_transformed.points.size(); ++i) { 65 | // Find its nearest neighbor in the target 66 | tree_->nearestKSearch(input_transformed.points[i], 1, nn_indices, nn_dists); 67 | 68 | // Deal with occlusions (incomplete targets) 69 | if(nn_dists[0] <= max_range) { 70 | // Add to the fitness score 71 | fitness_score += nn_dists[0]; 72 | nr++; 73 | } 74 | } 75 | 76 | if(nr > 0) 77 | return (fitness_score / nr); 78 | else 79 | return (std::numeric_limits::max()); 80 | } 81 | 82 | } // namespace hdl_graph_slam 83 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/keyframe.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hdl_graph_slam { 12 | 13 | KeyFrame::KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud::ConstPtr& cloud) : stamp(stamp), odom(odom), accum_distance(accum_distance), cloud(cloud), node(nullptr) {} 14 | 15 | KeyFrame::KeyFrame(const std::string& directory, g2o::HyperGraph* graph) : stamp(), odom(Eigen::Isometry3d::Identity()), accum_distance(-1), cloud(nullptr), node(nullptr) { 16 | load(directory, graph); 17 | } 18 | 19 | KeyFrame::~KeyFrame() {} 20 | 21 | void KeyFrame::save(const std::string& directory) { 22 | if(!boost::filesystem::is_directory(directory)) { 23 | boost::filesystem::create_directory(directory); 24 | } 25 | 26 | std::ofstream ofs(directory + "/data"); 27 | ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n"; 28 | 29 | ofs << "estimate\n"; 30 | ofs << node->estimate().matrix() << "\n"; 31 | 32 | ofs << "odom\n"; 33 | ofs << odom.matrix() << "\n"; 34 | 35 | ofs << "accum_distance " << accum_distance << "\n"; 36 | 37 | if(floor_coeffs) { 38 | ofs << "floor_coeffs " << floor_coeffs->transpose() << "\n"; 39 | } 40 | 41 | if(utm_coord) { 42 | ofs << "utm_coord " << utm_coord->transpose() << "\n"; 43 | } 44 | 45 | if(acceleration) { 46 | ofs << "acceleration " << acceleration->transpose() << "\n"; 47 | } 48 | 49 | if(orientation) { 50 | ofs << "orientation " << orientation->w() << " " << orientation->x() << " " << orientation->y() << " " << orientation->z() << "\n"; 51 | } 52 | 53 | if(node) { 54 | ofs << "id " << node->id() << "\n"; 55 | } 56 | 57 | pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud); 58 | } 59 | 60 | bool KeyFrame::load(const std::string& directory, g2o::HyperGraph* graph) { 61 | std::ifstream ifs(directory + "/data"); 62 | if(!ifs) { 63 | return false; 64 | } 65 | 66 | long node_id = -1; 67 | boost::optional estimate; 68 | 69 | while(!ifs.eof()) { 70 | std::string token; 71 | ifs >> token; 72 | 73 | if(token == "stamp") { 74 | ifs >> stamp.sec >> stamp.nsec; 75 | } else if(token == "estimate") { 76 | Eigen::Matrix4d mat; 77 | for(int i = 0; i < 4; i++) { 78 | for(int j = 0; j < 4; j++) { 79 | ifs >> mat(i, j); 80 | } 81 | } 82 | estimate = Eigen::Isometry3d::Identity(); 83 | estimate->linear() = mat.block<3, 3>(0, 0); 84 | estimate->translation() = mat.block<3, 1>(0, 3); 85 | } else if(token == "odom") { 86 | Eigen::Matrix4d odom_mat = Eigen::Matrix4d::Identity(); 87 | for(int i = 0; i < 4; i++) { 88 | for(int j = 0; j < 4; j++) { 89 | ifs >> odom_mat(i, j); 90 | } 91 | } 92 | 93 | odom.setIdentity(); 94 | odom.linear() = odom_mat.block<3, 3>(0, 0); 95 | odom.translation() = odom_mat.block<3, 1>(0, 3); 96 | } else if(token == "accum_distance") { 97 | ifs >> accum_distance; 98 | } else if(token == "floor_coeffs") { 99 | Eigen::Vector4d coeffs; 100 | ifs >> coeffs[0] >> coeffs[1] >> coeffs[2] >> coeffs[3]; 101 | floor_coeffs = coeffs; 102 | } else if(token == "utm_coord") { 103 | Eigen::Vector3d coord; 104 | ifs >> coord[0] >> coord[1] >> coord[2]; 105 | utm_coord = coord; 106 | } else if(token == "acceleration") { 107 | Eigen::Vector3d acc; 108 | ifs >> acc[0] >> acc[1] >> acc[2]; 109 | acceleration = acc; 110 | } else if(token == "orientation") { 111 | Eigen::Quaterniond quat; 112 | ifs >> quat.w() >> quat.x() >> quat.y() >> quat.z(); 113 | orientation = quat; 114 | } else if(token == "id") { 115 | ifs >> node_id; 116 | } 117 | } 118 | 119 | if(node_id < 0) { 120 | ROS_ERROR_STREAM("invalid node id!!"); 121 | ROS_ERROR_STREAM(directory); 122 | return false; 123 | } 124 | 125 | if(graph->vertices().find(node_id) == graph->vertices().end()) { 126 | ROS_ERROR_STREAM("vertex ID=" << node_id << " does not exist!!"); 127 | return false; 128 | } 129 | 130 | node = dynamic_cast(graph->vertices()[node_id]); 131 | if(node == nullptr) { 132 | ROS_ERROR_STREAM("failed to downcast!!"); 133 | return false; 134 | } 135 | 136 | if(estimate) { 137 | node->setEstimate(*estimate); 138 | } 139 | 140 | pcl::PointCloud::Ptr cloud_(new pcl::PointCloud()); 141 | pcl::io::loadPCDFile(directory + "/cloud.pcd", *cloud_); 142 | cloud = cloud_; 143 | 144 | return true; 145 | } 146 | 147 | long KeyFrame::id() const { 148 | return node->id(); 149 | } 150 | 151 | Eigen::Isometry3d KeyFrame::estimate() const { 152 | return node->estimate(); 153 | } 154 | 155 | KeyFrameSnapshot::KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud::ConstPtr& cloud) : pose(pose), cloud(cloud) {} 156 | 157 | KeyFrameSnapshot::KeyFrameSnapshot(const KeyFrame::Ptr& key) : pose(key->node->estimate()), cloud(key->cloud) {} 158 | 159 | KeyFrameSnapshot::~KeyFrameSnapshot() {} 160 | 161 | } // namespace hdl_graph_slam 162 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/map2odom_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # SPDX-License-Identifier: BSD-2-Clause 3 | import tf 4 | import rospy 5 | from geometry_msgs.msg import * 6 | 7 | 8 | class Map2OdomPublisher: 9 | def __init__(self, odom_frame_id = 'odom', map_frame_id = 'map'): 10 | self.default_odom_frame_id = odom_frame_id 11 | self.default_map_frame_id = map_frame_id 12 | self.broadcaster = tf.TransformBroadcaster() 13 | self.subscriber = rospy.Subscriber('/hdl_graph_slam/odom2pub', TransformStamped, self.callback) 14 | 15 | def callback(self, odom_msg): 16 | self.odom_msg = odom_msg 17 | 18 | def spin(self): 19 | if not hasattr(self, 'odom_msg'): 20 | self.broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.default_odom_frame_id, self.default_map_frame_id) 21 | return 22 | 23 | pose = self.odom_msg.transform 24 | pos = (pose.translation.x, pose.translation.y, pose.translation.z) 25 | quat = (pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w) 26 | 27 | map_frame_id = self.odom_msg.header.frame_id 28 | odom_frame_id = self.odom_msg.child_frame_id 29 | 30 | self.broadcaster.sendTransform(pos, quat, rospy.Time.now(), odom_frame_id, map_frame_id) 31 | 32 | 33 | def main(): 34 | rospy.init_node('map2odom_publisher') 35 | 36 | # get some parameters to define what default frame_id's should be used while we wait for our first odom message 37 | map_frame_id = rospy.get_param('~map_frame_id', 'map') 38 | odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') 39 | 40 | node = Map2OdomPublisher(odom_frame_id, map_frame_id) 41 | 42 | rate = rospy.Rate(10.0) 43 | while not rospy.is_shutdown(): 44 | node.spin() 45 | rate.sleep() 46 | 47 | if __name__ == '__main__': 48 | main() 49 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/map_cloud_generator.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace hdl_graph_slam { 8 | 9 | MapCloudGenerator::MapCloudGenerator() {} 10 | 11 | MapCloudGenerator::~MapCloudGenerator() {} 12 | 13 | pcl::PointCloud::Ptr MapCloudGenerator::generate(const std::vector& keyframes, double resolution) const { 14 | if(keyframes.empty()) { 15 | std::cerr << "warning: keyframes empty!!" << std::endl; 16 | return nullptr; 17 | } 18 | 19 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 20 | cloud->reserve(keyframes.front()->cloud->size() * keyframes.size()); 21 | 22 | for(const auto& keyframe : keyframes) { 23 | Eigen::Matrix4f pose = keyframe->pose.matrix().cast(); 24 | for(const auto& src_pt : keyframe->cloud->points) { 25 | PointT dst_pt; 26 | dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap(); 27 | dst_pt.intensity = src_pt.intensity; 28 | cloud->push_back(dst_pt); 29 | } 30 | } 31 | 32 | cloud->width = cloud->size(); 33 | cloud->height = 1; 34 | cloud->is_dense = false; 35 | 36 | if (resolution <=0.0) 37 | return cloud; // To get unfiltered point cloud with intensity 38 | 39 | pcl::octree::OctreePointCloud octree(resolution); 40 | octree.setInputCloud(cloud); 41 | octree.addPointsFromInputCloud(); 42 | 43 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 44 | octree.getOccupiedVoxelCenters(filtered->points); 45 | 46 | filtered->width = filtered->size(); 47 | filtered->height = 1; 48 | filtered->is_dense = false; 49 | 50 | return filtered; 51 | } 52 | 53 | } // namespace hdl_graph_slam 54 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/registrations.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-2-Clause 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #ifdef USE_VGICP_CUDA 17 | #include 18 | #endif 19 | 20 | namespace hdl_graph_slam { 21 | 22 | pcl::Registration::Ptr select_registration_method(ros::NodeHandle& pnh) { 23 | using PointT = pcl::PointXYZI; 24 | 25 | // select a registration method (ICP, GICP, NDT) 26 | std::string registration_method = pnh.param("registration_method", "NDT_OMP"); 27 | if(registration_method == "FAST_GICP") { 28 | std::cout << "registration: FAST_GICP" << std::endl; 29 | fast_gicp::FastGICP::Ptr gicp(new fast_gicp::FastGICP()); 30 | gicp->setNumThreads(pnh.param("reg_num_threads", 0)); 31 | gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 32 | gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 33 | gicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); 34 | gicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); 35 | return gicp; 36 | } 37 | #ifdef USE_VGICP_CUDA 38 | else if(registration_method == "FAST_VGICP_CUDA") { 39 | std::cout << "registration: FAST_VGICP_CUDA" << std::endl; 40 | fast_gicp::FastVGICPCuda::Ptr vgicp(new fast_gicp::FastVGICPCuda()); 41 | vgicp->setResolution(pnh.param("reg_resolution", 1.0)); 42 | vgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 43 | vgicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 44 | vgicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); 45 | return vgicp; 46 | } 47 | #endif 48 | else if(registration_method == "FAST_VGICP") { 49 | std::cout << "registration: FAST_VGICP" << std::endl; 50 | fast_gicp::FastVGICP::Ptr vgicp(new fast_gicp::FastVGICP()); 51 | vgicp->setNumThreads(pnh.param("reg_num_threads", 0)); 52 | vgicp->setResolution(pnh.param("reg_resolution", 1.0)); 53 | vgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 54 | vgicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 55 | vgicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); 56 | return vgicp; 57 | } else if(registration_method == "ICP") { 58 | std::cout << "registration: ICP" << std::endl; 59 | pcl::IterativeClosestPoint::Ptr icp(new pcl::IterativeClosestPoint()); 60 | icp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 61 | icp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 62 | icp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); 63 | icp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); 64 | return icp; 65 | } else if(registration_method.find("GICP") != std::string::npos) { 66 | if(registration_method.find("OMP") == std::string::npos) { 67 | std::cout << "registration: GICP" << std::endl; 68 | pcl::GeneralizedIterativeClosestPoint::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint()); 69 | gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 70 | gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 71 | gicp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); 72 | gicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); 73 | gicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); 74 | gicp->setMaximumOptimizerIterations(pnh.param("reg_max_optimizer_iterations", 20)); 75 | return gicp; 76 | } else { 77 | std::cout << "registration: GICP_OMP" << std::endl; 78 | pclomp::GeneralizedIterativeClosestPoint::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint()); 79 | gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 80 | gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 81 | gicp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); 82 | gicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); 83 | gicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); 84 | gicp->setMaximumOptimizerIterations(pnh.param("reg_max_optimizer_iterations", 20)); 85 | return gicp; 86 | } 87 | } else { 88 | if(registration_method.find("NDT") == std::string::npos) { 89 | std::cerr << "warning: unknown registration type(" << registration_method << ")" << std::endl; 90 | std::cerr << " : use NDT" << std::endl; 91 | } 92 | 93 | double ndt_resolution = pnh.param("reg_resolution", 0.5); 94 | if(registration_method.find("OMP") == std::string::npos) { 95 | std::cout << "registration: NDT " << ndt_resolution << std::endl; 96 | pcl::NormalDistributionsTransform::Ptr ndt(new pcl::NormalDistributionsTransform()); 97 | ndt->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 98 | ndt->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 99 | ndt->setResolution(ndt_resolution); 100 | return ndt; 101 | } else { 102 | int num_threads = pnh.param("reg_num_threads", 0); 103 | std::string nn_search_method = pnh.param("reg_nn_search_method", "DIRECT7"); 104 | std::cout << "registration: NDT_OMP " << nn_search_method << " " << ndt_resolution << " (" << num_threads << " threads)" << std::endl; 105 | pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); 106 | if(num_threads > 0) { 107 | ndt->setNumThreads(num_threads); 108 | } 109 | ndt->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); 110 | ndt->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); 111 | ndt->setResolution(ndt_resolution); 112 | if(nn_search_method == "KDTREE") { 113 | ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); 114 | } else if(nn_search_method == "DIRECT1") { 115 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); 116 | } else { 117 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); 118 | } 119 | return ndt; 120 | } 121 | } 122 | 123 | return nullptr; 124 | } 125 | 126 | } // namespace hdl_graph_slam 127 | -------------------------------------------------------------------------------- /srv/DumpGraph.srv: -------------------------------------------------------------------------------- 1 | 2 | string destination 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /srv/LoadGraph.srv: -------------------------------------------------------------------------------- 1 | 2 | string path 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /srv/SaveMap.srv: -------------------------------------------------------------------------------- 1 | bool utm 2 | float32 resolution 3 | string destination 4 | --- 5 | bool success 6 | --------------------------------------------------------------------------------