├── .github └── workflows │ └── main.yml ├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── Thirdparty └── ndt_omp_ros2 │ ├── .clang-format │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── apps │ └── align.cpp │ ├── data │ ├── 251370668.pcd │ ├── 251371071.pcd │ └── screenshot.png │ ├── include │ └── pclomp │ │ ├── gicp_omp.h │ │ ├── gicp_omp_impl.hpp │ │ ├── ndt_omp.h │ │ ├── ndt_omp_impl.hpp │ │ ├── voxel_grid_covariance_omp.h │ │ └── voxel_grid_covariance_omp_impl.hpp │ ├── package.xml │ └── src │ └── pclomp │ ├── gicp_omp.cpp │ ├── ndt_omp.cpp │ └── voxel_grid_covariance_omp.cpp ├── graph_based_slam ├── CMakeLists.txt ├── include │ └── graph_based_slam │ │ └── graph_based_slam_component.h ├── launch │ └── graphbasedslam.launch.py ├── package.xml ├── param │ └── graphbasedslam.yaml └── src │ ├── graph_based_slam_component.cpp │ └── graph_based_slam_node.cpp ├── lidarslam_msgs ├── CMakeLists.txt ├── msg │ ├── MapArray.msg │ └── SubMap.msg └── package.xml └── scanmatcher ├── CMakeLists.txt ├── images ├── big_loop_with_lo.png ├── big_loop_without_lo.png ├── li_slam.gif ├── li_slam.png ├── liosam_thesis.png └── rosgraph.png ├── include └── scanmatcher │ ├── scanmatcher_component.h │ └── utility.h ├── launch ├── lio.launch.py └── lio_bigloop.launch.py ├── package.xml ├── param ├── lio.yaml └── lio_bigloop.yaml ├── rviz ├── lio.rviz ├── lio_bigloop.rviz └── mapping.rviz └── src ├── image_projection.cpp ├── imu_preintegration.cpp ├── scanmatcher_component.cpp └── scanmatcher_node.cpp /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - humble 7 | - develop 8 | 9 | jobs: 10 | job: 11 | name: Build 12 | runs-on: ubuntu-22.04 13 | container: ros:humble-ros-core 14 | steps: 15 | - name: Install Git and software-properties-common 16 | run: | 17 | apt-get update 18 | apt-get install -y git software-properties-common 19 | shell: bash 20 | - uses: actions/checkout@v2 21 | with: 22 | submodules: true 23 | - name: Copy repository 24 | run: | 25 | mkdir -p ~/ros2_ws/src/li_slam_ros2 26 | cp -rf . ~/ros2_ws/src/li_slam_ros2 27 | - name: Install gtsam 28 | run: | 29 | add-apt-repository ppa:borglab/gtsam-release-4.1 30 | apt update 31 | apt install -y libgtsam-dev libgtsam-unstable-dev 32 | - name: Install dependencies 33 | run: | 34 | source /opt/ros/humble/setup.bash 35 | apt install -y python3-rosdep 36 | rosdep init 37 | rosdep update 38 | cd ~/ros2_ws/src 39 | rosdep install -r -y --from-paths . --ignore-src 40 | shell: bash 41 | - name: Build packages 42 | run: | 43 | source /opt/ros/humble/setup.bash 44 | # Install colcon 45 | # Ref: https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/ 46 | sudo apt install -y python3-colcon-common-extensions 47 | cd ~/ros2_ws 48 | colcon build 49 | source ~/ros2_ws/install/setup.bash 50 | shell: bash 51 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | scanmatcher/launch/test.launch.py 3 | scanmatcher/param/test.yaml 4 | scanmatcher/rviz/test.rviz -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "Thirdparty/ndt_omp_ros2"] 2 | path = Thirdparty/ndt_omp_ros2 3 | url = https://github.com/rsasaki0109/ndt_omp_ros2 -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2020, Ryohei Sasaki 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 | li_slam_ros2 2 | ==== 3 | 4 | This package is a combination of [lidarslam_ros2](https://github.com/rsasaki0109/lidarslam_ros2) and the [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM) IMU composite method. 5 | 6 | See LIO-SAM for IMU composites, otherwise see lidarslam_ros2. 7 | 8 | - Walking dataset(casual_walk.bag) 9 | 10 | 11 | Yellow path: path without loop closure, Green path: modified path, Red: map 12 | 13 | Reference(From the LIO-SAM paper) 14 | https://github.com/TixiaoShan/LIO-SAM/blob/master/config/doc/paper.pdf 15 | 16 | 17 | - Campus dataset (large) demo(big_loop.bag) 18 | 19 | 20 | 21 | Yellow path: path without loop closure, Red: map 22 | (the 10x10 grids in size of 10m × 10m) 23 | 24 | 25 | 26 | Green path: modified path with loop closure, Red: map 27 | 28 | 29 | 30 | ## requirement to build 31 | You need [ndt_omp_ros2](https://github.com/rsasaki0109/ndt_omp_ros2) and gtsam for scan-matcher 32 | 33 | clone 34 | ``` 35 | cd ~/ros2_ws/src 36 | git clone --recursive https://github.com/rsasaki0109/li_slam_ros2 37 | ``` 38 | gtsam install 39 | ``` 40 | sudo add-apt-repository ppa:borglab/gtsam-release-4.1 41 | sudo apt update 42 | sudo apt install libgtsam-dev libgtsam-unstable-dev 43 | ``` 44 | build 45 | ``` 46 | cd ~/ros2_ws 47 | rosdep update 48 | rosdep install --from-paths src --ignore-src -yr 49 | colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release 50 | ``` 51 | 52 | 53 | ## Walking dataset demo(casual_walk.bag) 54 | 55 | The optimization pipeline in Lidar Inertial SLAM were taken from [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM). 56 | 57 | (Note: See the LIO-SAM repository for detailed settings regarding IMU. 58 | The other thing to note is that the speed will diverge if the voxel_grid_size is large. 59 | 60 | demo data(ROS1) in LIO-SAM 61 | https://github.com/TixiaoShan/LIO-SAM 62 | To use ros1 rosbag , use [rosbags](https://pypi.org/project/rosbags/). 63 | The Velodyne VLP-16 was used in this data. 64 | 65 | ``` 66 | rviz2 -d src/li_slam_ros2/scanmatcher/rviz/lio.rviz 67 | ``` 68 | 69 | ``` 70 | ros2 launch scanmatcher lio.launch.py 71 | ``` 72 | 73 | ``` 74 | ros2 bag play walking_dataset/ 75 | ``` 76 | 77 | 78 | 79 | Green arrow: pose, Yellow path: path, Green path: path by imu 80 | 81 | 82 | 83 | Yellow path: path without loop closure, Green path: modified path, Red: map 84 | 85 | rosgraph 86 | 87 | 88 | 89 | `pose_graph.g2o` and `map.pcd` are saved in loop closing or using the following service call. 90 | 91 | ``` 92 | ros2 service call /map_save std_srvs/Empty 93 | ``` 94 | 95 | ## Campus dataset (large) demo(big_loop.bag) 96 | 97 | ``` 98 | rviz2 -d src/li_slam_ros2/scanmatcher/rviz/lio_bigloop.rviz 99 | ``` 100 | 101 | ``` 102 | ros2 launch scanmatcher lio_bigloop.launch.py 103 | ``` 104 | 105 | ``` 106 | ros2 bag play rosbag_v2 big_loop/ 107 | ``` 108 | 109 | 110 | 111 | 112 | Yellow path: path without loop closure, Red: map 113 | (the 10x10 grids in size of 10m × 10m) 114 | 115 | 116 | 117 | Green path: modified path with loop closure, Red: map 118 | 119 | 120 | 121 | ## Used Libraries 122 | 123 | - Eigen 124 | - PCL(BSD3) 125 | - g2o(BSD2 except a part) 126 | - [ndt_omp](https://github.com/koide3/ndt_omp) (BSD2) 127 | - gtsam(BSD2) 128 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/.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 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ndt_omp_ros2) 3 | 4 | # -mavx causes a lot of errors!! 5 | #add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 6 | #set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 7 | 8 | if(NOT CMAKE_CXX_STANDARD) 9 | set(CMAKE_CXX_STANDARD 14) 10 | endif() 11 | 12 | # pcl 1.7 causes a segfault when it is built with debug mode 13 | set(CMAKE_BUILD_TYPE "RELEASE") 14 | 15 | #find_package(catkin REQUIRED COMPONENTS 16 | # roscpp 17 | # pcl_ros 18 | #) 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(std_msgs REQUIRED) 22 | 23 | find_package(PCL 1.12 REQUIRED) 24 | include_directories(${PCL_INCLUDE_DIRS}) 25 | link_directories(${PCL_LIBRARY_DIRS}) 26 | add_definitions(${PCL_DEFINITIONS}) 27 | 28 | message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) 29 | message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) 30 | message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) 31 | 32 | find_package(OpenMP) 33 | if (OPENMP_FOUND) 34 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 35 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 36 | endif() 37 | 38 | ################################### 39 | ## catkin specific configuration ## 40 | ################################### 41 | #catkin_package( 42 | # INCLUDE_DIRS include 43 | # LIBRARIES ndt_omp 44 | #) 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | include_directories( 50 | include 51 | ) 52 | 53 | add_library(ndt_omp 54 | src/pclomp/voxel_grid_covariance_omp.cpp 55 | src/pclomp/ndt_omp.cpp 56 | src/pclomp/gicp_omp.cpp 57 | ) 58 | 59 | add_executable(align 60 | apps/align.cpp 61 | ) 62 | add_dependencies(align 63 | ndt_omp 64 | ) 65 | target_link_libraries(align 66 | ${PCL_LIBRARIES} 67 | ndt_omp 68 | ) 69 | 70 | ament_target_dependencies(align 71 | rclcpp 72 | std_msgs 73 | ) 74 | 75 | ############ 76 | ## INSTAL ## 77 | ############ 78 | 79 | install(TARGETS 80 | align 81 | DESTINATION lib/${PROJECT_NAME} 82 | ) 83 | 84 | install( 85 | TARGETS 86 | align 87 | ARCHIVE DESTINATION lib 88 | LIBRARY DESTINATION lib 89 | RUNTIME DESTINATION bin 90 | ) 91 | 92 | # install headers 93 | install( 94 | DIRECTORY "include/" 95 | DESTINATION include 96 | ) 97 | 98 | ament_export_dependencies(rclcpp) 99 | ament_export_include_directories(include) 100 | 101 | ament_package() 102 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/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 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/README.md: -------------------------------------------------------------------------------- 1 | # ndt_omp_ros2 2 | This package provides an OpenMP-boosted Normal Distributions Transform (and GICP) algorithm derived from pcl. The NDT algorithm is modified to be SSE-friendly and multi-threaded. It can run up to 10 times faster than its original version in pcl. 3 | 4 | ### Benchmark 5 | ``` 6 | $ cd ~/ros2_ws/src/ndt_omp_ros2/data 7 | $ ros2 run ndt_omp_ros2 align 251370668.pcd 251371071.pcd 8 | 9 | --- pcl::GICP --- 10 | single : 267.385[msec] 11 | 10times: 1151.76[msec] 12 | fitness: 0.220382 13 | 14 | --- pclomp::GICP --- 15 | single : 173.152[msec] 16 | 10times: 1299.14[msec] 17 | fitness: 0.220388 18 | 19 | --- pcl::NDT --- 20 | single : 425.142[msec] 21 | 10times: 3638.77[msec] 22 | fitness: 0.213937 23 | 24 | --- pclomp::NDT (KDTREE, 1 threads) --- 25 | single : 308.935[msec] 26 | 10times: 3095.53[msec] 27 | fitness: 0.213937 28 | 29 | --- pclomp::NDT (DIRECT7, 1 threads) --- 30 | single : 188.942[msec] 31 | 10times: 1373.47[msec] 32 | fitness: 0.214205 33 | 34 | --- pclomp::NDT (DIRECT1, 1 threads) --- 35 | single : 41.3584[msec] 36 | 10times: 347.261[msec] 37 | fitness: 0.208511 38 | 39 | --- pclomp::NDT (KDTREE, 8 threads) --- 40 | single : 108.68[msec] 41 | 10times: 1046.16[msec] 42 | fitness: 0.213937 43 | 44 | --- pclomp::NDT (DIRECT7, 8 threads) --- 45 | single : 56.9189[msec] 46 | 10times: 545.279[msec] 47 | fitness: 0.214205 48 | 49 | --- pclomp::NDT (DIRECT1, 8 threads) --- 50 | single : 16.7266[msec] 51 | 10times: 169.097[msec] 52 | fitness: 0.208511 53 | ``` 54 | 55 | Several methods for neighbor voxel search are implemented. If you select pclomp::KDTREE, results will be completely same as the original pcl::NDT. We recommend to use pclomp::DIRECT7 which is faster and stable. If you need extremely fast registration, choose pclomp::DIRECT1, but it might be a bit unstable. 56 | 57 |
58 | Red: target, Green: source, Blue: aligned 59 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/apps/align.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | //#include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | // align point clouds and measure processing time 18 | pcl::PointCloud::Ptr align(boost::shared_ptr> registration, const pcl::PointCloud::Ptr& target_cloud, const pcl::PointCloud::Ptr& source_cloud ) { 19 | registration->setInputTarget(target_cloud); 20 | registration->setInputSource(source_cloud); 21 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 22 | 23 | rclcpp::Clock system_clock; 24 | 25 | ///auto t1 = ros::WallTime::now(); 26 | auto t1 = system_clock.now(); 27 | registration->align(*aligned); 28 | auto t2 = system_clock.now(); 29 | //std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl; 30 | std::cout << "single : " << (t2 - t1).seconds()* 1000 << "[msec]" << std::endl; 31 | 32 | for(int i=0; i<10; i++) { 33 | registration->align(*aligned); 34 | } 35 | auto t3 = system_clock.now(); 36 | std::cout << "10times: " << (t3 - t2).seconds() * 1000 << "[msec]" << std::endl; 37 | std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl; 38 | 39 | return aligned; 40 | } 41 | 42 | int main(int argc, char** argv) { 43 | if(argc != 3) { 44 | std::cout << "usage: align target.pcd source.pcd" << std::endl; 45 | return 0; 46 | } 47 | 48 | std::string target_pcd = argv[1]; 49 | std::string source_pcd = argv[2]; 50 | 51 | pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud()); 52 | pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud()); 53 | 54 | if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) { 55 | std::cerr << "failed to load " << target_pcd << std::endl; 56 | return 0; 57 | } 58 | if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) { 59 | std::cerr << "failed to load " << source_pcd << std::endl; 60 | return 0; 61 | } 62 | 63 | // downsampling 64 | pcl::PointCloud::Ptr downsampled(new pcl::PointCloud()); 65 | 66 | pcl::VoxelGrid voxelgrid; 67 | voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f); 68 | 69 | voxelgrid.setInputCloud(target_cloud); 70 | voxelgrid.filter(*downsampled); 71 | *target_cloud = *downsampled; 72 | 73 | voxelgrid.setInputCloud(source_cloud); 74 | voxelgrid.filter(*downsampled); 75 | source_cloud = downsampled; 76 | 77 | //ros::Time::init(); 78 | 79 | // benchmark 80 | std::cout << "--- pcl::GICP ---" << std::endl; 81 | boost::shared_ptr> gicp(new pcl::GeneralizedIterativeClosestPoint()); 82 | pcl::PointCloud::Ptr aligned = align(gicp, target_cloud, source_cloud); 83 | 84 | //TODO:The problem of uninitialized member variables 85 | std::cout << "--- pclomp::GICP ---" << std::endl; 86 | boost::shared_ptr> gicp_omp(new pclomp::GeneralizedIterativeClosestPoint()); 87 | aligned = align(gicp_omp, target_cloud, source_cloud); 88 | 89 | 90 | std::cout << "--- pcl::NDT ---" << std::endl; 91 | boost::shared_ptr> ndt(new pcl::NormalDistributionsTransform()); 92 | ndt->setResolution(1.0); 93 | aligned = align(ndt, target_cloud, source_cloud); 94 | 95 | std::vector num_threads = {1, omp_get_max_threads()}; 96 | std::vector> search_methods = { 97 | {"KDTREE", pclomp::KDTREE}, 98 | {"DIRECT7", pclomp::DIRECT7}, 99 | {"DIRECT1", pclomp::DIRECT1} 100 | }; 101 | 102 | pclomp::NormalDistributionsTransform::Ptr ndt_omp(new pclomp::NormalDistributionsTransform()); 103 | ndt_omp->setResolution(1.0); 104 | 105 | 106 | for(int n : num_threads) { 107 | for(const auto& search_method : search_methods) { 108 | std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---" << std::endl; 109 | ndt_omp->setNumThreads(n); 110 | ndt_omp->setNeighborhoodSearchMethod(search_method.second); 111 | aligned = align(ndt_omp, target_cloud, source_cloud); 112 | } 113 | } 114 | 115 | 116 | // visulization 117 | pcl::visualization::PCLVisualizer vis("vis"); 118 | pcl::visualization::PointCloudColorHandlerCustom target_handler(target_cloud, 255.0, 0.0, 0.0); 119 | pcl::visualization::PointCloudColorHandlerCustom source_handler(source_cloud, 0.0, 255.0, 0.0); 120 | pcl::visualization::PointCloudColorHandlerCustom aligned_handler(aligned, 0.0, 0.0, 255.0); 121 | vis.addPointCloud(target_cloud, target_handler, "target"); 122 | vis.addPointCloud(source_cloud, source_handler, "source"); 123 | vis.addPointCloud(aligned, aligned_handler, "aligned"); 124 | vis.spin(); 125 | 126 | return 0; 127 | } 128 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/data/251370668.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/Thirdparty/ndt_omp_ros2/data/251370668.pcd -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/data/251371071.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/Thirdparty/ndt_omp_ros2/data/251371071.pcd -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/data/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/Thirdparty/ndt_omp_ros2/data/screenshot.png -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/include/pclomp/gicp_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | 41 | #ifndef PCL_GICP_OMP_H_ 42 | #define PCL_GICP_OMP_H_ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | namespace pclomp 49 | { 50 | /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the 51 | * generalized iterative closest point algorithm as described by Alex Segal et al. in 52 | * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf 53 | * The approach is based on using anistropic cost functions to optimize the alignment 54 | * after closest point assignments have been made. 55 | * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and 56 | * FLANN. 57 | * \author Nizar Sallem 58 | * \ingroup registration 59 | */ 60 | template 61 | class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint 62 | { 63 | public: 64 | using pcl::IterativeClosestPoint::reg_name_; 65 | using pcl::IterativeClosestPoint::getClassName; 66 | using pcl::IterativeClosestPoint::indices_; 67 | using pcl::IterativeClosestPoint::target_; 68 | using pcl::IterativeClosestPoint::input_; 69 | using pcl::IterativeClosestPoint::tree_; 70 | using pcl::IterativeClosestPoint::tree_reciprocal_; 71 | using pcl::IterativeClosestPoint::nr_iterations_; 72 | using pcl::IterativeClosestPoint::max_iterations_; 73 | using pcl::IterativeClosestPoint::previous_transformation_; 74 | using pcl::IterativeClosestPoint::final_transformation_; 75 | using pcl::IterativeClosestPoint::transformation_; 76 | using pcl::IterativeClosestPoint::transformation_epsilon_; 77 | using pcl::IterativeClosestPoint::converged_; 78 | using pcl::IterativeClosestPoint::corr_dist_threshold_; 79 | using pcl::IterativeClosestPoint::inlier_threshold_; 80 | using pcl::IterativeClosestPoint::min_number_correspondences_; 81 | using pcl::IterativeClosestPoint::update_visualizer_; 82 | 83 | using PointCloudSource = pcl::PointCloud; 84 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 85 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 86 | 87 | using PointCloudTarget = pcl::PointCloud; 88 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 89 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 90 | 91 | using PointIndicesPtr = pcl::PointIndices::Ptr; 92 | using PointIndicesConstPtr = pcl::PointIndices::ConstPtr; 93 | 94 | using MatricesVector = std::vector >; 95 | using MatricesVectorPtr = boost::shared_ptr; 96 | using MatricesVectorConstPtr = boost::shared_ptr; 97 | 98 | using InputKdTree = typename pcl::Registration::KdTree; 99 | using InputKdTreePtr = typename pcl::Registration::KdTreePtr; 100 | 101 | using Ptr = boost::shared_ptr >; 102 | using ConstPtr = boost::shared_ptr >; 103 | 104 | 105 | using Vector6d = Eigen::Matrix; 106 | 107 | /** \brief Empty constructor. */ 108 | GeneralizedIterativeClosestPoint () 109 | : k_correspondences_(20) 110 | , gicp_epsilon_(0.001) 111 | , rotation_epsilon_(2e-3) 112 | , mahalanobis_(0) 113 | , max_inner_iterations_(20) 114 | { 115 | min_number_correspondences_ = 4; 116 | reg_name_ = "GeneralizedIterativeClosestPoint"; 117 | max_iterations_ = 200; 118 | transformation_epsilon_ = 5e-4; 119 | corr_dist_threshold_ = 5.; 120 | rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src, 121 | const std::vector& indices_src, 122 | const PointCloudTarget& cloud_tgt, 123 | const std::vector& indices_tgt, 124 | Eigen::Matrix4f& transformation_matrix) 125 | { 126 | estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 127 | }; 128 | } 129 | 130 | /** \brief Provide a pointer to the input dataset 131 | * \param cloud the const boost shared pointer to a PointCloud message 132 | */ 133 | inline void 134 | setInputSource (const PointCloudSourceConstPtr &cloud) override 135 | { 136 | 137 | if (cloud->points.empty ()) 138 | { 139 | PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 140 | return; 141 | } 142 | PointCloudSource input = *cloud; 143 | // Set all the point.data[3] values to 1 to aid the rigid transformation 144 | for (size_t i = 0; i < input.size (); ++i) 145 | input[i].data[3] = 1.0; 146 | 147 | pcl::IterativeClosestPoint::setInputSource (cloud); 148 | input_covariances_.reset (); 149 | } 150 | 151 | /** \brief Provide a pointer to the covariances of the input source (if computed externally!). 152 | * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. 153 | * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). 154 | * \param[in] target the input point cloud target 155 | */ 156 | inline void 157 | setSourceCovariances (const MatricesVectorPtr& covariances) 158 | { 159 | input_covariances_ = covariances; 160 | } 161 | 162 | /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) 163 | * \param[in] target the input point cloud target 164 | */ 165 | inline void 166 | setInputTarget (const PointCloudTargetConstPtr &target) override 167 | { 168 | pcl::IterativeClosestPoint::setInputTarget(target); 169 | target_covariances_.reset (); 170 | } 171 | 172 | /** \brief Provide a pointer to the covariances of the input target (if computed externally!). 173 | * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. 174 | * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). 175 | * \param[in] target the input point cloud target 176 | */ 177 | inline void 178 | setTargetCovariances (const MatricesVectorPtr& covariances) 179 | { 180 | target_covariances_ = covariances; 181 | } 182 | 183 | /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative 184 | * non-linear Levenberg-Marquardt approach. 185 | * \param[in] cloud_src the source point cloud dataset 186 | * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 187 | * \param[in] cloud_tgt the target point cloud dataset 188 | * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src 189 | * \param[out] transformation_matrix the resultant transformation matrix 190 | */ 191 | void 192 | estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 193 | const std::vector &indices_src, 194 | const PointCloudTarget &cloud_tgt, 195 | const std::vector &indices_tgt, 196 | Eigen::Matrix4f &transformation_matrix); 197 | 198 | /** \brief \return Mahalanobis distance matrix for the given point index */ 199 | inline const Eigen::Matrix4f& mahalanobis(size_t index) const 200 | { 201 | assert(index < mahalanobis_.size()); 202 | return mahalanobis_[index]; 203 | } 204 | 205 | /** \brief Computes rotation matrix derivative. 206 | * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5] 207 | * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5] 208 | * param x array representing 3D transformation 209 | * param R rotation matrix 210 | * param g gradient vector 211 | */ 212 | void 213 | computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const; 214 | 215 | /** \brief Set the rotation epsilon (maximum allowable difference between two 216 | * consecutive rotations) in order for an optimization to be considered as having 217 | * converged to the final solution. 218 | * \param epsilon the rotation epsilon 219 | */ 220 | inline void 221 | setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } 222 | 223 | /** \brief Get the rotation epsilon (maximum allowable difference between two 224 | * consecutive rotations) as set by the user. 225 | */ 226 | inline double 227 | getRotationEpsilon () { return (rotation_epsilon_); } 228 | 229 | /** \brief Set the number of neighbors used when selecting a point neighbourhood 230 | * to compute covariances. 231 | * A higher value will bring more accurate covariance matrix but will make 232 | * covariances computation slower. 233 | * \param k the number of neighbors to use when computing covariances 234 | */ 235 | void 236 | setCorrespondenceRandomness (int k) { k_correspondences_ = k; } 237 | 238 | /** \brief Get the number of neighbors used when computing covariances as set by 239 | * the user 240 | */ 241 | int 242 | getCorrespondenceRandomness () { return (k_correspondences_); } 243 | 244 | /** set maximum number of iterations at the optimization step 245 | * \param[in] max maximum number of iterations for the optimizer 246 | */ 247 | void 248 | setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; } 249 | 250 | ///\return maximum number of iterations at the optimization step 251 | int 252 | getMaximumOptimizerIterations () { return (max_inner_iterations_); } 253 | 254 | protected: 255 | 256 | /** \brief The number of neighbors used for covariances computation. 257 | * default: 20 258 | */ 259 | int k_correspondences_; 260 | 261 | /** \brief The epsilon constant for gicp paper; this is NOT the convergence 262 | * tolerance 263 | * default: 0.001 264 | */ 265 | double gicp_epsilon_; 266 | 267 | /** The epsilon constant for rotation error. (In GICP the transformation epsilon 268 | * is split in rotation part and translation part). 269 | * default: 2e-3 270 | */ 271 | double rotation_epsilon_; 272 | 273 | /** \brief base transformation */ 274 | Eigen::Matrix4f base_transformation_; 275 | 276 | /** \brief Temporary pointer to the source dataset. */ 277 | const PointCloudSource *tmp_src_; 278 | 279 | /** \brief Temporary pointer to the target dataset. */ 280 | const PointCloudTarget *tmp_tgt_; 281 | 282 | /** \brief Temporary pointer to the source dataset indices. */ 283 | const std::vector *tmp_idx_src_; 284 | 285 | /** \brief Temporary pointer to the target dataset indices. */ 286 | const std::vector *tmp_idx_tgt_; 287 | 288 | 289 | /** \brief Input cloud points covariances. */ 290 | MatricesVectorPtr input_covariances_; 291 | 292 | /** \brief Target cloud points covariances. */ 293 | MatricesVectorPtr target_covariances_; 294 | 295 | /** \brief Mahalanobis matrices holder. */ 296 | std::vector mahalanobis_; 297 | 298 | /** \brief maximum number of optimizations */ 299 | int max_inner_iterations_; 300 | 301 | /** \brief compute points covariances matrices according to the K nearest 302 | * neighbors. K is set via setCorrespondenceRandomness() method. 303 | * \param cloud pointer to point cloud 304 | * \param tree KD tree performer for nearest neighbors search 305 | * \param[out] cloud_covariances covariances matrices for each point in the cloud 306 | */ 307 | template 308 | void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, 309 | const typename pcl::search::KdTree::ConstPtr tree, 310 | MatricesVector& cloud_covariances); 311 | 312 | /** \return trace of mat1^t . mat2 313 | * \param mat1 matrix of dimension nxm 314 | * \param mat2 matrix of dimension nxp 315 | */ 316 | inline double 317 | matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const 318 | { 319 | double r = 0.; 320 | size_t n = mat1.rows(); 321 | // tr(mat1^t.mat2) 322 | for(size_t i = 0; i < n; i++) 323 | for(size_t j = 0; j < n; j++) 324 | r += mat1 (j, i) * mat2 (i,j); 325 | return r; 326 | } 327 | 328 | /** \brief Rigid transformation computation method with initial guess. 329 | * \param output the transformed input point cloud dataset using the rigid transformation found 330 | * \param guess the initial guess of the transformation to compute 331 | */ 332 | void 333 | computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; 334 | 335 | /** \brief Search for the closest nearest neighbor of a given point. 336 | * \param query the point to search a nearest neighbour for 337 | * \param index vector of size 1 to store the index of the nearest neighbour found 338 | * \param distance vector of size 1 to store the distance to nearest neighbour found 339 | */ 340 | inline bool 341 | searchForNeighbors (const PointSource &query, std::vector& index, std::vector& distance) const 342 | { 343 | int k = tree_->nearestKSearch (query, 1, index, distance); 344 | if (k == 0) 345 | return (false); 346 | return (true); 347 | } 348 | 349 | /// \brief compute transformation matrix from transformation matrix 350 | void applyState(Eigen::Matrix4f &t, const Vector6d& x) const; 351 | 352 | /// \brief optimization functor structure 353 | struct OptimizationFunctorWithIndices : public BFGSDummyFunctor 354 | { 355 | OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp) 356 | : BFGSDummyFunctor (), gicp_(gicp) {} 357 | double operator() (const Vector6d& x) override; 358 | void df(const Vector6d &x, Vector6d &df) override; 359 | void fdf(const Vector6d &x, double &f, Vector6d &df) override; 360 | 361 | const GeneralizedIterativeClosestPoint *gicp_; 362 | }; 363 | 364 | std::function &cloud_src, 365 | const std::vector &src_indices, 366 | const pcl::PointCloud &cloud_tgt, 367 | const std::vector &tgt_indices, 368 | Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_; 369 | }; 370 | } 371 | 372 | #endif //#ifndef PCL_GICP_H_ 373 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/include/pclomp/voxel_grid_covariance_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_ 39 | #define PCL_VOXEL_GRID_COVARIANCE_OMP_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace pclomp 49 | { 50 | /** \brief A searchable voxel strucure containing the mean and covariance of the data. 51 | * \note For more information please see 52 | * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 53 | * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 54 | * PhD thesis, Orebro University. Orebro Studies in Technology 36 55 | * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 56 | */ 57 | template 58 | class VoxelGridCovariance : public pcl::VoxelGrid 59 | { 60 | protected: 61 | using pcl::VoxelGrid::filter_name_; 62 | using pcl::VoxelGrid::getClassName; 63 | using pcl::VoxelGrid::input_; 64 | using pcl::VoxelGrid::indices_; 65 | using pcl::VoxelGrid::filter_limit_negative_; 66 | using pcl::VoxelGrid::filter_limit_min_; 67 | using pcl::VoxelGrid::filter_limit_max_; 68 | using pcl::VoxelGrid::filter_field_name_; 69 | 70 | using pcl::VoxelGrid::downsample_all_data_; 71 | using pcl::VoxelGrid::leaf_layout_; 72 | using pcl::VoxelGrid::save_leaf_layout_; 73 | using pcl::VoxelGrid::leaf_size_; 74 | using pcl::VoxelGrid::min_b_; 75 | using pcl::VoxelGrid::max_b_; 76 | using pcl::VoxelGrid::inverse_leaf_size_; 77 | using pcl::VoxelGrid::div_b_; 78 | using pcl::VoxelGrid::divb_mul_; 79 | 80 | typedef typename pcl::traits::fieldList::type FieldList; 81 | typedef typename pcl::Filter::PointCloud PointCloud; 82 | typedef typename PointCloud::Ptr PointCloudPtr; 83 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 84 | 85 | public: 86 | 87 | typedef boost::shared_ptr< pcl::VoxelGrid > Ptr; 88 | typedef boost::shared_ptr< const pcl::VoxelGrid > ConstPtr; 89 | 90 | /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf. 91 | * Inverse covariance, eigen vectors and engen values are precomputed. */ 92 | struct Leaf 93 | { 94 | /** \brief Constructor. 95 | * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix 96 | */ 97 | Leaf () : 98 | nr_points (0), 99 | mean_ (Eigen::Vector3d::Zero ()), 100 | centroid (), 101 | cov_ (Eigen::Matrix3d::Identity ()), 102 | icov_ (Eigen::Matrix3d::Zero ()), 103 | evecs_ (Eigen::Matrix3d::Identity ()), 104 | evals_ (Eigen::Vector3d::Zero ()) 105 | { 106 | } 107 | 108 | /** \brief Get the voxel covariance. 109 | * \return covariance matrix 110 | */ 111 | Eigen::Matrix3d 112 | getCov () const 113 | { 114 | return (cov_); 115 | } 116 | 117 | /** \brief Get the inverse of the voxel covariance. 118 | * \return inverse covariance matrix 119 | */ 120 | Eigen::Matrix3d 121 | getInverseCov () const 122 | { 123 | return (icov_); 124 | } 125 | 126 | /** \brief Get the voxel centroid. 127 | * \return centroid 128 | */ 129 | Eigen::Vector3d 130 | getMean () const 131 | { 132 | return (mean_); 133 | } 134 | 135 | /** \brief Get the eigen vectors of the voxel covariance. 136 | * \note Order corresponds with \ref getEvals 137 | * \return matrix whose columns contain eigen vectors 138 | */ 139 | Eigen::Matrix3d 140 | getEvecs () const 141 | { 142 | return (evecs_); 143 | } 144 | 145 | /** \brief Get the eigen values of the voxel covariance. 146 | * \note Order corresponds with \ref getEvecs 147 | * \return vector of eigen values 148 | */ 149 | Eigen::Vector3d 150 | getEvals () const 151 | { 152 | return (evals_); 153 | } 154 | 155 | /** \brief Get the number of points contained by this voxel. 156 | * \return number of points 157 | */ 158 | int 159 | getPointCount () const 160 | { 161 | return (nr_points); 162 | } 163 | 164 | /** \brief Number of points contained by voxel */ 165 | int nr_points; 166 | 167 | /** \brief 3D voxel centroid */ 168 | Eigen::Vector3d mean_; 169 | 170 | /** \brief Nd voxel centroid 171 | * \note Differs from \ref mean_ when color data is used 172 | */ 173 | Eigen::VectorXf centroid; 174 | 175 | /** \brief Voxel covariance matrix */ 176 | Eigen::Matrix3d cov_; 177 | 178 | /** \brief Inverse of voxel covariance matrix */ 179 | Eigen::Matrix3d icov_; 180 | 181 | /** \brief Eigen vectors of voxel covariance matrix */ 182 | Eigen::Matrix3d evecs_; 183 | 184 | /** \brief Eigen values of voxel covariance matrix */ 185 | Eigen::Vector3d evals_; 186 | 187 | }; 188 | 189 | /** \brief Pointer to VoxelGridCovariance leaf structure */ 190 | typedef Leaf* LeafPtr; 191 | 192 | /** \brief Const pointer to VoxelGridCovariance leaf structure */ 193 | typedef const Leaf* LeafConstPtr; 194 | 195 | typedef std::map Map; 196 | 197 | public: 198 | 199 | /** \brief Constructor. 200 | * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. 201 | */ 202 | VoxelGridCovariance () : 203 | searchable_ (true), 204 | min_points_per_voxel_ (6), 205 | min_covar_eigvalue_mult_ (0.01), 206 | leaves_ (), 207 | voxel_centroids_ (), 208 | voxel_centroids_leaf_indices_ (), 209 | kdtree_ () 210 | { 211 | downsample_all_data_ = false; 212 | save_leaf_layout_ = false; 213 | leaf_size_.setZero (); 214 | min_b_.setZero (); 215 | max_b_.setZero (); 216 | filter_name_ = "VoxelGridCovariance"; 217 | } 218 | 219 | /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation). 220 | * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used 221 | */ 222 | inline void 223 | setMinPointPerVoxel (int min_points_per_voxel) 224 | { 225 | if(min_points_per_voxel > 2) 226 | { 227 | min_points_per_voxel_ = min_points_per_voxel; 228 | } 229 | else 230 | { 231 | PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ()); 232 | min_points_per_voxel_ = 3; 233 | } 234 | } 235 | 236 | /** \brief Get the minimum number of points required for a cell to be used. 237 | * \return the minimum number of points for required for a voxel to be used 238 | */ 239 | inline int 240 | getMinPointPerVoxel () 241 | { 242 | return min_points_per_voxel_; 243 | } 244 | 245 | /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 246 | * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues 247 | */ 248 | inline void 249 | setCovEigValueInflationRatio (double min_covar_eigvalue_mult) 250 | { 251 | min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; 252 | } 253 | 254 | /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 255 | * \return the minimum allowable ratio between eigenvalues 256 | */ 257 | inline double 258 | getCovEigValueInflationRatio () 259 | { 260 | return min_covar_eigvalue_mult_; 261 | } 262 | 263 | /** \brief Filter cloud and initializes voxel structure. 264 | * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 265 | * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 266 | */ 267 | inline void 268 | filter (PointCloud &output, bool searchable = false) 269 | { 270 | searchable_ = searchable; 271 | applyFilter (output); 272 | 273 | voxel_centroids_ = PointCloudPtr (new PointCloud (output)); 274 | 275 | if (searchable_ && voxel_centroids_->size() > 0) 276 | { 277 | // Initiates kdtree of the centroids of voxels containing a sufficient number of points 278 | kdtree_.setInputCloud (voxel_centroids_); 279 | } 280 | } 281 | 282 | /** \brief Initializes voxel structure. 283 | * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 284 | */ 285 | inline void 286 | filter (bool searchable = false) 287 | { 288 | searchable_ = searchable; 289 | voxel_centroids_ = PointCloudPtr (new PointCloud); 290 | applyFilter (*voxel_centroids_); 291 | 292 | if (searchable_ && voxel_centroids_->size() > 0) 293 | { 294 | // Initiates kdtree of the centroids of voxels containing a sufficient number of points 295 | kdtree_.setInputCloud (voxel_centroids_); 296 | } 297 | } 298 | 299 | /** \brief Get the voxel containing point p. 300 | * \param[in] index the index of the leaf structure node 301 | * \return const pointer to leaf structure 302 | */ 303 | inline LeafConstPtr 304 | getLeaf (int index) 305 | { 306 | auto leaf_iter = leaves_.find (index); 307 | if (leaf_iter != leaves_.end ()) 308 | { 309 | LeafConstPtr ret (&(leaf_iter->second)); 310 | return ret; 311 | } 312 | else 313 | return NULL; 314 | } 315 | 316 | /** \brief Get the voxel containing point p. 317 | * \param[in] p the point to get the leaf structure at 318 | * \return const pointer to leaf structure 319 | */ 320 | inline LeafConstPtr 321 | getLeaf (PointT &p) 322 | { 323 | // Generate index associated with p 324 | int ijk0 = static_cast (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]); 325 | int ijk1 = static_cast (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]); 326 | int ijk2 = static_cast (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]); 327 | 328 | // Compute the centroid leaf index 329 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 330 | 331 | // Find leaf associated with index 332 | auto leaf_iter = leaves_.find (idx); 333 | if (leaf_iter != leaves_.end ()) 334 | { 335 | // If such a leaf exists return the pointer to the leaf structure 336 | LeafConstPtr ret (&(leaf_iter->second)); 337 | return ret; 338 | } 339 | else 340 | return NULL; 341 | } 342 | 343 | /** \brief Get the voxel containing point p. 344 | * \param[in] p the point to get the leaf structure at 345 | * \return const pointer to leaf structure 346 | */ 347 | inline LeafConstPtr 348 | getLeaf (Eigen::Vector3f &p) 349 | { 350 | // Generate index associated with p 351 | int ijk0 = static_cast (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]); 352 | int ijk1 = static_cast (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]); 353 | int ijk2 = static_cast (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]); 354 | 355 | // Compute the centroid leaf index 356 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 357 | 358 | // Find leaf associated with index 359 | auto leaf_iter = leaves_.find (idx); 360 | if (leaf_iter != leaves_.end ()) 361 | { 362 | // If such a leaf exists return the pointer to the leaf structure 363 | LeafConstPtr ret (&(leaf_iter->second)); 364 | return ret; 365 | } 366 | else 367 | return NULL; 368 | 369 | } 370 | 371 | /** \brief Get the voxels surrounding point p, not including the voxel contating point p. 372 | * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice). 373 | * \param[in] reference_point the point to get the leaf structure at 374 | * \param[out] neighbors 375 | * \return number of neighbors found 376 | */ 377 | int getNeighborhoodAtPoint(const Eigen::MatrixXi&, const PointT& reference_point, std::vector &neighbors) const ; 378 | int getNeighborhoodAtPoint(const PointT& reference_point, std::vector &neighbors) const ; 379 | int getNeighborhoodAtPoint7(const PointT& reference_point, std::vector &neighbors) const ; 380 | int getNeighborhoodAtPoint1(const PointT& reference_point, std::vector &neighbors) const ; 381 | 382 | /** \brief Get the leaf structure map 383 | * \return a map contataining all leaves 384 | */ 385 | inline const Map& 386 | getLeaves () 387 | { 388 | return leaves_; 389 | } 390 | 391 | /** \brief Get a pointcloud containing the voxel centroids 392 | * \note Only voxels containing a sufficient number of points are used. 393 | * \return a map contataining all leaves 394 | */ 395 | inline PointCloudPtr 396 | getCentroids () 397 | { 398 | return voxel_centroids_; 399 | } 400 | 401 | 402 | /** \brief Get a cloud to visualize each voxels normal distribution. 403 | * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel 404 | */ 405 | void 406 | getDisplayCloud (pcl::PointCloud& cell_cloud); 407 | 408 | /** \brief Search for the k-nearest occupied voxels for the given query point. 409 | * \note Only voxels containing a sufficient number of points are used. 410 | * \param[in] point the given query point 411 | * \param[in] k the number of neighbors to search for 412 | * \param[out] k_leaves the resultant leaves of the neighboring points 413 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 414 | * \return number of neighbors found 415 | */ 416 | int 417 | nearestKSearch (const PointT &point, int k, 418 | std::vector &k_leaves, std::vector &k_sqr_distances) 419 | { 420 | k_leaves.clear (); 421 | 422 | // Check if kdtree has been built 423 | if (!searchable_) 424 | { 425 | PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 426 | return 0; 427 | } 428 | 429 | // Find k-nearest neighbors in the occupied voxel centroid cloud 430 | std::vector k_indices; 431 | k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances); 432 | 433 | // Find leaves corresponding to neighbors 434 | k_leaves.reserve (k); 435 | for (std::vector::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 436 | { 437 | k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]); 438 | } 439 | return k; 440 | } 441 | 442 | /** \brief Search for the k-nearest occupied voxels for the given query point. 443 | * \note Only voxels containing a sufficient number of points are used. 444 | * \param[in] cloud the given query point 445 | * \param[in] index the index 446 | * \param[in] k the number of neighbors to search for 447 | * \param[out] k_leaves the resultant leaves of the neighboring points 448 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 449 | * \return number of neighbors found 450 | */ 451 | inline int 452 | nearestKSearch (const PointCloud &cloud, int index, int k, 453 | std::vector &k_leaves, std::vector &k_sqr_distances) 454 | { 455 | if (index >= static_cast (cloud.points.size ()) || index < 0) 456 | return (0); 457 | return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances)); 458 | } 459 | 460 | 461 | /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 462 | * \note Only voxels containing a sufficient number of points are used. 463 | * \param[in] point the given query point 464 | * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 465 | * \param[out] k_leaves the resultant leaves of the neighboring points 466 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 467 | * \param[in] max_nn 468 | * \return number of neighbors found 469 | */ 470 | int 471 | radiusSearch (const PointT &point, double radius, std::vector &k_leaves, 472 | std::vector &k_sqr_distances, unsigned int max_nn = 0) const 473 | { 474 | k_leaves.clear (); 475 | 476 | // Check if kdtree has been built 477 | if (!searchable_) 478 | { 479 | PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 480 | return 0; 481 | } 482 | 483 | // Find neighbors within radius in the occupied voxel centroid cloud 484 | std::vector k_indices; 485 | int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); 486 | 487 | // Find leaves corresponding to neighbors 488 | k_leaves.reserve (k); 489 | for (std::vector::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 490 | { 491 | auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]); 492 | if (leaf == leaves_.end()) { 493 | std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl; 494 | std::cin.ignore(1); 495 | } 496 | k_leaves.push_back (&(leaf->second)); 497 | } 498 | return k; 499 | } 500 | 501 | /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 502 | * \note Only voxels containing a sufficient number of points are used. 503 | * \param[in] cloud the given query point 504 | * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point 505 | * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 506 | * \param[out] k_leaves the resultant leaves of the neighboring points 507 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 508 | * \param[in] max_nn 509 | * \return number of neighbors found 510 | */ 511 | inline int 512 | radiusSearch (const PointCloud &cloud, int index, double radius, 513 | std::vector &k_leaves, std::vector &k_sqr_distances, 514 | unsigned int max_nn = 0) const 515 | { 516 | if (index >= static_cast (cloud.points.size ()) || index < 0) 517 | return (0); 518 | return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); 519 | } 520 | 521 | protected: 522 | 523 | /** \brief Filter cloud and initializes voxel structure. 524 | * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 525 | */ 526 | void applyFilter (PointCloud &output); 527 | 528 | /** \brief Flag to determine if voxel structure is searchable. */ 529 | bool searchable_; 530 | 531 | /** \brief Minimum points contained with in a voxel to allow it to be useable. */ 532 | int min_points_per_voxel_; 533 | 534 | /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ 535 | double min_covar_eigvalue_mult_; 536 | 537 | /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */ 538 | Map leaves_; 539 | 540 | /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */ 541 | PointCloudPtr voxel_centroids_; 542 | 543 | /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */ 544 | std::vector voxel_centroids_leaf_indices_; 545 | 546 | /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ 547 | pcl::KdTreeFLANN kdtree_; 548 | }; 549 | } 550 | 551 | #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_ 552 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/include/pclomp/voxel_grid_covariance_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ 39 | #define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ 40 | 41 | #include 42 | #include 43 | #include "voxel_grid_covariance_omp.h" 44 | #include 45 | #include 46 | 47 | ////////////////////////////////////////////////////////////////////////////////////////// 48 | template void 49 | pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) 50 | { 51 | voxel_centroids_leaf_indices_.clear (); 52 | 53 | // Has the input dataset been set already? 54 | if (!input_) 55 | { 56 | PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 57 | output.width = output.height = 0; 58 | output.points.clear (); 59 | return; 60 | } 61 | 62 | // Copy the header (and thus the frame_id) + allocate enough space for points 63 | output.height = 1; // downsampling breaks the organized structure 64 | output.is_dense = true; // we filter out invalid points 65 | output.points.clear (); 66 | 67 | Eigen::Vector4f min_p, max_p; 68 | // Get the minimum and maximum dimensions 69 | if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... 70 | pcl::getMinMax3D (input_, filter_field_name_, static_cast (filter_limit_min_), static_cast (filter_limit_max_), min_p, max_p, filter_limit_negative_); 71 | else 72 | pcl::getMinMax3D (*input_, min_p, max_p); 73 | 74 | // Check that the leaf size is not too small, given the size of the data 75 | int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; 76 | int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; 77 | int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; 78 | 79 | if((dx*dy*dz) > std::numeric_limits::max()) 80 | { 81 | PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); 82 | output.clear(); 83 | return; 84 | } 85 | 86 | // Compute the minimum and maximum bounding box values 87 | min_b_[0] = static_cast (floor (min_p[0] * inverse_leaf_size_[0])); 88 | max_b_[0] = static_cast (floor (max_p[0] * inverse_leaf_size_[0])); 89 | min_b_[1] = static_cast (floor (min_p[1] * inverse_leaf_size_[1])); 90 | max_b_[1] = static_cast (floor (max_p[1] * inverse_leaf_size_[1])); 91 | min_b_[2] = static_cast (floor (min_p[2] * inverse_leaf_size_[2])); 92 | max_b_[2] = static_cast (floor (max_p[2] * inverse_leaf_size_[2])); 93 | 94 | // Compute the number of divisions needed along all axis 95 | div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); 96 | div_b_[3] = 0; 97 | 98 | // Clear the leaves 99 | leaves_.clear (); 100 | // leaves_.reserve(8192); 101 | 102 | // Set up the division multiplier 103 | divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); 104 | 105 | int centroid_size = 4; 106 | 107 | if (downsample_all_data_) 108 | centroid_size = boost::mpl::size::value; 109 | 110 | // ---[ RGB special case 111 | std::vector fields; 112 | int rgba_index = -1; 113 | rgba_index = pcl::getFieldIndex ("rgb", fields); 114 | if (rgba_index == -1) 115 | rgba_index = pcl::getFieldIndex ("rgba", fields); 116 | if (rgba_index >= 0) 117 | { 118 | rgba_index = fields[rgba_index].offset; 119 | centroid_size += 3; 120 | } 121 | 122 | // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... 123 | if (!filter_field_name_.empty ()) 124 | { 125 | // Get the distance field index 126 | std::vector fields; 127 | int distance_idx = pcl::getFieldIndex (filter_field_name_, fields); 128 | if (distance_idx == -1) 129 | PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); 130 | 131 | // First pass: go over all points and insert them into the right leaf 132 | for (size_t cp = 0; cp < input_->points.size (); ++cp) 133 | { 134 | if (!input_->is_dense) 135 | // Check if the point is invalid 136 | if (!std::isfinite (input_->points[cp].x) || 137 | !std::isfinite (input_->points[cp].y) || 138 | !std::isfinite (input_->points[cp].z)) 139 | continue; 140 | 141 | // Get the distance value 142 | const uint8_t* pt_data = reinterpret_cast (&input_->points[cp]); 143 | float distance_value = 0; 144 | memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 145 | 146 | if (filter_limit_negative_) 147 | { 148 | // Use a threshold for cutting out points which inside the interval 149 | if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) 150 | continue; 151 | } 152 | else 153 | { 154 | // Use a threshold for cutting out points which are too close/far away 155 | if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) 156 | continue; 157 | } 158 | 159 | int ijk0 = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); 160 | int ijk1 = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); 161 | int ijk2 = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); 162 | 163 | // Compute the centroid leaf index 164 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 165 | 166 | Leaf& leaf = leaves_[idx]; 167 | if (leaf.nr_points == 0) 168 | { 169 | leaf.centroid.resize (centroid_size); 170 | leaf.centroid.setZero (); 171 | } 172 | 173 | Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 174 | // Accumulate point sum for centroid calculation 175 | leaf.mean_ += pt3d; 176 | // Accumulate x*xT for single pass covariance calculation 177 | leaf.cov_ += pt3d * pt3d.transpose (); 178 | 179 | // Do we need to process all the fields? 180 | if (!downsample_all_data_) 181 | { 182 | Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 183 | leaf.centroid.template head<4> () += pt; 184 | } 185 | else 186 | { 187 | // Copy all the fields 188 | Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 189 | // ---[ RGB special case 190 | if (rgba_index >= 0) 191 | { 192 | // fill r/g/b data 193 | int rgb; 194 | memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); 195 | centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); 196 | centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); 197 | centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); 198 | } 199 | pcl::for_each_type (pcl::NdCopyPointEigenFunctor (input_->points[cp], centroid)); 200 | leaf.centroid += centroid; 201 | } 202 | ++leaf.nr_points; 203 | } 204 | } 205 | // No distance filtering, process all data 206 | else 207 | { 208 | // First pass: go over all points and insert them into the right leaf 209 | for (size_t cp = 0; cp < input_->points.size (); ++cp) 210 | { 211 | if (!input_->is_dense) 212 | // Check if the point is invalid 213 | if (!std::isfinite (input_->points[cp].x) || 214 | !std::isfinite (input_->points[cp].y) || 215 | !std::isfinite (input_->points[cp].z)) 216 | continue; 217 | 218 | int ijk0 = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); 219 | int ijk1 = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); 220 | int ijk2 = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); 221 | 222 | // Compute the centroid leaf index 223 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 224 | 225 | //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast ()).matrix () - min_b_).dot (divb_mul_); 226 | Leaf& leaf = leaves_[idx]; 227 | if (leaf.nr_points == 0) 228 | { 229 | leaf.centroid.resize (centroid_size); 230 | leaf.centroid.setZero (); 231 | } 232 | 233 | Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 234 | // Accumulate point sum for centroid calculation 235 | leaf.mean_ += pt3d; 236 | // Accumulate x*xT for single pass covariance calculation 237 | leaf.cov_ += pt3d * pt3d.transpose (); 238 | 239 | // Do we need to process all the fields? 240 | if (!downsample_all_data_) 241 | { 242 | Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 243 | leaf.centroid.template head<4> () += pt; 244 | } 245 | else 246 | { 247 | // Copy all the fields 248 | Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 249 | // ---[ RGB special case 250 | if (rgba_index >= 0) 251 | { 252 | // Fill r/g/b data, assuming that the order is BGRA 253 | int rgb; 254 | memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); 255 | centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); 256 | centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); 257 | centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); 258 | } 259 | pcl::for_each_type (pcl::NdCopyPointEigenFunctor (input_->points[cp], centroid)); 260 | leaf.centroid += centroid; 261 | } 262 | ++leaf.nr_points; 263 | } 264 | } 265 | 266 | // Second pass: go over all leaves and compute centroids and covariance matrices 267 | output.points.reserve (leaves_.size ()); 268 | if (searchable_) 269 | voxel_centroids_leaf_indices_.reserve (leaves_.size ()); 270 | int cp = 0; 271 | if (save_leaf_layout_) 272 | leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1); 273 | 274 | // Eigen values and vectors calculated to prevent near singluar matrices 275 | Eigen::SelfAdjointEigenSolver eigensolver; 276 | Eigen::Matrix3d eigen_val; 277 | Eigen::Vector3d pt_sum; 278 | 279 | // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value. 280 | double min_covar_eigvalue; 281 | 282 | for (auto it = leaves_.begin (); it != leaves_.end (); ++it) 283 | { 284 | 285 | // Normalize the centroid 286 | Leaf& leaf = it->second; 287 | 288 | // Normalize the centroid 289 | leaf.centroid /= static_cast (leaf.nr_points); 290 | // Point sum used for single pass covariance calculation 291 | pt_sum = leaf.mean_; 292 | // Normalize mean 293 | leaf.mean_ /= leaf.nr_points; 294 | 295 | // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds. 296 | // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution. 297 | if (leaf.nr_points >= min_points_per_voxel_) 298 | { 299 | if (save_leaf_layout_) 300 | leaf_layout_[it->first] = cp++; 301 | 302 | output.push_back (PointT ()); 303 | 304 | // Do we need to process all the fields? 305 | if (!downsample_all_data_) 306 | { 307 | output.points.back ().x = leaf.centroid[0]; 308 | output.points.back ().y = leaf.centroid[1]; 309 | output.points.back ().z = leaf.centroid[2]; 310 | } 311 | else 312 | { 313 | pcl::for_each_type (pcl::NdCopyEigenPointFunctor (leaf.centroid, output.back ())); 314 | // ---[ RGB special case 315 | if (rgba_index >= 0) 316 | { 317 | // pack r/g/b into rgb 318 | float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1]; 319 | int rgb = (static_cast (r)) << 16 | (static_cast (g)) << 8 | (static_cast (b)); 320 | memcpy (reinterpret_cast (&output.points.back ()) + rgba_index, &rgb, sizeof (float)); 321 | } 322 | } 323 | 324 | // Stores the voxel indice for fast access searching 325 | if (searchable_) 326 | voxel_centroids_leaf_indices_.push_back (static_cast (it->first)); 327 | 328 | // Single pass covariance calculation 329 | leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose (); 330 | leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; 331 | 332 | //Normalize Eigen Val such that max no more than 100x min. 333 | eigensolver.compute (leaf.cov_); 334 | eigen_val = eigensolver.eigenvalues ().asDiagonal (); 335 | leaf.evecs_ = eigensolver.eigenvectors (); 336 | 337 | if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0) 338 | { 339 | leaf.nr_points = -1; 340 | continue; 341 | } 342 | 343 | // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] 344 | 345 | min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2); 346 | if (eigen_val (0, 0) < min_covar_eigvalue) 347 | { 348 | eigen_val (0, 0) = min_covar_eigvalue; 349 | 350 | if (eigen_val (1, 1) < min_covar_eigvalue) 351 | { 352 | eigen_val (1, 1) = min_covar_eigvalue; 353 | } 354 | 355 | leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse (); 356 | } 357 | leaf.evals_ = eigen_val.diagonal (); 358 | 359 | leaf.icov_ = leaf.cov_.inverse (); 360 | if (leaf.icov_.maxCoeff () == std::numeric_limits::infinity ( ) 361 | || leaf.icov_.minCoeff () == -std::numeric_limits::infinity ( ) ) 362 | { 363 | leaf.nr_points = -1; 364 | } 365 | 366 | } 367 | } 368 | 369 | output.width = static_cast (output.points.size ()); 370 | } 371 | 372 | ////////////////////////////////////////////////////////////////////////////////////////// 373 | template int 374 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector &neighbors) const 375 | { 376 | neighbors.clear(); 377 | 378 | // Find displacement coordinates 379 | Eigen::Vector4i ijk(static_cast (floor(reference_point.x / leaf_size_[0])), 380 | static_cast (floor(reference_point.y / leaf_size_[1])), 381 | static_cast (floor(reference_point.z / leaf_size_[2])), 0); 382 | Eigen::Array4i diff2min = min_b_ - ijk; 383 | Eigen::Array4i diff2max = max_b_ - ijk; 384 | neighbors.reserve(relative_coordinates.cols()); 385 | 386 | // Check each neighbor to see if it is occupied and contains sufficient points 387 | // Slower than radius search because needs to check 26 indices 388 | for (int ni = 0; ni < relative_coordinates.cols(); ni++) 389 | { 390 | Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 391 | // Checking if the specified cell is in the grid 392 | if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 393 | { 394 | auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_))); 395 | if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_) 396 | { 397 | LeafConstPtr leaf = &(leaf_iter->second); 398 | neighbors.push_back(leaf); 399 | } 400 | } 401 | } 402 | 403 | return (static_cast (neighbors.size())); 404 | } 405 | 406 | ////////////////////////////////////////////////////////////////////////////////////////// 407 | template int 408 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const PointT& reference_point, std::vector &neighbors) const 409 | { 410 | neighbors.clear(); 411 | 412 | // Find displacement coordinates 413 | Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices(); 414 | return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); 415 | } 416 | 417 | ////////////////////////////////////////////////////////////////////////////////////////// 418 | template int 419 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector &neighbors) const 420 | { 421 | neighbors.clear(); 422 | 423 | Eigen::MatrixXi relative_coordinates(3, 7); 424 | relative_coordinates.setZero(); 425 | relative_coordinates(0, 1) = 1; 426 | relative_coordinates(0, 2) = -1; 427 | relative_coordinates(1, 3) = 1; 428 | relative_coordinates(1, 4) = -1; 429 | relative_coordinates(2, 5) = 1; 430 | relative_coordinates(2, 6) = -1; 431 | 432 | return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); 433 | } 434 | 435 | 436 | ////////////////////////////////////////////////////////////////////////////////////////// 437 | template int 438 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector &neighbors) const 439 | { 440 | neighbors.clear(); 441 | return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors); 442 | } 443 | 444 | 445 | 446 | ////////////////////////////////////////////////////////////////////////////////////////// 447 | template void 448 | pclomp::VoxelGridCovariance::getDisplayCloud (pcl::PointCloud& cell_cloud) 449 | { 450 | cell_cloud.clear (); 451 | 452 | int pnt_per_cell = 1000; 453 | boost::mt19937 rng; 454 | boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ()); 455 | boost::variate_generator > var_nor (rng, nd); 456 | 457 | Eigen::LLT llt_of_cov; 458 | Eigen::Matrix3d cholesky_decomp; 459 | Eigen::Vector3d cell_mean; 460 | Eigen::Vector3d rand_point; 461 | Eigen::Vector3d dist_point; 462 | 463 | // Generate points for each occupied voxel with sufficient points. 464 | for (auto it = leaves_.begin (); it != leaves_.end (); ++it) 465 | { 466 | Leaf& leaf = it->second; 467 | 468 | if (leaf.nr_points >= min_points_per_voxel_) 469 | { 470 | cell_mean = leaf.mean_; 471 | llt_of_cov.compute (leaf.cov_); 472 | cholesky_decomp = llt_of_cov.matrixL (); 473 | 474 | // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix 475 | for (int i = 0; i < pnt_per_cell; i++) 476 | { 477 | rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ()); 478 | dist_point = cell_mean + cholesky_decomp * rand_point; 479 | cell_cloud.push_back (pcl::PointXYZ (static_cast (dist_point (0)), static_cast (dist_point (1)), static_cast (dist_point (2)))); 480 | } 481 | } 482 | } 483 | } 484 | 485 | #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance; 486 | 487 | #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 488 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ndt_omp_ros2 4 | 0.0.0 5 | OpenMP boosted NDT and GICP algorithms 6 | 7 | koide 8 | 9 | BSD 10 | 11 | ament_cmake 12 | rclcpp 13 | libpcl-all-dev 14 | std_msgs 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/src/pclomp/gicp_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::GeneralizedIterativeClosestPoint; 5 | template class pclomp::GeneralizedIterativeClosestPoint; 6 | 7 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/src/pclomp/ndt_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::NormalDistributionsTransform; 5 | template class pclomp::NormalDistributionsTransform; 6 | -------------------------------------------------------------------------------- /Thirdparty/ndt_omp_ros2/src/pclomp/voxel_grid_covariance_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::VoxelGridCovariance; 5 | template class pclomp::VoxelGridCovariance; 6 | -------------------------------------------------------------------------------- /graph_based_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(graph_based_slam) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | #add_compile_options(-Wall -Wextra -Wpedantic) 16 | #add_compile_options(-Wextra -Wpedantic) 17 | endif() 18 | 19 | SET(CMAKE_CXX_FLAGS "-O2 -g ${CMAKE_CXX_FLAGS}") 20 | add_compile_definitions(G2O_USE_VENDORED_CERES) 21 | 22 | # find dependencies 23 | find_package(ament_cmake REQUIRED) 24 | find_package(rclcpp REQUIRED) 25 | find_package(rclcpp_components REQUIRED) 26 | find_package(geometry_msgs REQUIRED) 27 | find_package(sensor_msgs REQUIRED) 28 | find_package(nav_msgs REQUIRED) 29 | find_package(std_srvs REQUIRED) 30 | find_package(tf2_ros REQUIRED) 31 | find_package(tf2_eigen REQUIRED) 32 | find_package(tf2_geometry_msgs REQUIRED) 33 | find_package(tf2_sensor_msgs REQUIRED) 34 | find_package(pcl_conversions REQUIRED) 35 | find_package(lidarslam_msgs REQUIRED) 36 | find_package(ndt_omp_ros2 REQUIRED) 37 | find_package(PCL REQUIRED) 38 | find_package(g2o REQUIRED 39 | COMPONENTS 40 | g2o::core 41 | g2o::types_slam3d 42 | g2o::solver_eigen 43 | ) 44 | find_package(OpenMP) 45 | 46 | if (OPENMP_FOUND) 47 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 48 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 49 | endif() 50 | 51 | if(BUILD_TESTING) 52 | find_package(ament_lint_auto REQUIRED) 53 | ament_lint_auto_find_test_dependencies() 54 | endif() 55 | 56 | add_library(graph_based_slam_component SHARED 57 | src/graph_based_slam_component.cpp 58 | ) 59 | 60 | target_compile_definitions(graph_based_slam_component PRIVATE "GS_GBS_BUILDING_DLL") 61 | 62 | ament_target_dependencies(graph_based_slam_component 63 | rclcpp 64 | rclcpp_components 65 | tf2_ros 66 | tf2_eigen 67 | tf2_geometry_msgs 68 | tf2_sensor_msgs 69 | pcl_conversions 70 | geometry_msgs 71 | sensor_msgs 72 | nav_msgs 73 | std_srvs 74 | lidarslam_msgs 75 | ndt_omp_ros2 76 | ) 77 | 78 | add_executable(graph_based_slam_node 79 | src/graph_based_slam_node.cpp 80 | ) 81 | 82 | target_link_libraries(graph_based_slam_node 83 | graph_based_slam_component 84 | ${PCL_LIBRARIES} 85 | g2o::core 86 | g2o::types_slam3d 87 | g2o::solver_eigen 88 | ) 89 | 90 | ament_target_dependencies(graph_based_slam_node 91 | rclcpp) 92 | 93 | include_directories( 94 | include 95 | ${PCL_INCLUDE_DIRS} 96 | ) 97 | link_directories( 98 | ${PCL_LIBRARY_DIRS} 99 | ) 100 | add_definitions(${PCL_DEFINITIONS}) 101 | 102 | rclcpp_components_register_nodes(graph_based_slam_component 103 | "graphslam::GraphBasedSlamComponent") 104 | 105 | ament_export_libraries(graph_based_slam_component) 106 | 107 | install( 108 | DIRECTORY "include/" 109 | DESTINATION include 110 | ) 111 | 112 | install( 113 | DIRECTORY launch param 114 | DESTINATION share/${PROJECT_NAME} 115 | ) 116 | 117 | install(TARGETS 118 | graph_based_slam_component 119 | ARCHIVE DESTINATION lib 120 | LIBRARY DESTINATION lib 121 | RUNTIME DESTINATION bin 122 | ) 123 | 124 | install(TARGETS 125 | graph_based_slam_node 126 | DESTINATION lib/${PROJECT_NAME} 127 | ) 128 | 129 | ament_export_include_directories(include) 130 | ament_package() 131 | -------------------------------------------------------------------------------- /graph_based_slam/include/graph_based_slam/graph_based_slam_component.h: -------------------------------------------------------------------------------- 1 | #ifndef GS_GBS_COMPONENT_H_INCLUDED 2 | #define GS_GBS_COMPONENT_H_INCLUDED 3 | 4 | #if __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | // The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from 9 | // demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos 10 | #if defined _WIN32 || defined __CYGWIN__ 11 | #ifdef __GNUC__ 12 | #define GS_GBS_EXPORT __attribute__ ((dllexport)) 13 | #define GS_GBS_IMPORT __attribute__ ((dllimport)) 14 | #else 15 | #define GS_GBS_EXPORT __declspec(dllexport) 16 | #define GS_GBS_IMPORT __declspec(dllimport) 17 | #endif 18 | #ifdef GS_GBS_BUILDING_DLL 19 | #define GS_GBS_PUBLIC GS_GBS_EXPORT 20 | #else 21 | #define GS_GBS_PUBLIC GS_GBS_IMPORT 22 | #endif 23 | #define GS_GBS_PUBLIC_TYPE GS_GBS_PUBLIC 24 | #define GS_GBS_LOCAL 25 | #else 26 | #define GS_GBS_EXPORT __attribute__ ((visibility("default"))) 27 | #define GS_GBS_IMPORT 28 | #if __GNUC__ >= 4 29 | #define GS_GBS_PUBLIC __attribute__ ((visibility("default"))) 30 | #define GS_GBS_LOCAL __attribute__ ((visibility("hidden"))) 31 | #else 32 | #define GS_GBS_PUBLIC 33 | #define GS_GBS_LOCAL 34 | #endif 35 | #define GS_GBS_PUBLIC_TYPE 36 | #endif 37 | 38 | #if __cplusplus 39 | } // extern "C" 40 | #endif 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | #include 60 | 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | 75 | #include "g2o/core/sparse_optimizer.h" 76 | #include "g2o/core/optimization_algorithm_levenberg.h" 77 | #include "g2o/core/block_solver.h" 78 | 79 | #include "g2o/solvers/eigen/linear_solver_eigen.h" 80 | 81 | #include "g2o/types/slam3d/vertex_se3.h" 82 | #include "g2o/types/slam3d/vertex_pointxyz.h" 83 | #include "g2o/types/slam3d/edge_se3.h" 84 | #include "g2o/types/slam3d/edge_se3_pointxyz.h" 85 | #include "g2o/types/slam3d/se3quat.h" 86 | #include "g2o/types/slam3d/parameter_se3_offset.h" 87 | 88 | #include 89 | 90 | namespace graphslam 91 | { 92 | class GraphBasedSlamComponent: public rclcpp::Node 93 | { 94 | public: 95 | GS_GBS_PUBLIC 96 | explicit GraphBasedSlamComponent(const rclcpp::NodeOptions & options); 97 | 98 | private: 99 | std::mutex mtx_; 100 | 101 | rclcpp::Clock clock_; 102 | tf2_ros::Buffer tfbuffer_; 103 | tf2_ros::TransformListener listener_; 104 | tf2_ros::TransformBroadcaster broadcaster_; 105 | 106 | boost::shared_ptr> registration_; 107 | pcl::VoxelGrid < pcl::PointXYZI > voxelgrid_; 108 | 109 | lidarslam_msgs::msg::MapArray map_array_msg_; 110 | rclcpp::Subscription < lidarslam_msgs::msg::MapArray > ::SharedPtr map_array_sub_; 111 | rclcpp::Publisher < lidarslam_msgs::msg::MapArray > ::SharedPtr modified_map_array_pub_; 112 | rclcpp::Publisher < nav_msgs::msg::Path > ::SharedPtr modified_path_pub_; 113 | rclcpp::Publisher < sensor_msgs::msg::PointCloud2 > ::SharedPtr modified_map_pub_; 114 | rclcpp::TimerBase::SharedPtr loop_detect_timer_; 115 | rclcpp::Service < std_srvs::srv::Empty > ::SharedPtr map_save_srv_; 116 | 117 | void initializePubSub(); 118 | void searchLoop(); 119 | void doPoseAdjustment(lidarslam_msgs::msg::MapArray map_array_msg, bool do_save_map); 120 | void publishMapAndPose(); 121 | 122 | // loop search parameter 123 | int loop_detection_period_; 124 | double threshold_loop_closure_score_; 125 | double distance_loop_closure_; 126 | double range_of_searching_loop_closure_; 127 | int search_submap_num_; 128 | 129 | // pose graph optimization parameter 130 | int num_adjacent_pose_cnstraints_; 131 | bool use_save_map_in_loop_ {true}; 132 | 133 | bool initial_map_array_received_ {false}; 134 | bool is_map_array_updated_ {false}; 135 | int previous_submaps_num_ {0}; 136 | 137 | struct LoopEdge 138 | { 139 | std::pair < int, int > pair_id; 140 | Eigen::Isometry3d relative_pose; 141 | }; 142 | std::vector < LoopEdge > loop_edges_; 143 | 144 | }; 145 | } // namespace graphslam 146 | 147 | #endif //GS_GBS_COMPONENT_H_INCLUDED 148 | -------------------------------------------------------------------------------- /graph_based_slam/launch/graphbasedslam.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import launch 4 | import launch_ros.actions 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | def generate_launch_description(): 9 | 10 | graphbasedslam_param_dir = launch.substitutions.LaunchConfiguration( 11 | 'graphbasedslam_param_dir', 12 | default=os.path.join( 13 | get_package_share_directory('graph_based_slam'), 14 | 'param', 15 | 'graphbasedslam.yaml')) 16 | 17 | graphbasedslam = launch_ros.actions.Node( 18 | package='graph_based_slam', 19 | executable='graph_based_slam_node', 20 | parameters=[graphbasedslam_param_dir], 21 | output='screen' 22 | ) 23 | 24 | 25 | return launch.LaunchDescription([ 26 | launch.actions.DeclareLaunchArgument( 27 | 'graphbasedslam_param_dir', 28 | default_value=graphbasedslam_param_dir, 29 | description='Full path to graphbasedslam parameter file to load'), 30 | graphbasedslam, 31 | ]) -------------------------------------------------------------------------------- /graph_based_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | graph_based_slam 5 | 0.0.0 6 | ros2 package of graph based slam 7 | sasaki 8 | BSD2 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_components 14 | geometry_msgs 15 | sensor_msgs 16 | tf2_ros 17 | tf2_sensor_msgs 18 | tf2_geometry_msgs 19 | tf2_eigen 20 | pcl_conversions 21 | lidarslam_msgs 22 | ndt_omp_ros2 23 | libg2o 24 | libpcl-all-dev 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /graph_based_slam/param/graphbasedslam.yaml: -------------------------------------------------------------------------------- 1 | graph_based_slam: 2 | ros__parameters: 3 | ndt_resolution: 1.5 4 | voxel_leaf_size: 0.2 5 | loop_detection_period: 3000 6 | threshold_loop_closure_score: 1.5 7 | distance_loop_closure: 30.0 8 | -------------------------------------------------------------------------------- /graph_based_slam/src/graph_based_slam_component.cpp: -------------------------------------------------------------------------------- 1 | #include "graph_based_slam/graph_based_slam_component.h" 2 | #include 3 | 4 | using namespace std::chrono_literals; 5 | 6 | namespace graphslam 7 | { 8 | GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & options) 9 | : Node("graph_based_slam", options), 10 | clock_(RCL_ROS_TIME), 11 | tfbuffer_(std::make_shared(clock_)), 12 | listener_(tfbuffer_), 13 | broadcaster_(this) 14 | { 15 | RCLCPP_INFO(get_logger(), "initialization start"); 16 | std::string registration_method; 17 | double voxel_leaf_size; 18 | double ndt_resolution; 19 | int ndt_num_threads; 20 | 21 | declare_parameter("registration_method", "NDT"); 22 | get_parameter("registration_method", registration_method); 23 | declare_parameter("voxel_leaf_size", 0.2); 24 | get_parameter("voxel_leaf_size", voxel_leaf_size); 25 | declare_parameter("ndt_resolution", 5.0); 26 | get_parameter("ndt_resolution", ndt_resolution); 27 | declare_parameter("ndt_num_threads", 0); 28 | get_parameter("ndt_num_threads", ndt_num_threads); 29 | declare_parameter("loop_detection_period", 1000); 30 | get_parameter("loop_detection_period", loop_detection_period_); 31 | declare_parameter("threshold_loop_closure_score", 1.0); 32 | get_parameter("threshold_loop_closure_score", threshold_loop_closure_score_); 33 | declare_parameter("distance_loop_closure", 20.0); 34 | get_parameter("distance_loop_closure", distance_loop_closure_); 35 | declare_parameter("range_of_searching_loop_closure", 20.0); 36 | get_parameter("range_of_searching_loop_closure", range_of_searching_loop_closure_); 37 | declare_parameter("search_submap_num", 3); 38 | get_parameter("search_submap_num", search_submap_num_); 39 | declare_parameter("num_adjacent_pose_cnstraints", 5); 40 | get_parameter("num_adjacent_pose_cnstraints", num_adjacent_pose_cnstraints_); 41 | declare_parameter("use_save_map_in_loop", true); 42 | get_parameter("use_save_map_in_loop", use_save_map_in_loop_); 43 | 44 | std::cout << "registration_method:" << registration_method << std::endl; 45 | std::cout << "voxel_leaf_size[m]:" << voxel_leaf_size << std::endl; 46 | std::cout << "ndt_resolution[m]:" << ndt_resolution << std::endl; 47 | std::cout << "ndt_num_threads:" << ndt_num_threads << std::endl; 48 | std::cout << "loop_detection_period[Hz]:" << loop_detection_period_ << std::endl; 49 | std::cout << "threshold_loop_closure_score:" << threshold_loop_closure_score_ << std::endl; 50 | std::cout << "distance_loop_closure[m]:" << distance_loop_closure_ << std::endl; 51 | std::cout << "range_of_searching_loop_closure[m]:" << range_of_searching_loop_closure_ << 52 | std::endl; 53 | std::cout << "search_submap_num:" << search_submap_num_ << std::endl; 54 | std::cout << "num_adjacent_pose_cnstraints:" << num_adjacent_pose_cnstraints_ << std::endl; 55 | std::cout << "use_save_map_in_loop:" << std::boolalpha << use_save_map_in_loop_ << std::endl; 56 | std::cout << "------------------" << std::endl; 57 | 58 | voxelgrid_.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); 59 | 60 | if (registration_method == "NDT") { 61 | boost::shared_ptr> 62 | ndt(new pclomp::NormalDistributionsTransform()); 63 | ndt->setMaximumIterations(100); 64 | ndt->setResolution(ndt_resolution); 65 | // ndt->setTransformationEpsilon(0.01); 66 | ndt->setTransformationEpsilon(1e-6); 67 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); 68 | if (ndt_num_threads > 0) {ndt->setNumThreads(ndt_num_threads);} 69 | registration_ = ndt; 70 | } else if (registration_method == "GICP") { 71 | boost::shared_ptr> 72 | gicp(new pclomp::GeneralizedIterativeClosestPoint()); 73 | gicp->setMaxCorrespondenceDistance(30); 74 | gicp->setMaximumIterations(100); 75 | //gicp->setCorrespondenceRandomness(20); 76 | gicp->setTransformationEpsilon(1e-8); 77 | gicp->setEuclideanFitnessEpsilon(1e-6); 78 | gicp->setRANSACIterations(0); 79 | registration_ = gicp; 80 | } else { 81 | RCLCPP_ERROR(get_logger(), "invalid registration method"); 82 | exit(1); 83 | } 84 | 85 | initializePubSub(); 86 | 87 | auto map_save_callback = 88 | [this](const std::shared_ptr request_header, 89 | const std::shared_ptr request, 90 | const std::shared_ptr response) -> void 91 | { 92 | std::cout << "Received an request to save the map" << std::endl; 93 | if (initial_map_array_received_ == false) { 94 | std::cout << "initial map is not received" << std::endl; 95 | return; 96 | } 97 | doPoseAdjustment(map_array_msg_, true); 98 | }; 99 | 100 | map_save_srv_ = create_service("map_save", map_save_callback); 101 | 102 | } 103 | 104 | void GraphBasedSlamComponent::initializePubSub() 105 | { 106 | RCLCPP_INFO(get_logger(), "initialize Publishers and Subscribers"); 107 | 108 | auto map_array_callback = 109 | [this](const typename lidarslam_msgs::msg::MapArray::SharedPtr msg_ptr) -> void 110 | { 111 | std::lock_guard lock(mtx_); 112 | map_array_msg_ = *msg_ptr; 113 | initial_map_array_received_ = true; 114 | is_map_array_updated_ = true; 115 | }; 116 | 117 | map_array_sub_ = 118 | create_subscription( 119 | "map_array", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), map_array_callback); 120 | 121 | std::chrono::milliseconds period(loop_detection_period_); 122 | loop_detect_timer_ = create_wall_timer( 123 | std::chrono::duration_cast(period), 124 | std::bind(&GraphBasedSlamComponent::searchLoop, this) 125 | ); 126 | 127 | modified_map_pub_ = create_publisher( 128 | "modified_map", 129 | rclcpp::QoS(10)); 130 | 131 | modified_map_array_pub_ = create_publisher( 132 | "modified_map_array", rclcpp::QoS(10)); 133 | 134 | modified_path_pub_ = create_publisher( 135 | "modified_path", 136 | rclcpp::QoS(10)); 137 | 138 | RCLCPP_INFO(get_logger(), "initialization end"); 139 | 140 | } 141 | 142 | void GraphBasedSlamComponent::searchLoop() 143 | { 144 | 145 | if (initial_map_array_received_ == false) {return;} 146 | if (is_map_array_updated_ == false) {return;} 147 | if (map_array_msg_.cloud_coordinate != map_array_msg_.LOCAL) { 148 | RCLCPP_WARN(get_logger(), "cloud_coordinate should be local, but it's not local."); 149 | } 150 | is_map_array_updated_ = false; 151 | 152 | lidarslam_msgs::msg::MapArray map_array_msg = map_array_msg_; 153 | std::lock_guard lock(mtx_); 154 | int num_submaps = map_array_msg.submaps.size(); 155 | RCLCPP_INFO(get_logger(), "searching Loop, num_submaps:%d", num_submaps); 156 | 157 | double min_fitness_score = std::numeric_limits::max(); 158 | double distance_min_fitness_score = 0; 159 | bool is_candidate = false; 160 | 161 | lidarslam_msgs::msg::SubMap latest_submap; 162 | latest_submap = map_array_msg.submaps[num_submaps - 1]; 163 | Eigen::Affine3d latest_submap_affine; 164 | tf2::fromMsg(latest_submap.pose, latest_submap_affine); 165 | pcl::PointCloud::Ptr latest_submap_cloud_ptr(new pcl::PointCloud); 166 | pcl::fromROSMsg(latest_submap.cloud, *latest_submap_cloud_ptr); 167 | pcl::PointCloud::Ptr transformed_latest_submap_cloud_ptr(new pcl::PointCloud); 168 | Eigen::Affine3d latest_affine; 169 | tf2::fromMsg(latest_submap.pose, latest_affine); 170 | pcl::transformPointCloud(*latest_submap_cloud_ptr, *transformed_latest_submap_cloud_ptr, latest_affine.matrix().cast()); 171 | registration_->setInputSource(transformed_latest_submap_cloud_ptr); 172 | double latest_moving_distance = latest_submap.distance; 173 | Eigen::Vector3d latest_submap_pos{ 174 | latest_submap.pose.position.x, 175 | latest_submap.pose.position.y, 176 | latest_submap.pose.position.z}; 177 | int id_min = 0; 178 | double min_dist = std::numeric_limits::max(); 179 | lidarslam_msgs::msg::SubMap min_submap; 180 | for (int i = 0; i < num_submaps; i++) { 181 | auto submap = map_array_msg.submaps[i]; 182 | Eigen::Vector3d submap_pos{submap.pose.position.x, submap.pose.position.y, 183 | submap.pose.position.z}; 184 | double dist = (latest_submap_pos - submap_pos).norm(); 185 | if (latest_moving_distance - submap.distance > distance_loop_closure_ && 186 | dist < range_of_searching_loop_closure_) 187 | { 188 | is_candidate = true; 189 | if(dist < min_dist) { 190 | id_min = i; 191 | min_dist = dist; 192 | min_submap = submap; 193 | } 194 | } 195 | } 196 | 197 | if (is_candidate) { 198 | pcl::PointCloud::Ptr submap_clouds_ptr(new pcl::PointCloud); 199 | for (int j = 0; j <= 2 * search_submap_num_; ++j) { 200 | if (id_min + j - search_submap_num_ < 0) {continue;} 201 | auto near_submap = map_array_msg.submaps[id_min + j - search_submap_num_]; 202 | pcl::PointCloud::Ptr submap_cloud_ptr(new pcl::PointCloud); 203 | pcl::fromROSMsg(near_submap.cloud, *submap_cloud_ptr); 204 | pcl::PointCloud::Ptr transformed_submap_cloud_ptr(new pcl::PointCloud); 205 | Eigen::Affine3d affine; 206 | tf2::fromMsg(near_submap.pose, affine); 207 | pcl::transformPointCloud(*submap_cloud_ptr, *transformed_submap_cloud_ptr, affine.matrix().cast()); 208 | *submap_clouds_ptr += *transformed_submap_cloud_ptr; 209 | } 210 | 211 | pcl::PointCloud::Ptr filtered_clouds_ptr(new pcl::PointCloud()); 212 | voxelgrid_.setInputCloud(submap_clouds_ptr); 213 | voxelgrid_.filter(*filtered_clouds_ptr); 214 | registration_->setInputTarget(filtered_clouds_ptr); 215 | 216 | pcl::PointCloud::Ptr output_cloud_ptr(new pcl::PointCloud); 217 | registration_->align(*output_cloud_ptr); 218 | double fitness_score = registration_->getFitnessScore(); 219 | 220 | if (fitness_score < threshold_loop_closure_score_) { 221 | 222 | Eigen::Affine3d init_affine; 223 | tf2::fromMsg(latest_submap.pose, init_affine); 224 | Eigen::Affine3d submap_affine; 225 | tf2::fromMsg(min_submap.pose, submap_affine); 226 | 227 | LoopEdge loop_edge; 228 | loop_edge.pair_id = std::pair(id_min, num_submaps - 1); 229 | Eigen::Isometry3d from = Eigen::Isometry3d(submap_affine.matrix()); 230 | Eigen::Isometry3d to = Eigen::Isometry3d( 231 | registration_->getFinalTransformation().cast() * init_affine.matrix()); 232 | 233 | loop_edge.relative_pose = Eigen::Isometry3d(from.inverse() * to); 234 | loop_edges_.push_back(loop_edge); 235 | 236 | std::cout << "PoseAdjustment" << " distance:" << min_submap.distance << ", score:" << fitness_score << std::endl; 237 | std::cout << "id_loop_point 1:" << id_min << " id_loop_point 2:" << num_submaps - 1 << std::endl; 238 | std::cout << "final transformation:" << std::endl; 239 | std::cout << registration_->getFinalTransformation() << std::endl; 240 | doPoseAdjustment(map_array_msg, use_save_map_in_loop_); 241 | 242 | return; 243 | } 244 | 245 | std::cout << "min_submap_distance:" << min_submap.distance << " min_fitness_score:" << fitness_score << std::endl; 246 | } 247 | } 248 | 249 | void GraphBasedSlamComponent::doPoseAdjustment( 250 | lidarslam_msgs::msg::MapArray map_array_msg, 251 | bool do_save_map) 252 | { 253 | 254 | g2o::SparseOptimizer optimizer; 255 | optimizer.setVerbose(false); 256 | std::unique_ptr linear_solver = 257 | g2o::make_unique>(); 258 | g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg( 259 | g2o::make_unique(std::move(linear_solver))); 260 | 261 | optimizer.setAlgorithm(solver); 262 | 263 | int submaps_size = map_array_msg.submaps.size(); 264 | Eigen::Matrix info_mat = Eigen::Matrix::Identity(); 265 | for (int i = 0; i < submaps_size; i++) { 266 | Eigen::Affine3d affine; 267 | Eigen::fromMsg(map_array_msg.submaps[i].pose, affine); 268 | Eigen::Isometry3d pose(affine.matrix()); 269 | 270 | g2o::VertexSE3 * vertex_se3 = new g2o::VertexSE3(); 271 | vertex_se3->setId(i); 272 | vertex_se3->setEstimate(pose); 273 | if (i == 0) {vertex_se3->setFixed(true);} 274 | optimizer.addVertex(vertex_se3); 275 | 276 | if (i > num_adjacent_pose_cnstraints_) { 277 | for (int j = 0; j < num_adjacent_pose_cnstraints_; j++) { 278 | Eigen::Affine3d pre_affine; 279 | Eigen::fromMsg(map_array_msg.submaps[i - num_adjacent_pose_cnstraints_ + j].pose, pre_affine); 280 | Eigen::Isometry3d pre_pose(pre_affine.matrix()); 281 | Eigen::Isometry3d relative_pose = pre_pose.inverse() * pose; 282 | g2o::EdgeSE3 * edge_se3 = new g2o::EdgeSE3(); 283 | edge_se3->setMeasurement(relative_pose); 284 | edge_se3->setInformation(info_mat); 285 | edge_se3->vertices()[0] = optimizer.vertex(i - num_adjacent_pose_cnstraints_ + j); 286 | edge_se3->vertices()[1] = optimizer.vertex(i); 287 | optimizer.addEdge(edge_se3); 288 | } 289 | } 290 | 291 | } 292 | /* loop edge */ 293 | for (auto loop_edge : loop_edges_) { 294 | g2o::EdgeSE3 * edge_se3 = new g2o::EdgeSE3(); 295 | edge_se3->setMeasurement(loop_edge.relative_pose); 296 | edge_se3->setInformation(info_mat); 297 | edge_se3->vertices()[0] = optimizer.vertex(loop_edge.pair_id.first); 298 | edge_se3->vertices()[1] = optimizer.vertex(loop_edge.pair_id.second); 299 | optimizer.addEdge(edge_se3); 300 | } 301 | 302 | optimizer.initializeOptimization(); 303 | optimizer.optimize(10); 304 | optimizer.save("pose_graph.g2o"); 305 | 306 | /* modified_map publish */ 307 | std::cout << "modified_map publish" << std::endl; 308 | lidarslam_msgs::msg::MapArray modified_map_array_msg; 309 | modified_map_array_msg.header = map_array_msg.header; 310 | nav_msgs::msg::Path path; 311 | path.header.frame_id = "map"; 312 | pcl::PointCloud::Ptr map_ptr(new pcl::PointCloud()); 313 | for (int i = 0; i < submaps_size; i++) { 314 | g2o::VertexSE3 * vertex_se3 = static_cast(optimizer.vertex(i)); 315 | Eigen::Affine3d se3 = vertex_se3->estimate(); 316 | geometry_msgs::msg::Pose pose = tf2::toMsg(se3); 317 | 318 | /* map */ 319 | Eigen::Affine3d previous_affine; 320 | tf2::fromMsg(map_array_msg.submaps[i].pose, previous_affine); 321 | 322 | pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); 323 | pcl::PointCloud::Ptr transformed_cloud_ptr( 324 | new pcl::PointCloud()); 325 | pcl::fromROSMsg(map_array_msg.submaps[i].cloud, *cloud_ptr); 326 | 327 | pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, se3.matrix().cast()); 328 | sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr(new sensor_msgs::msg::PointCloud2); 329 | pcl::toROSMsg(*transformed_cloud_ptr, *cloud_msg_ptr); 330 | *map_ptr += *transformed_cloud_ptr; 331 | 332 | /* submap */ 333 | lidarslam_msgs::msg::SubMap submap; 334 | submap.header = map_array_msg.submaps[i].header; 335 | submap.pose = pose; 336 | submap.cloud = *cloud_msg_ptr; 337 | modified_map_array_msg.submaps.push_back(submap); 338 | 339 | /* path */ 340 | geometry_msgs::msg::PoseStamped pose_stamped; 341 | pose_stamped.header = submap.header; 342 | pose_stamped.pose = submap.pose; 343 | path.poses.push_back(pose_stamped); 344 | 345 | } 346 | 347 | modified_map_array_pub_->publish(modified_map_array_msg); 348 | modified_path_pub_->publish(path); 349 | 350 | sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2); 351 | pcl::toROSMsg(*map_ptr, *map_msg_ptr); 352 | map_msg_ptr->header.frame_id = "map"; 353 | modified_map_pub_->publish(*map_msg_ptr); 354 | if (do_save_map) {pcl::io::savePCDFileASCII("map.pcd", *map_ptr);} // too heavy 355 | 356 | } 357 | 358 | } // namespace graphslam 359 | 360 | #include 361 | RCLCPP_COMPONENTS_REGISTER_NODE(graphslam::GraphBasedSlamComponent) 362 | -------------------------------------------------------------------------------- /graph_based_slam/src/graph_based_slam_node.cpp: -------------------------------------------------------------------------------- 1 | #include "graph_based_slam/graph_based_slam_component.h" 2 | #include 3 | 4 | int main(int argc, char * argv[]) 5 | { 6 | rclcpp::init(argc, argv); 7 | rclcpp::NodeOptions options; 8 | options.use_intra_process_comms(true); 9 | rclcpp::spin(std::make_shared(options)); 10 | rclcpp::shutdown(); 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /lidarslam_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(lidarslam_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(sensor_msgs REQUIRED) 23 | find_package(builtin_interfaces REQUIRED) 24 | find_package(rosidl_default_generators REQUIRED) 25 | 26 | rosidl_generate_interfaces(${PROJECT_NAME} 27 | "msg/SubMap.msg" 28 | "msg/MapArray.msg" 29 | DEPENDENCIES builtin_interfaces std_msgs geometry_msgs sensor_msgs 30 | ) 31 | 32 | if(BUILD_TESTING) 33 | find_package(ament_lint_auto REQUIRED) 34 | ament_lint_auto_find_test_dependencies() 35 | endif() 36 | 37 | ament_export_dependencies(rosidl_default_runtime) 38 | ament_export_include_directories(include) 39 | ament_package() 40 | -------------------------------------------------------------------------------- /lidarslam_msgs/msg/MapArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | lidarslam_msgs/SubMap[] submaps 3 | int8 cloud_coordinate 4 | int8 LOCAL = 0 5 | int8 GLOBAL = 1 -------------------------------------------------------------------------------- /lidarslam_msgs/msg/SubMap.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64 distance 3 | geometry_msgs/Pose pose 4 | sensor_msgs/PointCloud2 cloud -------------------------------------------------------------------------------- /lidarslam_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidarslam_msgs 5 | 0.0.0 6 | lidarslam_ros2 msgs 7 | sasaki 8 | BSD2 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | std_msgs 14 | geometry_msgs 15 | sensor_msgs 16 | 17 | rosidl_default_generators 18 | rosidl_default_runtime 19 | 20 | rosidl_interface_packages 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /scanmatcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(scanmatcher) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | #set(CMAKE_CXX_STANDARD 14) 12 | set(CMAKE_CXX_STANDARD 17) 13 | endif() 14 | 15 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 16 | #add_compile_options(-Wall -Wextra -Wpedantic) 17 | #add_compile_options(-Wextra -Wpedantic) 18 | endif() 19 | 20 | #SET(CMAKE_BUILD_TYPE "RELEASE") 21 | SET(CMAKE_CXX_FLAGS "-O2 -g ${CMAKE_CXX_FLAGS}") 22 | 23 | # find dependencies 24 | find_package(ament_cmake REQUIRED) 25 | find_package(rclcpp REQUIRED) 26 | find_package(rclcpp_components REQUIRED) 27 | find_package(geometry_msgs REQUIRED) 28 | find_package(sensor_msgs REQUIRED) 29 | find_package(nav_msgs REQUIRED) 30 | find_package(tf2_ros REQUIRED) 31 | find_package(tf2_sensor_msgs REQUIRED) 32 | find_package(tf2_geometry_msgs REQUIRED) 33 | find_package(tf2_eigen REQUIRED) 34 | find_package(pcl_conversions REQUIRED) 35 | find_package(pcl_msgs REQUIRED) 36 | find_package(lidarslam_msgs REQUIRED) 37 | find_package(ndt_omp_ros2 REQUIRED) 38 | find_package(PCL REQUIRED) 39 | find_package(OpenMP) 40 | find_package(GTSAM) 41 | 42 | #find_package(liosam_msgs REQUIRED) 43 | find_package(OpenCV REQUIRED) 44 | 45 | 46 | if (OPENMP_FOUND) 47 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 48 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 49 | endif() 50 | 51 | if(BUILD_TESTING) 52 | find_package(ament_lint_auto REQUIRED) 53 | ament_lint_auto_find_test_dependencies() 54 | endif() 55 | 56 | add_library(scanmatcher_component SHARED 57 | src/scanmatcher_component.cpp 58 | ) 59 | 60 | target_compile_definitions(scanmatcher_component PRIVATE "GS_SM_BUILDING_DLL") 61 | 62 | ament_target_dependencies(scanmatcher_component 63 | rclcpp 64 | rclcpp_components 65 | tf2_ros 66 | tf2_geometry_msgs 67 | tf2_sensor_msgs 68 | tf2_eigen 69 | pcl_conversions 70 | pcl_msgs 71 | geometry_msgs 72 | sensor_msgs 73 | nav_msgs 74 | lidarslam_msgs 75 | ndt_omp_ros2 76 | OpenCV 77 | ) 78 | 79 | target_link_libraries(scanmatcher_component 80 | ${OpenCV_LIBRARIES} 81 | ${PCL_LIBRARIES}) 82 | 83 | 84 | add_executable(scanmatcher_node 85 | src/scanmatcher_node.cpp 86 | ) 87 | 88 | target_link_libraries(scanmatcher_node 89 | scanmatcher_component ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 90 | 91 | ament_target_dependencies(scanmatcher_node 92 | rclcpp) 93 | 94 | include_directories( 95 | include 96 | ${PCL_INCLUDE_DIRS} 97 | ${OpenCV_INCLUDE_DIRS} 98 | ) 99 | link_directories( 100 | ${PCL_LIBRARY_DIRS} 101 | ${OpenCV_LIBRARY_DIRS} 102 | ) 103 | add_definitions(${PCL_DEFINITIONS}) 104 | 105 | if(GTSAM_FOUND) 106 | include_directories(${GTSAM_INCLUDE_DIRS}) 107 | link_directories(${GTSAM_LIBRARY_DIRS}) 108 | endif() 109 | 110 | rclcpp_components_register_nodes(scanmatcher_component 111 | "graphslam::ScanMatcherComponent") 112 | 113 | ament_export_libraries(scanmatcher_component) 114 | 115 | install( 116 | DIRECTORY "include/" 117 | DESTINATION include 118 | ) 119 | 120 | install( 121 | DIRECTORY launch param 122 | DESTINATION share/${PROJECT_NAME} 123 | ) 124 | 125 | install(TARGETS 126 | scanmatcher_component 127 | ARCHIVE DESTINATION lib 128 | LIBRARY DESTINATION lib 129 | RUNTIME DESTINATION bin 130 | ) 131 | 132 | install(TARGETS 133 | scanmatcher_node 134 | DESTINATION lib/${PROJECT_NAME} 135 | ) 136 | 137 | if(GTSAM_FOUND) 138 | add_executable(imu_preintegration src/imu_preintegration.cpp) 139 | target_link_libraries(imu_preintegration 140 | gtsam 141 | ${GTSAM_LIBRARIES} 142 | ${PCL_LIBRARIES}) 143 | ament_target_dependencies(imu_preintegration 144 | rclcpp 145 | rclcpp_components 146 | tf2_ros 147 | tf2_geometry_msgs 148 | tf2_sensor_msgs 149 | tf2_eigen 150 | geometry_msgs 151 | sensor_msgs 152 | nav_msgs 153 | pcl_msgs 154 | #GTSAM 155 | ) 156 | add_executable(image_projection src/image_projection.cpp) 157 | ament_target_dependencies(image_projection 158 | rclcpp 159 | rclcpp_components 160 | tf2_ros 161 | tf2_geometry_msgs 162 | tf2_sensor_msgs 163 | tf2_eigen 164 | geometry_msgs 165 | sensor_msgs 166 | nav_msgs 167 | pcl_msgs 168 | #liosam_msgs 169 | PCL 170 | OpenCV 171 | ) 172 | target_link_libraries(imu_preintegration 173 | ${OpenCV_LIBRARIES} 174 | ${PCL_LIBRARIES}) 175 | 176 | install(TARGETS 177 | imu_preintegration 178 | DESTINATION lib/${PROJECT_NAME} 179 | ) 180 | install(TARGETS 181 | image_projection 182 | DESTINATION lib/${PROJECT_NAME} 183 | ) 184 | endif() 185 | 186 | ament_export_include_directories(include) 187 | ament_package() 188 | -------------------------------------------------------------------------------- /scanmatcher/images/big_loop_with_lo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/scanmatcher/images/big_loop_with_lo.png -------------------------------------------------------------------------------- /scanmatcher/images/big_loop_without_lo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/scanmatcher/images/big_loop_without_lo.png -------------------------------------------------------------------------------- /scanmatcher/images/li_slam.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/scanmatcher/images/li_slam.gif -------------------------------------------------------------------------------- /scanmatcher/images/li_slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/scanmatcher/images/li_slam.png -------------------------------------------------------------------------------- /scanmatcher/images/liosam_thesis.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/scanmatcher/images/liosam_thesis.png -------------------------------------------------------------------------------- /scanmatcher/images/rosgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rsasaki0109/li_slam_ros2/a684c7af7ba1f08bff957c3a0690c77476b7c077/scanmatcher/images/rosgraph.png -------------------------------------------------------------------------------- /scanmatcher/include/scanmatcher/scanmatcher_component.h: -------------------------------------------------------------------------------- 1 | #ifndef GS_SM_COMPONENT_H_INCLUDED 2 | #define GS_SM_COMPONENT_H_INCLUDED 3 | 4 | #if __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | // The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from 9 | // demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos 10 | #if defined _WIN32 || defined __CYGWIN__ 11 | #ifdef __GNUC__ 12 | #define GS_SM_EXPORT __attribute__ ((dllexport)) 13 | #define GS_SM_IMPORT __attribute__ ((dllimport)) 14 | #else 15 | #define GS_SM_EXPORT __declspec(dllexport) 16 | #define GS_SM_IMPORT __declspec(dllimport) 17 | #endif 18 | #ifdef GS_SM_BUILDING_DLL 19 | #define GS_SM_PUBLIC GS_SM_EXPORT 20 | #else 21 | #define GS_SM_PUBLIC GS_SM_IMPORT 22 | #endif 23 | #define GS_SM_PUBLIC_TYPE GS_SM_PUBLIC 24 | #define GS_SM_LOCAL 25 | #else 26 | #define GS_SM_EXPORT __attribute__ ((visibility("default"))) 27 | #define GS_SM_IMPORT 28 | #if __GNUC__ >= 4 29 | #define GS_SM_PUBLIC __attribute__ ((visibility("default"))) 30 | #define GS_SM_LOCAL __attribute__ ((visibility("hidden"))) 31 | #else 32 | #define GS_SM_PUBLIC 33 | #define GS_SM_LOCAL 34 | #endif 35 | #define GS_SM_PUBLIC_TYPE 36 | #endif 37 | 38 | #if __cplusplus 39 | } // extern "C" 40 | #endif 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | #include 61 | #include "scanmatcher/utility.h" 62 | 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | 71 | #include 72 | #include 73 | #include 74 | 75 | #include 76 | 77 | 78 | using PointType = pcl::PointXYZI; 79 | //using PointType = PointXYZIRT; 80 | 81 | namespace graphslam 82 | { 83 | //class ScanMatcherComponent : public ParamServer 84 | class ScanMatcherComponent: public rclcpp::Node 85 | { 86 | public: 87 | GS_SM_PUBLIC 88 | explicit ScanMatcherComponent(const rclcpp::NodeOptions & options); 89 | 90 | private: 91 | rclcpp::Clock clock_; 92 | tf2_ros::Buffer tfbuffer_; 93 | tf2_ros::TransformListener listener_; 94 | tf2_ros::TransformBroadcaster broadcaster_; 95 | 96 | std::string global_frame_id_; 97 | std::string robot_frame_id_; 98 | std::string odom_frame_id_; 99 | 100 | // pcl::Registration < PointType, PointType > ::Ptr registration_; 101 | boost::shared_ptr> registration_; 102 | 103 | rclcpp::Subscription < geometry_msgs::msg::PoseStamped > ::SharedPtr initial_pose_sub_; 104 | rclcpp::Subscription < sensor_msgs::msg::Imu > ::SharedPtr imu_sub_; 105 | rclcpp::Subscription < nav_msgs::msg::Odometry > ::SharedPtr odom_sub_; 106 | rclcpp::Subscription < sensor_msgs::msg::PointCloud2 > ::SharedPtr input_cloud_sub_; 107 | 108 | std::mutex mtx_; 109 | pcl::PointCloud < PointType > targeted_cloud_; 110 | rclcpp::Time last_map_time_; 111 | bool mapping_flag_ {false}; 112 | bool is_map_updated_ {false}; 113 | std::thread mapping_thread_; 114 | std::packaged_task < void() > mapping_task_; 115 | std::future < void > mapping_future_; 116 | 117 | geometry_msgs::msg::PoseStamped corrent_pose_stamped_; 118 | lidarslam_msgs::msg::MapArray map_array_msg_; 119 | nav_msgs::msg::Path path_; 120 | rclcpp::Publisher < geometry_msgs::msg::PoseStamped > ::SharedPtr pose_pub_; 121 | rclcpp::Publisher < sensor_msgs::msg::PointCloud2 > ::SharedPtr map_pub_; 122 | rclcpp::Publisher < lidarslam_msgs::msg::MapArray > ::SharedPtr map_array_pub_; 123 | rclcpp::Publisher < nav_msgs::msg::Path > ::SharedPtr path_pub_; 124 | rclcpp::Publisher < nav_msgs::msg::Odometry > ::SharedPtr odom_pub_; 125 | 126 | void initializePubSub(); 127 | void initializeMap(const pcl::PointCloud ::Ptr & cloud_ptr, const std_msgs::msg::Header & header); 128 | void receiveCloud( 129 | const pcl::PointCloud < PointType > ::ConstPtr & input_cloud_ptr, 130 | const rclcpp::Time stamp); 131 | void receiveImu(const sensor_msgs::msg::Imu imu_msg); 132 | void receiveOdom(const nav_msgs::msg::Odometry odom_msg); 133 | void publishMapAndPose( 134 | const pcl::PointCloud < PointType > ::ConstPtr & cloud_ptr, 135 | const Eigen::Matrix4f final_transformation, 136 | const rclcpp::Time stamp 137 | ); 138 | Eigen::Matrix4f getTransformation(const geometry_msgs::msg::Pose pose); 139 | void publishMap(const lidarslam_msgs::msg::MapArray & map_array_msg , const std::string & map_frame_id); 140 | void updateMap( 141 | const pcl::PointCloud < PointType > ::ConstPtr cloud_ptr, 142 | const Eigen::Matrix4f final_transformation, 143 | const geometry_msgs::msg::PoseStamped corrent_pose_stamped 144 | ); 145 | 146 | bool initial_pose_received_ {false}; 147 | bool initial_cloud_received_ {false}; 148 | 149 | // setting parameter 150 | double trans_for_mapupdate_; 151 | double vg_size_for_input_; 152 | double vg_size_for_map_; 153 | bool use_min_max_filter_ {false}; 154 | double scan_min_range_ {0.1}; 155 | double scan_max_range_ {100.0}; 156 | double map_publish_period_; 157 | int num_targeted_cloud_; 158 | 159 | bool set_initial_pose_ {false}; 160 | bool use_odom_ {false}; 161 | bool use_imu_ {false}; 162 | bool debug_flag_ {false}; 163 | 164 | // map 165 | Eigen::Vector3d previous_position_; 166 | double trans_; 167 | double latest_distance_ {0}; 168 | 169 | // initial_pose 170 | double initial_pose_x_; 171 | double initial_pose_y_; 172 | double initial_pose_z_; 173 | double initial_pose_qx_; 174 | double initial_pose_qy_; 175 | double initial_pose_qz_; 176 | double initial_pose_qw_; 177 | 178 | // odom 179 | Eigen::Matrix4f previous_odom_mat_ {Eigen::Matrix4f::Identity()}; 180 | 181 | // lider inertial slam 182 | static const int odom_que_length_ {200}; 183 | std::array < nav_msgs::msg::Odometry, odom_que_length_ > odom_que_; 184 | int odom_ptr_front_ {0}, odom_ptr_last_ {-1}; 185 | 186 | }; 187 | } 188 | 189 | #endif //GS_SM_COMPONENT_H_INCLUDED 190 | -------------------------------------------------------------------------------- /scanmatcher/include/scanmatcher/utility.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | // 3 | // Copyright (c) 2020, Tixiao Shan 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // * Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | #ifndef _UTILITY_LIDAR_ODOMETRY_H_ 33 | #define _UTILITY_LIDAR_ODOMETRY_H_ 34 | 35 | // #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | //#include 42 | #include 43 | #include 44 | 45 | // #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | #include 75 | #include 76 | #include 77 | #include 78 | #include 79 | #include 80 | #include 81 | #include 82 | #include 83 | #include 84 | #include 85 | 86 | using namespace std; 87 | 88 | enum class SensorType { VELODYNE, OUSTER, LIVOX }; 89 | 90 | template < typename T > 91 | double ROS_TIME(T msg) 92 | { 93 | return msg->header.stamp.sec + 94 | msg->header.stamp.nanosec * 1e-9; 95 | } 96 | 97 | class ParamServer: public rclcpp::Node 98 | { 99 | 100 | public: 101 | string pointCloudTopic; 102 | string imuTopic; 103 | string odomTopic; 104 | 105 | // Lidar Sensor Configuration 106 | SensorType sensor; 107 | int N_SCAN; 108 | int Horizon_SCAN; 109 | 110 | // IMU 111 | float imuAccNoise; 112 | float imuGyrNoise; 113 | float imuAccBiasN; 114 | float imuGyrBiasN; 115 | float imuGravity; 116 | vector < double > extRotV; 117 | vector < double > extRPYV; 118 | vector < double > extTransV; 119 | Eigen::Matrix3d extRot; 120 | Eigen::Matrix3d extRPY; 121 | Eigen::Vector3d extTrans; 122 | Eigen::Quaterniond extQRPY; 123 | 124 | ParamServer(std::string node_name, const rclcpp::NodeOptions & options) 125 | : Node(node_name, options) 126 | { 127 | declare_parameter("pointCloudTopic", "points_raw"); 128 | get_parameter("pointCloudTopic", pointCloudTopic); 129 | declare_parameter("imuTopic", "imu_correct"); 130 | get_parameter("imuTopic", imuTopic); 131 | declare_parameter("odomTopic", "preintegrated_odom"); 132 | get_parameter("odomTopic", odomTopic); 133 | 134 | std::string sensorStr; 135 | declare_parameter("sensor", ""); 136 | get_parameter("sensor", sensorStr); 137 | if (sensorStr == "velodyne") 138 | { 139 | sensor = SensorType::VELODYNE; 140 | } 141 | else if (sensorStr == "ouster") 142 | { 143 | sensor = SensorType::OUSTER; 144 | } 145 | else if (sensorStr == "livox") 146 | { 147 | sensor = SensorType::LIVOX; 148 | } 149 | else 150 | { 151 | RCLCPP_ERROR_STREAM(get_logger(), 152 | "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'livox'): " << sensorStr); 153 | rclcpp::shutdown(); 154 | } 155 | 156 | declare_parameter("N_SCAN", 16); 157 | get_parameter("N_SCAN", N_SCAN); 158 | declare_parameter("Horizon_SCAN", 1800); 159 | get_parameter("Horizon_SCAN", Horizon_SCAN); 160 | 161 | declare_parameter("imuAccNoise", 3.9939570888238808e-03); 162 | get_parameter("imuAccNoise", imuAccNoise); 163 | declare_parameter("imuGyrNoise", 1.5636343949698187e-03); 164 | get_parameter("imuGyrNoise", imuGyrNoise); 165 | declare_parameter("imuAccBiasN", 6.4356659353532566e-05); 166 | get_parameter("imuAccBiasN", imuAccBiasN); 167 | declare_parameter("imuGyrBiasN", 3.5640318696367613e-05); 168 | get_parameter("imuGyrBiasN", imuGyrBiasN); 169 | declare_parameter("imuGravity", 9.80511); 170 | get_parameter("imuGravity", imuGravity); 171 | double org_data1[] = {-1, 0, 0, 172 | 0, 1, 0, 173 | 0, 0, -1}; 174 | std::vector < double > data1(org_data1, std::end(org_data1)); 175 | declare_parameter("extrinsicRot", data1); 176 | get_parameter("extrinsicRot", extRotV); 177 | double org_data2[] = {0, 1, 0, 178 | -1, 0, 0, 179 | 0, 0, 1}; 180 | std::vector < double > data2(org_data2, std::end(org_data2)); 181 | declare_parameter("extrinsicRPY", data2); 182 | get_parameter("extrinsicRPY", extRPYV); 183 | double org_data3[] = {0, 0, 0}; 184 | std::vector < double > data3(org_data3, std::end(org_data3)); 185 | declare_parameter("extrinsicTrans", data3); 186 | get_parameter("extrinsicTrans", extTransV); 187 | extRot = Eigen::Map < const Eigen::Matrix < double, -1, -1, 188 | Eigen::RowMajor >> (extRotV.data(), 3, 3); 189 | extRPY = Eigen::Map < const Eigen::Matrix < double, -1, -1, 190 | Eigen::RowMajor >> (extRPYV.data(), 3, 3); 191 | extTrans = Eigen::Map < const Eigen::Matrix < double, -1, -1, 192 | Eigen::RowMajor >> (extTransV.data(), 3, 1); 193 | extQRPY = Eigen::Quaterniond(extRPY); 194 | 195 | } 196 | 197 | sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu & imu_in) 198 | { 199 | sensor_msgs::msg::Imu imu_out = imu_in; 200 | // rotate acceleration 201 | Eigen::Vector3d acc( 202 | imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, 203 | imu_in.linear_acceleration.z); 204 | acc = extRot * acc; 205 | imu_out.linear_acceleration.x = acc.x(); 206 | imu_out.linear_acceleration.y = acc.y(); 207 | imu_out.linear_acceleration.z = acc.z(); 208 | // rotate gyroscope 209 | Eigen::Vector3d gyr( 210 | imu_in.angular_velocity.x, imu_in.angular_velocity.y, 211 | imu_in.angular_velocity.z); 212 | gyr = extRot * gyr; 213 | imu_out.angular_velocity.x = gyr.x(); 214 | imu_out.angular_velocity.y = gyr.y(); 215 | imu_out.angular_velocity.z = gyr.z(); 216 | // rotate roll pitch yaw 217 | Eigen::Quaterniond q_from( 218 | imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, 219 | imu_in.orientation.z); 220 | Eigen::Quaterniond q_final = q_from * extQRPY; 221 | imu_out.orientation.x = q_final.x(); 222 | imu_out.orientation.y = q_final.y(); 223 | imu_out.orientation.z = q_final.z(); 224 | imu_out.orientation.w = q_final.w(); 225 | 226 | #if 0 227 | cout << "extRot: " << endl; 228 | cout << extRot << endl; 229 | cout << "extRPY: " << endl; 230 | cout << extRPY << endl; 231 | #endif 232 | 233 | return imu_out; 234 | } 235 | }; 236 | 237 | 238 | #endif 239 | -------------------------------------------------------------------------------- /scanmatcher/launch/lio.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import launch 4 | import launch_ros.actions 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | def generate_launch_description(): 9 | 10 | mapping_param_dir = launch.substitutions.LaunchConfiguration( 11 | 'mapping_param_dir', 12 | default=os.path.join( 13 | get_package_share_directory('scanmatcher'), 14 | 'param', 15 | 'lio.yaml')) 16 | 17 | mapping = launch_ros.actions.Node( 18 | package='scanmatcher', 19 | executable='scanmatcher_node', 20 | parameters=[mapping_param_dir], 21 | remappings=[('/input_cloud','/cloud_deskewed')], 22 | output='screen' 23 | ) 24 | 25 | graphbasedslam = launch_ros.actions.Node( 26 | package='graph_based_slam', 27 | executable='graph_based_slam_node', 28 | parameters=[mapping_param_dir], 29 | output='screen' 30 | ) 31 | 32 | tf = launch_ros.actions.Node( 33 | package='tf2_ros', 34 | executable='static_transform_publisher', 35 | arguments=['0','0','0','0','0','0','1','base_link','velodyne'] 36 | ) 37 | 38 | imu_pre = launch_ros.actions.Node( 39 | package='scanmatcher', 40 | executable='imu_preintegration', 41 | remappings=[('/odometry','/odom')], 42 | parameters=[mapping_param_dir], 43 | output='screen' 44 | ) 45 | 46 | img_pro = launch_ros.actions.Node( 47 | package='scanmatcher', 48 | executable='image_projection', 49 | parameters=[mapping_param_dir], 50 | output='screen' 51 | ) 52 | 53 | 54 | return launch.LaunchDescription([ 55 | launch.actions.DeclareLaunchArgument( 56 | 'mapping_param_dir', 57 | default_value=mapping_param_dir, 58 | description='Full path to mapping parameter file to load'), 59 | mapping, 60 | tf, 61 | imu_pre, 62 | img_pro, 63 | graphbasedslam, 64 | ]) -------------------------------------------------------------------------------- /scanmatcher/launch/lio_bigloop.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import launch 4 | import launch_ros.actions 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | def generate_launch_description(): 9 | 10 | mapping_param_dir = launch.substitutions.LaunchConfiguration( 11 | 'mapping_param_dir', 12 | default=os.path.join( 13 | get_package_share_directory('scanmatcher'), 14 | 'param', 15 | 'lio_bigloop.yaml')) 16 | 17 | mapping = launch_ros.actions.Node( 18 | package='scanmatcher', 19 | executable='scanmatcher_node', 20 | parameters=[mapping_param_dir], 21 | remappings=[('/input_cloud','/cloud_deskewed')], 22 | output='screen' 23 | ) 24 | 25 | graphbasedslam = launch_ros.actions.Node( 26 | package='graph_based_slam', 27 | executable='graph_based_slam_node', 28 | parameters=[mapping_param_dir], 29 | output='screen' 30 | ) 31 | 32 | tf = launch_ros.actions.Node( 33 | package='tf2_ros', 34 | executable='static_transform_publisher', 35 | arguments=['0','0','0','0','0','0','1','base_link','velodyne'] 36 | ) 37 | 38 | imu_pre = launch_ros.actions.Node( 39 | package='scanmatcher', 40 | executable='imu_preintegration', 41 | remappings=[('/odometry','/odom')], 42 | parameters=[mapping_param_dir], 43 | output='screen' 44 | ) 45 | 46 | img_pro = launch_ros.actions.Node( 47 | package='scanmatcher', 48 | executable='image_projection', 49 | parameters=[mapping_param_dir], 50 | output='screen' 51 | ) 52 | 53 | 54 | return launch.LaunchDescription([ 55 | launch.actions.DeclareLaunchArgument( 56 | 'mapping_param_dir', 57 | default_value=mapping_param_dir, 58 | description='Full path to mapping parameter file to load'), 59 | mapping, 60 | tf, 61 | imu_pre, 62 | img_pro, 63 | graphbasedslam, 64 | ]) -------------------------------------------------------------------------------- /scanmatcher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | scanmatcher 5 | 0.0.0 6 | scan-matching package 7 | sasaki 8 | BSD2 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | rclcpp 15 | rclcpp_components 16 | geometry_msgs 17 | sensor_msgs 18 | nav_msgs 19 | tf2_ros 20 | tf2_sensor_msgs 21 | tf2_geometry_msgs 22 | tf2_eigen 23 | pcl_conversions 24 | pcl_msgs 25 | lidarslam_msgs 26 | ndt_omp_ros2 27 | libg2o 28 | libpcl-all-dev 29 | libopencv-dev 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /scanmatcher/param/lio.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # LiDAR setting 4 | pointCloudTopic: "points_raw" 5 | sensor: velodyne # lidar sensor type, 'velodyne' or 'ouster' or 'livox' 6 | N_SCAN: 16 # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6) 7 | Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000) 8 | # IMU setting 9 | imuTopic: "imu_raw" 10 | imuAccNoise: 3.9939570888238808e-03 11 | imuGyrNoise: 1.5636343949698187e-03 12 | imuAccBiasN: 6.4356659353532566e-05 13 | imuGyrBiasN: 3.5640318696367613e-05 14 | imuGravity: 9.80511 15 | extrinsicTrans: [0.0, 0.0, 0.0] 16 | extrinsicRot: [-1.0, 0.0, 0.0, 17 | 0.0, 1.0, 0.0, 18 | 0.0, 0.0, -1.0] 19 | extrinsicRPY: [0.0, 1.0, 0.0, 20 | -1.0, 0.0, 0.0, 21 | 0.0, 0.0, 1.0] 22 | 23 | scan_matcher: 24 | ros__parameters: 25 | global_frame_id: "map" 26 | robot_frame_id: "base_link" 27 | registration_method: "NDT" 28 | ndt_resolution: 2.0 29 | ndt_num_threads: 0 30 | trans_for_mapupdate: 1.5 31 | vg_size_for_input: 0.1 32 | vg_size_for_map: 0.1 33 | use_min_max_filter: false 34 | map_publish_period: 100.0 35 | num_targeted_cloud: 20 36 | set_initial_pose: true 37 | initial_pose_x: 0.0 38 | initial_pose_y: 0.0 39 | initial_pose_z: 0.0 40 | initial_pose_qx: 0.0 41 | initial_pose_qy: 0.0 42 | initial_pose_qz: 0.0 43 | initial_pose_qw: 1.0 44 | use_imu: true 45 | use_odom: false 46 | debug_flag: false 47 | 48 | graph_based_slam: 49 | ros__parameters: 50 | registration_method: "GICP" 51 | ndt_resolution: 5.0 52 | ndt_num_threads: 1 53 | voxel_leaf_size: 0.1 54 | loop_detection_period: 3000 55 | threshold_loop_closure_score: 1.0 56 | distance_loop_closure: 25.0 57 | range_of_searching_loop_closure: 20.0 58 | search_submap_num: 4 59 | num_adjacent_pose_cnstraints: 5 60 | use_save_map_in_loop: false 61 | -------------------------------------------------------------------------------- /scanmatcher/param/lio_bigloop.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # LiDAR setting 4 | pointCloudTopic: "points_raw" 5 | sensor: velodyne # lidar sensor type, 'velodyne' or 'ouster' or 'livox' 6 | N_SCAN: 16 # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6) 7 | Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000) 8 | # IMU setting 9 | imuTopic: "imu_correct" 10 | imuAccNoise: 3.9939570888238808e-03 11 | imuGyrNoise: 1.5636343949698187e-03 12 | imuAccBiasN: 6.4356659353532566e-05 13 | imuGyrBiasN: 3.5640318696367613e-05 14 | imuGravity: 9.80511 15 | extrinsicTrans: [0.0, 0.0, 0.0] 16 | extrinsicRot: [1.0, 0.0, 0.0, 17 | 0.0, 1.0, 0.0, 18 | 0.0, 0.0, 1.0] 19 | extrinsicRPY: [1.0, 0.0, 0.0, 20 | 0.0, 1.0, 0.0, 21 | 0.0, 0.0, 1.0] 22 | 23 | scan_matcher: 24 | ros__parameters: 25 | global_frame_id: "map" 26 | robot_frame_id: "base_link" 27 | registration_method: "NDT" 28 | ndt_resolution: 2.0 29 | ndt_num_threads: 0 30 | trans_for_mapupdate: 1.5 31 | vg_size_for_input: 0.1 32 | vg_size_for_map: 0.1 33 | use_min_max_filter: false 34 | map_publish_period: 100.0 35 | num_targeted_cloud: 20 36 | set_initial_pose: true 37 | initial_pose_x: 0.0 38 | initial_pose_y: 0.0 39 | initial_pose_z: 0.0 40 | initial_pose_qx: 0.0 41 | initial_pose_qy: 0.0 42 | initial_pose_qz: 0.0 43 | initial_pose_qw: 1.0 44 | use_imu: true 45 | use_odom: false 46 | debug_flag: false 47 | 48 | graph_based_slam: 49 | ros__parameters: 50 | registration_method: "GICP" 51 | ndt_resolution: 5.0 52 | ndt_num_threads: 1 53 | voxel_leaf_size: 0.1 54 | loop_detection_period: 3000 55 | threshold_loop_closure_score: 1.0 56 | distance_loop_closure: 25.0 57 | range_of_searching_loop_closure: 20.0 58 | search_submap_num: 4 59 | num_adjacent_pose_cnstraints: 5 60 | use_save_map_in_loop: false 61 | -------------------------------------------------------------------------------- /scanmatcher/rviz/lio.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | Splitter Ratio: 0.5 11 | Tree Height: 613 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | Visualization Manager: 25 | Class: "" 26 | Displays: 27 | - Alpha: 0.5 28 | Cell Size: 10 29 | Class: rviz_default_plugins/Grid 30 | Color: 160; 160; 164 31 | Enabled: true 32 | Line Style: 33 | Line Width: 0.029999999329447746 34 | Value: Lines 35 | Name: Grid 36 | Normal Cell Count: 0 37 | Offset: 38 | X: 0 39 | Y: 0 40 | Z: 0 41 | Plane: XY 42 | Plane Cell Count: 30 43 | Reference Frame: 44 | Value: true 45 | - Alpha: 1 46 | Autocompute Intensity Bounds: true 47 | Autocompute Value Bounds: 48 | Max Value: 10 49 | Min Value: -10 50 | Value: true 51 | Axis: Z 52 | Channel Name: intensity 53 | Class: rviz_default_plugins/PointCloud2 54 | Color: 255; 255; 255 55 | Color Transformer: Intensity 56 | Decay Time: 0 57 | Enabled: true 58 | Invert Rainbow: false 59 | Max Color: 255; 255; 255 60 | Max Intensity: 130 61 | Min Color: 0; 0; 0 62 | Min Intensity: 0 63 | Name: PointCloud2 64 | Position Transformer: XYZ 65 | Selectable: false 66 | Size (Pixels): 3 67 | Size (m): 0.009999999776482582 68 | Style: Flat Squares 69 | Topic: 70 | Depth: 5 71 | Durability Policy: Volatile 72 | Filter size: 10 73 | History Policy: Keep Last 74 | Reliability Policy: Reliable 75 | Value: /map 76 | Use Fixed Frame: true 77 | Use rainbow: true 78 | Value: true 79 | - Alpha: 1 80 | Axes Length: 1 81 | Axes Radius: 0.10000000149011612 82 | Class: rviz_default_plugins/Pose 83 | Color: 0; 255; 0 84 | Enabled: true 85 | Head Length: 0.30000001192092896 86 | Head Radius: 0.10000000149011612 87 | Name: Pose 88 | Shaft Length: 1 89 | Shaft Radius: 0.05000000074505806 90 | Shape: Arrow 91 | Topic: 92 | Depth: 5 93 | Durability Policy: Volatile 94 | Filter size: 10 95 | History Policy: Keep Last 96 | Reliability Policy: Reliable 97 | Value: /current_pose 98 | Value: true 99 | - Alpha: 1 100 | Buffer Length: 1 101 | Class: rviz_default_plugins/Path 102 | Color: 255; 255; 0 103 | Enabled: true 104 | Head Diameter: 0.30000001192092896 105 | Head Length: 0.20000000298023224 106 | Length: 0.30000001192092896 107 | Line Style: Lines 108 | Line Width: 0.029999999329447746 109 | Name: Path 110 | Offset: 111 | X: 0 112 | Y: 0 113 | Z: 0 114 | Pose Color: 255; 85; 255 115 | Pose Style: None 116 | Radius: 0.029999999329447746 117 | Shaft Diameter: 0.10000000149011612 118 | Shaft Length: 0.10000000149011612 119 | Topic: 120 | Depth: 5 121 | Durability Policy: Volatile 122 | Filter size: 10 123 | History Policy: Keep Last 124 | Reliability Policy: Reliable 125 | Value: /path 126 | Value: true 127 | - Alpha: 1 128 | Buffer Length: 1 129 | Class: rviz_default_plugins/Path 130 | Color: 25; 255; 0 131 | Enabled: true 132 | Head Diameter: 0.30000001192092896 133 | Head Length: 0.20000000298023224 134 | Length: 0.30000001192092896 135 | Line Style: Lines 136 | Line Width: 0.029999999329447746 137 | Name: Path 138 | Offset: 139 | X: 0 140 | Y: 0 141 | Z: 0 142 | Pose Color: 255; 85; 255 143 | Pose Style: None 144 | Radius: 0.029999999329447746 145 | Shaft Diameter: 0.10000000149011612 146 | Shaft Length: 0.10000000149011612 147 | Topic: 148 | Depth: 5 149 | Durability Policy: Volatile 150 | Filter size: 10 151 | History Policy: Keep Last 152 | Reliability Policy: Reliable 153 | Value: /imu_path 154 | Value: true 155 | - Alpha: 1 156 | Autocompute Intensity Bounds: true 157 | Autocompute Value Bounds: 158 | Max Value: 10 159 | Min Value: -10 160 | Value: true 161 | Axis: Z 162 | Channel Name: intensity 163 | Class: rviz_default_plugins/PointCloud2 164 | Color: 255; 255; 255 165 | Color Transformer: Intensity 166 | Decay Time: 0 167 | Enabled: true 168 | Invert Rainbow: false 169 | Max Color: 255; 255; 255 170 | Max Intensity: 255 171 | Min Color: 0; 0; 0 172 | Min Intensity: 0 173 | Name: PointCloud2 174 | Position Transformer: XYZ 175 | Selectable: true 176 | Size (Pixels): 3 177 | Size (m): 0.05000000074505806 178 | Style: Flat Squares 179 | Topic: 180 | Depth: 5 181 | Durability Policy: Volatile 182 | Filter size: 10 183 | History Policy: Keep Last 184 | Reliability Policy: Reliable 185 | Value: /modified_map 186 | Use Fixed Frame: true 187 | Use rainbow: true 188 | Value: true 189 | - Alpha: 1 190 | Buffer Length: 1 191 | Class: rviz_default_plugins/Path 192 | Color: 25; 255; 0 193 | Enabled: true 194 | Head Diameter: 0.30000001192092896 195 | Head Length: 0.20000000298023224 196 | Length: 0.30000001192092896 197 | Line Style: Lines 198 | Line Width: 0.029999999329447746 199 | Name: Path 200 | Offset: 201 | X: 0 202 | Y: 0 203 | Z: 0 204 | Pose Color: 255; 85; 255 205 | Pose Style: None 206 | Radius: 0.029999999329447746 207 | Shaft Diameter: 0.10000000149011612 208 | Shaft Length: 0.10000000149011612 209 | Topic: 210 | Depth: 5 211 | Durability Policy: Volatile 212 | Filter size: 10 213 | History Policy: Keep Last 214 | Reliability Policy: Reliable 215 | Value: /modified_path 216 | Value: true 217 | Enabled: true 218 | Global Options: 219 | Background Color: 48; 48; 48 220 | Fixed Frame: map 221 | Frame Rate: 30 222 | Name: root 223 | Tools: 224 | - Class: rviz_default_plugins/MoveCamera 225 | - Class: rviz_default_plugins/Select 226 | - Class: rviz_default_plugins/FocusCamera 227 | - Class: rviz_default_plugins/Measure 228 | Line color: 128; 128; 0 229 | - Class: rviz_default_plugins/SetInitialPose 230 | Covariance x: 0.25 231 | Covariance y: 0.25 232 | Covariance yaw: 0.06853891909122467 233 | Topic: 234 | Depth: 5 235 | Durability Policy: Volatile 236 | History Policy: Keep Last 237 | Reliability Policy: Reliable 238 | Value: /initialpose 239 | - Class: rviz_default_plugins/SetGoal 240 | Topic: 241 | Depth: 5 242 | Durability Policy: Volatile 243 | History Policy: Keep Last 244 | Reliability Policy: Reliable 245 | Value: /move_base_simple/goal 246 | - Class: rviz_default_plugins/PublishPoint 247 | Single click: true 248 | Topic: 249 | Depth: 5 250 | Durability Policy: Volatile 251 | History Policy: Keep Last 252 | Reliability Policy: Reliable 253 | Value: /clicked_point 254 | Transformation: 255 | Current: 256 | Class: rviz_default_plugins/TF 257 | Value: true 258 | Views: 259 | Current: 260 | Class: rviz_default_plugins/Orbit 261 | Distance: 109.79656219482422 262 | Enable Stereo Rendering: 263 | Stereo Eye Separation: 0.05999999865889549 264 | Stereo Focal Distance: 1 265 | Swap Stereo Eyes: false 266 | Value: false 267 | Focal Point: 268 | X: 20.250993728637695 269 | Y: -32.018898010253906 270 | Z: 58.781944274902344 271 | Focal Shape Fixed Size: true 272 | Focal Shape Size: 0.05000000074505806 273 | Invert Z Axis: false 274 | Name: Current View 275 | Near Clip Distance: 0.009999999776482582 276 | Pitch: 1.0397968292236328 277 | Target Frame: 278 | Value: Orbit (rviz) 279 | Yaw: 3.103959560394287 280 | Saved: ~ 281 | Window Geometry: 282 | Displays: 283 | collapsed: false 284 | Height: 842 285 | Hide Left Dock: false 286 | Hide Right Dock: true 287 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000035c000002f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 288 | Selection: 289 | collapsed: false 290 | Tool Properties: 291 | collapsed: false 292 | Views: 293 | collapsed: true 294 | Width: 1208 295 | X: 670 296 | Y: 80 297 | -------------------------------------------------------------------------------- /scanmatcher/rviz/lio_bigloop.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /PointCloud22 11 | Splitter Ratio: 0.5 12 | Tree Height: 613 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 10 30 | Class: rviz_default_plugins/Grid 31 | Color: 160; 160; 164 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 30 44 | Reference Frame: 45 | Value: true 46 | - Alpha: 1 47 | Autocompute Intensity Bounds: true 48 | Autocompute Value Bounds: 49 | Max Value: 10 50 | Min Value: -10 51 | Value: true 52 | Axis: Z 53 | Channel Name: intensity 54 | Class: rviz_default_plugins/PointCloud2 55 | Color: 255; 255; 255 56 | Color Transformer: Intensity 57 | Decay Time: 0 58 | Enabled: false 59 | Invert Rainbow: false 60 | Max Color: 255; 255; 255 61 | Max Intensity: 130 62 | Min Color: 0; 0; 0 63 | Min Intensity: 0 64 | Name: PointCloud2 65 | Position Transformer: XYZ 66 | Selectable: false 67 | Size (Pixels): 3 68 | Size (m): 0.009999999776482582 69 | Style: Flat Squares 70 | Topic: 71 | Depth: 5 72 | Durability Policy: Volatile 73 | History Policy: Keep Last 74 | Reliability Policy: Reliable 75 | Value: /map 76 | Use Fixed Frame: true 77 | Use rainbow: true 78 | Value: false 79 | - Alpha: 1 80 | Axes Length: 1 81 | Axes Radius: 0.10000000149011612 82 | Class: rviz_default_plugins/Pose 83 | Color: 0; 255; 0 84 | Enabled: true 85 | Head Length: 0.30000001192092896 86 | Head Radius: 0.10000000149011612 87 | Name: Pose 88 | Shaft Length: 1 89 | Shaft Radius: 0.05000000074505806 90 | Shape: Arrow 91 | Topic: 92 | Depth: 5 93 | Durability Policy: Volatile 94 | History Policy: Keep Last 95 | Reliability Policy: Reliable 96 | Value: /current_pose 97 | Value: true 98 | - Alpha: 1 99 | Buffer Length: 1 100 | Class: rviz_default_plugins/Path 101 | Color: 255; 255; 0 102 | Enabled: true 103 | Head Diameter: 0.30000001192092896 104 | Head Length: 0.20000000298023224 105 | Length: 0.30000001192092896 106 | Line Style: Lines 107 | Line Width: 0.029999999329447746 108 | Name: Path 109 | Offset: 110 | X: 0 111 | Y: 0 112 | Z: 0 113 | Pose Color: 255; 85; 255 114 | Pose Style: None 115 | Radius: 0.029999999329447746 116 | Shaft Diameter: 0.10000000149011612 117 | Shaft Length: 0.10000000149011612 118 | Topic: 119 | Depth: 5 120 | Durability Policy: Volatile 121 | History Policy: Keep Last 122 | Reliability Policy: Reliable 123 | Value: /path 124 | Value: true 125 | - Alpha: 1 126 | Buffer Length: 1 127 | Class: rviz_default_plugins/Path 128 | Color: 25; 255; 0 129 | Enabled: true 130 | Head Diameter: 0.30000001192092896 131 | Head Length: 0.20000000298023224 132 | Length: 0.30000001192092896 133 | Line Style: Lines 134 | Line Width: 0.029999999329447746 135 | Name: Path 136 | Offset: 137 | X: 0 138 | Y: 0 139 | Z: 0 140 | Pose Color: 255; 85; 255 141 | Pose Style: None 142 | Radius: 0.029999999329447746 143 | Shaft Diameter: 0.10000000149011612 144 | Shaft Length: 0.10000000149011612 145 | Topic: 146 | Depth: 5 147 | Durability Policy: Volatile 148 | History Policy: Keep Last 149 | Reliability Policy: Reliable 150 | Value: /imu_path 151 | Value: true 152 | - Alpha: 1 153 | Autocompute Intensity Bounds: true 154 | Autocompute Value Bounds: 155 | Max Value: 10 156 | Min Value: -10 157 | Value: true 158 | Axis: Z 159 | Channel Name: intensity 160 | Class: rviz_default_plugins/PointCloud2 161 | Color: 255; 255; 255 162 | Color Transformer: Intensity 163 | Decay Time: 0 164 | Enabled: false 165 | Invert Rainbow: false 166 | Max Color: 255; 255; 255 167 | Max Intensity: 255 168 | Min Color: 0; 0; 0 169 | Min Intensity: 0 170 | Name: PointCloud2 171 | Position Transformer: XYZ 172 | Selectable: true 173 | Size (Pixels): 3 174 | Size (m): 0.05000000074505806 175 | Style: Flat Squares 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /modified_map 182 | Use Fixed Frame: true 183 | Use rainbow: true 184 | Value: false 185 | - Alpha: 1 186 | Buffer Length: 1 187 | Class: rviz_default_plugins/Path 188 | Color: 25; 255; 0 189 | Enabled: true 190 | Head Diameter: 0.30000001192092896 191 | Head Length: 0.20000000298023224 192 | Length: 0.30000001192092896 193 | Line Style: Lines 194 | Line Width: 0.029999999329447746 195 | Name: Path 196 | Offset: 197 | X: 0 198 | Y: 0 199 | Z: 0 200 | Pose Color: 255; 85; 255 201 | Pose Style: None 202 | Radius: 0.029999999329447746 203 | Shaft Diameter: 0.10000000149011612 204 | Shaft Length: 0.10000000149011612 205 | Topic: 206 | Depth: 5 207 | Durability Policy: Volatile 208 | History Policy: Keep Last 209 | Reliability Policy: Reliable 210 | Value: /modified_path 211 | Value: true 212 | Enabled: true 213 | Global Options: 214 | Background Color: 48; 48; 48 215 | Fixed Frame: map 216 | Frame Rate: 30 217 | Name: root 218 | Tools: 219 | - Class: rviz_default_plugins/MoveCamera 220 | - Class: rviz_default_plugins/Select 221 | - Class: rviz_default_plugins/FocusCamera 222 | - Class: rviz_default_plugins/Measure 223 | Line color: 128; 128; 0 224 | - Class: rviz_default_plugins/SetInitialPose 225 | Topic: 226 | Depth: 5 227 | Durability Policy: Volatile 228 | History Policy: Keep Last 229 | Reliability Policy: Reliable 230 | Value: /initialpose 231 | - Class: rviz_default_plugins/SetGoal 232 | Topic: 233 | Depth: 5 234 | Durability Policy: Volatile 235 | History Policy: Keep Last 236 | Reliability Policy: Reliable 237 | Value: /move_base_simple/goal 238 | - Class: rviz_default_plugins/PublishPoint 239 | Single click: true 240 | Topic: 241 | Depth: 5 242 | Durability Policy: Volatile 243 | History Policy: Keep Last 244 | Reliability Policy: Reliable 245 | Value: /clicked_point 246 | Transformation: 247 | Current: 248 | Class: rviz_default_plugins/TF 249 | Value: true 250 | Views: 251 | Current: 252 | Class: rviz_default_plugins/Orbit 253 | Distance: 623.6841430664062 254 | Enable Stereo Rendering: 255 | Stereo Eye Separation: 0.05999999865889549 256 | Stereo Focal Distance: 1 257 | Swap Stereo Eyes: false 258 | Value: false 259 | Focal Point: 260 | X: 82.94154357910156 261 | Y: 71.35243225097656 262 | Z: 69.77112579345703 263 | Focal Shape Fixed Size: true 264 | Focal Shape Size: 0.05000000074505806 265 | Invert Z Axis: false 266 | Name: Current View 267 | Near Clip Distance: 0.009999999776482582 268 | Pitch: 1.5697963237762451 269 | Target Frame: 270 | Value: Orbit (rviz) 271 | Yaw: 3.108959436416626 272 | Saved: ~ 273 | Window Geometry: 274 | Displays: 275 | collapsed: false 276 | Height: 842 277 | Hide Left Dock: false 278 | Hide Right Dock: true 279 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000035c000002f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 280 | Selection: 281 | collapsed: false 282 | Tool Properties: 283 | collapsed: false 284 | Views: 285 | collapsed: true 286 | Width: 1208 287 | X: 670 288 | Y: 80 289 | -------------------------------------------------------------------------------- /scanmatcher/rviz/mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 613 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | Visualization Manager: 25 | Class: "" 26 | Displays: 27 | - Alpha: 0.5 28 | Cell Size: 10 29 | Class: rviz_default_plugins/Grid 30 | Color: 160; 160; 164 31 | Enabled: true 32 | Line Style: 33 | Line Width: 0.029999999329447746 34 | Value: Lines 35 | Name: Grid 36 | Normal Cell Count: 0 37 | Offset: 38 | X: 0 39 | Y: 0 40 | Z: 0 41 | Plane: XY 42 | Plane Cell Count: 25 43 | Reference Frame: 44 | Value: true 45 | - Alpha: 1 46 | Autocompute Intensity Bounds: true 47 | Autocompute Value Bounds: 48 | Max Value: 10 49 | Min Value: -10 50 | Value: true 51 | Axis: Z 52 | Channel Name: intensity 53 | Class: rviz_default_plugins/PointCloud2 54 | Color: 255; 255; 255 55 | Color Transformer: Intensity 56 | Decay Time: 0 57 | Enabled: true 58 | Invert Rainbow: false 59 | Max Color: 255; 255; 255 60 | Max Intensity: 255 61 | Min Color: 0; 0; 0 62 | Min Intensity: 4 63 | Name: PointCloud2 64 | Position Transformer: XYZ 65 | Queue Size: 10 66 | Selectable: false 67 | Size (Pixels): 3 68 | Size (m): 0.009999999776482582 69 | Style: Flat Squares 70 | Topic: /map 71 | Unreliable: false 72 | Use Fixed Frame: true 73 | Use rainbow: true 74 | Value: true 75 | - Alpha: 1 76 | Axes Length: 1 77 | Axes Radius: 0.10000000149011612 78 | Class: rviz_default_plugins/Pose 79 | Color: 0; 255; 0 80 | Enabled: true 81 | Head Length: 0.30000001192092896 82 | Head Radius: 0.10000000149011612 83 | Name: Pose 84 | Shaft Length: 1 85 | Shaft Radius: 0.05000000074505806 86 | Shape: Arrow 87 | Topic: /current_pose 88 | Unreliable: false 89 | Value: true 90 | - Alpha: 1 91 | Buffer Length: 1 92 | Class: rviz_default_plugins/Path 93 | Color: 255; 255; 0 94 | Enabled: true 95 | Head Diameter: 0.30000001192092896 96 | Head Length: 0.20000000298023224 97 | Length: 0.30000001192092896 98 | Line Style: Lines 99 | Line Width: 0.029999999329447746 100 | Name: Path 101 | Offset: 102 | X: 0 103 | Y: 0 104 | Z: 0 105 | Pose Color: 255; 85; 255 106 | Pose Style: None 107 | Radius: 0.029999999329447746 108 | Shaft Diameter: 0.10000000149011612 109 | Shaft Length: 0.10000000149011612 110 | Topic: /path 111 | Unreliable: false 112 | Value: true 113 | Enabled: true 114 | Global Options: 115 | Background Color: 48; 48; 48 116 | Fixed Frame: map 117 | Frame Rate: 30 118 | Name: root 119 | Tools: 120 | - Class: rviz_default_plugins/MoveCamera 121 | - Class: rviz_default_plugins/Select 122 | - Class: rviz_default_plugins/FocusCamera 123 | - Class: rviz_default_plugins/Measure 124 | Line color: 128; 128; 0 125 | - Class: rviz_default_plugins/SetInitialPose 126 | Topic: /initialpose 127 | - Class: rviz_default_plugins/SetGoal 128 | Topic: /move_base_simple/goal 129 | - Class: rviz_default_plugins/PublishPoint 130 | Single click: true 131 | Topic: /clicked_point 132 | Transformation: 133 | Current: 134 | Class: rviz_default_plugins/TF 135 | Value: true 136 | Views: 137 | Current: 138 | Class: rviz_default_plugins/Orbit 139 | Distance: 76.20225524902344 140 | Enable Stereo Rendering: 141 | Stereo Eye Separation: 0.05999999865889549 142 | Stereo Focal Distance: 1 143 | Swap Stereo Eyes: false 144 | Value: false 145 | Focal Point: 146 | X: 0.2535023093223572 147 | Y: -7.471435070037842 148 | Z: 2.8620872497558594 149 | Focal Shape Fixed Size: true 150 | Focal Shape Size: 0.05000000074505806 151 | Invert Z Axis: false 152 | Name: Current View 153 | Near Clip Distance: 0.009999999776482582 154 | Pitch: 0.2347976267337799 155 | Target Frame: 156 | Value: Orbit (rviz) 157 | Yaw: 1.3105117082595825 158 | Saved: ~ 159 | Window Geometry: 160 | Displays: 161 | collapsed: false 162 | Height: 842 163 | Hide Left Dock: false 164 | Hide Right Dock: false 165 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000024f000002f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 166 | Selection: 167 | collapsed: false 168 | Tool Properties: 169 | collapsed: false 170 | Views: 171 | collapsed: false 172 | Width: 1201 173 | X: 632 174 | Y: 79 175 | -------------------------------------------------------------------------------- /scanmatcher/src/imu_preintegration.cpp: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | // 3 | // Copyright (c) 2020, Tixiao Shan 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // * Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "scanmatcher/utility.h" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | 51 | using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 52 | using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) 53 | using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) 54 | 55 | using namespace std; 56 | 57 | 58 | // class IMUPreintegration : public ParamServer 59 | class IMUPreintegration : public ParamServer 60 | { 61 | public: 62 | rclcpp::Subscription::SharedPtr subImu; 63 | rclcpp::Subscription::SharedPtr subOdometry; 64 | rclcpp::Publisher::SharedPtr pubImuOdometry; 65 | rclcpp::Publisher::SharedPtr pubImuPath; 66 | 67 | // map -> odom 68 | tf2::Transform map_to_odom; 69 | //tf2_ros::TransformBroadcaster tfMap2Odom; 70 | // odom -> base_link 71 | //tf2_ros::TransformBroadcaster tfOdom2BaseLink; 72 | 73 | bool systemInitialized = false; 74 | 75 | gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; 76 | gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; 77 | gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; 78 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; 79 | gtsam::Vector noiseModelBetweenBias; 80 | 81 | 82 | gtsam::PreintegratedImuMeasurements * imuIntegratorOpt_; 83 | gtsam::PreintegratedImuMeasurements * imuIntegratorImu_; 84 | 85 | std::deque imuQueOpt; 86 | std::deque imuQueImu; 87 | 88 | gtsam::Pose3 prevPose_; 89 | gtsam::Vector3 prevVel_; 90 | gtsam::NavState prevState_; 91 | gtsam::imuBias::ConstantBias prevBias_; 92 | 93 | gtsam::NavState prevStateOdom; 94 | gtsam::imuBias::ConstantBias prevBiasOdom; 95 | 96 | bool doneFirstOpt = false; 97 | double lastImuT_imu = -1; 98 | double lastImuT_opt = -1; 99 | 100 | gtsam::ISAM2 optimizer; 101 | gtsam::NonlinearFactorGraph graphFactors; 102 | gtsam::Values graphValues; 103 | 104 | const double delta_t = 0; 105 | 106 | int key = 1; 107 | int imuPreintegrationResetId = 0; 108 | 109 | gtsam::Pose3 imu2Lidar = 110 | gtsam::Pose3( 111 | gtsam::Rot3(1, 0, 0, 0), 112 | gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z())); 113 | gtsam::Pose3 lidar2Imu = 114 | gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z())); 115 | 116 | rclcpp::Subscription::SharedPtr sub_; 117 | 118 | IMUPreintegration(const rclcpp::NodeOptions & options) 119 | : ParamServer("imu_preintegration", options) 120 | // tfMap2Odom(this), 121 | // tfOdom2BaseLink(this) 122 | { 123 | 124 | auto imu_callback = 125 | [this](const sensor_msgs::msg::Imu::ConstPtr msg) -> void 126 | { 127 | imuHandler(msg); 128 | }; 129 | auto odom_callback = 130 | [this](const nav_msgs::msg::Odometry::ConstPtr msg) -> void 131 | { 132 | odometryHandler(msg); 133 | }; 134 | subImu = create_subscription( 135 | imuTopic, rclcpp::SensorDataQoS(), imu_callback); 136 | subOdometry = create_subscription( 137 | "odometry", 5, odom_callback); 138 | 139 | pubImuOdometry = create_publisher(odomTopic, 2000); 140 | pubImuPath = create_publisher("imu_path", 1); 141 | 142 | 143 | // map_to_odom = tf2::Transform(tf2::createQuaternionFromRPY(0, 0, 0), tf2::Vector3(0, 0, 0)); 144 | Eigen::Quaterniond quat_eig = 145 | Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) * 146 | Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) * 147 | Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()); 148 | geometry_msgs::msg::Quaternion quat_msg = tf2::toMsg(quat_eig); 149 | tf2::Quaternion quat_tf; 150 | tf2::fromMsg(quat_msg, quat_tf); 151 | map_to_odom = tf2::Transform(quat_tf, tf2::Vector3(0, 0, 0)); 152 | 153 | boost::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU( 154 | imuGravity); 155 | p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // acc white noise in continuous 156 | p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2); // gyro white noise in continuous 157 | p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); // error committed in integrating position from velocities 158 | gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // assume zero initial bias 159 | 160 | priorPoseNoise = 161 | gtsam::noiseModel::Diagonal::Sigmas( 162 | (gtsam::Vector( 163 | 6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m 164 | priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s 165 | priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good 166 | correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-2); // meter 167 | noiseModelBetweenBias = 168 | (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, 169 | imuGyrBiasN).finished(); 170 | 171 | imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread 172 | imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization 173 | } 174 | 175 | void resetOptimization() 176 | { 177 | gtsam::ISAM2Params optParameters; 178 | optParameters.relinearizeThreshold = 0.1; 179 | optParameters.relinearizeSkip = 1; 180 | optimizer = gtsam::ISAM2(optParameters); 181 | 182 | gtsam::NonlinearFactorGraph newGraphFactors; 183 | graphFactors = newGraphFactors; 184 | 185 | gtsam::Values NewGraphValues; 186 | graphValues = NewGraphValues; 187 | } 188 | 189 | void resetParams() 190 | { 191 | lastImuT_imu = -1; 192 | doneFirstOpt = false; 193 | systemInitialized = false; 194 | } 195 | 196 | void odometryHandler(const nav_msgs::msg::Odometry::ConstPtr & odomMsg) 197 | { 198 | double currentCorrectionTime = ROS_TIME(odomMsg); 199 | 200 | // make sure we have imu data to integrate 201 | if (imuQueOpt.empty()) { 202 | return; 203 | } 204 | 205 | float p_x = odomMsg->pose.pose.position.x; 206 | float p_y = odomMsg->pose.pose.position.y; 207 | float p_z = odomMsg->pose.pose.position.z; 208 | float r_x = odomMsg->pose.pose.orientation.x; 209 | float r_y = odomMsg->pose.pose.orientation.y; 210 | float r_z = odomMsg->pose.pose.orientation.z; 211 | float r_w = odomMsg->pose.pose.orientation.w; 212 | int currentResetId = round(odomMsg->pose.covariance[0]); 213 | gtsam::Pose3 lidarPose = gtsam::Pose3( 214 | gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3( 215 | p_x, p_y, p_z)); 216 | 217 | // correction pose jumped, reset imu pre-integration 218 | if (currentResetId != imuPreintegrationResetId) { 219 | resetParams(); 220 | imuPreintegrationResetId = currentResetId; 221 | } 222 | 223 | 224 | // 0. initialize system 225 | if (systemInitialized == false) { 226 | resetOptimization(); 227 | 228 | // pop old IMU message 229 | while (!imuQueOpt.empty()) { 230 | if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t) { 231 | lastImuT_opt = ROS_TIME(&imuQueOpt.front()); 232 | imuQueOpt.pop_front(); 233 | } else { 234 | break; 235 | } 236 | } 237 | // initial pose 238 | prevPose_ = lidarPose.compose(lidar2Imu); 239 | gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise); 240 | graphFactors.add(priorPose); 241 | // initial velocity 242 | prevVel_ = gtsam::Vector3(0, 0, 0); 243 | gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise); 244 | graphFactors.add(priorVel); 245 | // initial bias 246 | prevBias_ = gtsam::imuBias::ConstantBias(); 247 | gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise); 248 | graphFactors.add(priorBias); 249 | // add values 250 | graphValues.insert(X(0), prevPose_); 251 | graphValues.insert(V(0), prevVel_); 252 | graphValues.insert(B(0), prevBias_); 253 | // optimize once 254 | optimizer.update(graphFactors, graphValues); 255 | graphFactors.resize(0); 256 | graphValues.clear(); 257 | 258 | imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); 259 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); 260 | 261 | key = 1; 262 | systemInitialized = true; 263 | return; 264 | } 265 | 266 | 267 | // reset graph for speed 268 | if (key == 100) { 269 | // get updated noise before reset 270 | gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = 271 | gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key - 1))); 272 | gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = 273 | gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key - 1))); 274 | gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = 275 | gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key - 1))); 276 | // reset graph 277 | resetOptimization(); 278 | // add pose 279 | gtsam::PriorFactor priorPose(X(0), prevPose_, updatedPoseNoise); 280 | graphFactors.add(priorPose); 281 | // add velocity 282 | gtsam::PriorFactor priorVel(V(0), prevVel_, updatedVelNoise); 283 | graphFactors.add(priorVel); 284 | // add bias 285 | gtsam::PriorFactor priorBias(B(0), prevBias_, updatedBiasNoise); 286 | graphFactors.add(priorBias); 287 | // add values 288 | graphValues.insert(X(0), prevPose_); 289 | graphValues.insert(V(0), prevVel_); 290 | graphValues.insert(B(0), prevBias_); 291 | // optimize once 292 | optimizer.update(graphFactors, graphValues); 293 | graphFactors.resize(0); 294 | graphValues.clear(); 295 | 296 | key = 1; 297 | } 298 | 299 | 300 | // 1. integrate imu data and optimize 301 | while (!imuQueOpt.empty()) { 302 | // pop and integrate imu data that is between two optimizations 303 | sensor_msgs::msg::Imu * thisImu = &imuQueOpt.front(); 304 | double imuTime = ROS_TIME(thisImu); 305 | if (imuTime < currentCorrectionTime - delta_t) { 306 | double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt); 307 | imuIntegratorOpt_->integrateMeasurement( 308 | gtsam::Vector3( 309 | thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, 310 | thisImu->linear_acceleration.z), 311 | gtsam::Vector3( 312 | thisImu->angular_velocity.x, thisImu->angular_velocity.y, 313 | thisImu->angular_velocity.z), dt); 314 | 315 | lastImuT_opt = imuTime; 316 | imuQueOpt.pop_front(); 317 | } else { 318 | break; 319 | } 320 | } 321 | // add imu factor to graph 322 | const gtsam::PreintegratedImuMeasurements & preint_imu = 323 | dynamic_cast(*imuIntegratorOpt_); 324 | gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu); 325 | graphFactors.add(imu_factor); 326 | // add imu bias between factor 327 | graphFactors.add( 328 | gtsam::BetweenFactor( 329 | B(key - 1), B(key), gtsam::imuBias::ConstantBias(), 330 | gtsam::noiseModel::Diagonal::Sigmas( 331 | sqrt(imuIntegratorOpt_->deltaTij()) * 332 | noiseModelBetweenBias))); 333 | // add pose factor 334 | gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu); 335 | gtsam::PriorFactor pose_factor(X(key), curPose, correctionNoise); 336 | graphFactors.add(pose_factor); 337 | // insert predicted values 338 | gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); 339 | graphValues.insert(X(key), propState_.pose()); 340 | graphValues.insert(V(key), propState_.v()); 341 | graphValues.insert(B(key), prevBias_); 342 | // optimize 343 | optimizer.update(graphFactors, graphValues); 344 | optimizer.update(); 345 | graphFactors.resize(0); 346 | graphValues.clear(); 347 | // Overwrite the beginning of the preintegration for the next step. 348 | gtsam::Values result = optimizer.calculateEstimate(); 349 | prevPose_ = result.at(X(key)); 350 | prevVel_ = result.at(V(key)); 351 | prevState_ = gtsam::NavState(prevPose_, prevVel_); 352 | prevBias_ = result.at(B(key)); 353 | // Reset the optimization preintegration object. 354 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); 355 | // check optimization 356 | if (failureDetection(prevVel_, prevBias_)) { 357 | resetParams(); 358 | return; 359 | } 360 | 361 | 362 | // 2. after optiization, re-propagate imu odometry preintegration 363 | prevStateOdom = prevState_; 364 | prevBiasOdom = prevBias_; 365 | // first pop imu message older than current correction data 366 | double lastImuQT = -1; 367 | while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t) { 368 | lastImuQT = ROS_TIME(&imuQueImu.front()); 369 | imuQueImu.pop_front(); 370 | } 371 | // repropogate 372 | if (!imuQueImu.empty()) { 373 | // reset bias use the newly optimized bias 374 | imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom); 375 | // integrate imu message from the beginning of this optimization 376 | for (int i = 0; i < (int)imuQueImu.size(); ++i) { 377 | sensor_msgs::msg::Imu * thisImu = &imuQueImu[i]; 378 | double imuTime = ROS_TIME(thisImu); 379 | double dt = (lastImuQT < 0) ? (1.0 / 500.0) : (imuTime - lastImuQT); 380 | 381 | imuIntegratorImu_->integrateMeasurement( 382 | gtsam::Vector3( 383 | thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, 384 | thisImu->linear_acceleration.z), 385 | gtsam::Vector3( 386 | thisImu->angular_velocity.x, thisImu->angular_velocity.y, 387 | thisImu->angular_velocity.z), dt); 388 | lastImuQT = imuTime; 389 | } 390 | } 391 | 392 | ++key; 393 | doneFirstOpt = true; 394 | } 395 | 396 | bool failureDetection(const gtsam::Vector3 & velCur, const gtsam::imuBias::ConstantBias & biasCur) 397 | { 398 | Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z()); 399 | if (vel.norm() > 10) { 400 | RCLCPP_WARN(get_logger(), "Large velocity, reset IMU-preintegration!"); 401 | return true; 402 | } 403 | 404 | Eigen::Vector3f ba(biasCur.accelerometer().x(), 405 | biasCur.accelerometer().y(), biasCur.accelerometer().z()); 406 | Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z()); 407 | if (ba.norm() > 0.1 || bg.norm() > 0.1) { 408 | RCLCPP_WARN(get_logger(), "Large bias, reset IMU-preintegration!"); 409 | return true; 410 | } 411 | 412 | return false; 413 | } 414 | 415 | void imuHandler(const sensor_msgs::msg::Imu::ConstPtr & imu_raw) 416 | { 417 | sensor_msgs::msg::Imu thisImu = imuConverter(*imu_raw); 418 | // publish static tf 419 | // tfMap2Odom.sendTransform(tf2::StampedTransform(map_to_odom, thisImu.header.stamp, "map", "odom")); 420 | geometry_msgs::msg::Pose pose; 421 | tf2::toMsg(map_to_odom, pose); 422 | geometry_msgs::msg::TransformStamped trans_stamped; 423 | trans_stamped.header.stamp = thisImu.header.stamp; 424 | trans_stamped.header.frame_id = "map"; 425 | trans_stamped.child_frame_id = "odom"; 426 | trans_stamped.transform.translation.x = pose.position.x; 427 | trans_stamped.transform.translation.y = pose.position.y; 428 | trans_stamped.transform.translation.z = pose.position.z; 429 | trans_stamped.transform.rotation = pose.orientation; 430 | //tfMap2Odom.sendTransform(trans_stamped); 431 | 432 | imuQueOpt.push_back(thisImu); 433 | imuQueImu.push_back(thisImu); 434 | 435 | if (doneFirstOpt == false) { 436 | return; 437 | } 438 | 439 | double imuTime = ROS_TIME(&thisImu); 440 | double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu); 441 | lastImuT_imu = imuTime; 442 | 443 | // integrate this single imu message 444 | imuIntegratorImu_->integrateMeasurement( 445 | gtsam::Vector3( 446 | thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, 447 | thisImu.linear_acceleration.z), 448 | gtsam::Vector3( 449 | thisImu.angular_velocity.x, thisImu.angular_velocity.y, 450 | thisImu.angular_velocity.z), dt); 451 | 452 | // predict odometry 453 | gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 454 | 455 | // publish odometry 456 | nav_msgs::msg::Odometry odometry; 457 | odometry.header.stamp = thisImu.header.stamp; 458 | odometry.header.frame_id = "odom"; 459 | odometry.child_frame_id = "odom_imu"; 460 | 461 | // transform imu pose to ldiar 462 | gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position()); 463 | gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar); 464 | 465 | odometry.pose.pose.position.x = lidarPose.translation().x(); 466 | odometry.pose.pose.position.y = lidarPose.translation().y(); 467 | odometry.pose.pose.position.z = lidarPose.translation().z(); 468 | odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x(); 469 | odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y(); 470 | odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z(); 471 | odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w(); 472 | 473 | odometry.twist.twist.linear.x = currentState.velocity().x(); 474 | odometry.twist.twist.linear.y = currentState.velocity().y(); 475 | odometry.twist.twist.linear.z = currentState.velocity().z(); 476 | odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x(); 477 | odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y(); 478 | odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z(); 479 | odometry.pose.covariance[0] = double(imuPreintegrationResetId); 480 | pubImuOdometry->publish(odometry); 481 | 482 | // publish imu path 483 | static nav_msgs::msg::Path imuPath; 484 | static double last_path_time = -1; 485 | if (imuTime - last_path_time > 0.1) { 486 | last_path_time = imuTime; 487 | geometry_msgs::msg::PoseStamped pose_stamped; 488 | pose_stamped.header.stamp = thisImu.header.stamp; 489 | pose_stamped.header.frame_id = "odom"; 490 | pose_stamped.pose = odometry.pose.pose; 491 | imuPath.poses.push_back(pose_stamped); 492 | double t1 = imuPath.poses.front().header.stamp.sec + 493 | imuPath.poses.front().header.stamp.nanosec * 1e-9; 494 | double t2 = imuPath.poses.back().header.stamp.sec + 495 | imuPath.poses.back().header.stamp.nanosec * 1e-9; 496 | while (!imuPath.poses.empty() && 497 | // abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0) 498 | //abs(ROS_TIME(imuPath.poses.front()) - ROS_TIME(imuPath.poses.back())) > 3.0) 499 | abs(t1 - t2) > 3.0) 500 | { 501 | imuPath.poses.erase(imuPath.poses.begin()); 502 | } 503 | // if (pubImuPath.getNumSubscribers() != 0) 504 | { 505 | imuPath.header.stamp = thisImu.header.stamp; 506 | // imuPath.header.frame_id = "odom"; 507 | imuPath.header.frame_id = "map"; 508 | pubImuPath->publish(imuPath); 509 | } 510 | } 511 | 512 | // publish transformation 513 | tf2::Transform tCur; 514 | geometry_msgs::msg::TransformStamped odom_2_baselink; 515 | odom_2_baselink.header.stamp = thisImu.header.stamp; 516 | odom_2_baselink.header.frame_id = "odom"; 517 | odom_2_baselink.child_frame_id = "base_link"; 518 | odom_2_baselink.transform.translation.x = odometry.pose.pose.position.x; 519 | odom_2_baselink.transform.translation.y = odometry.pose.pose.position.y; 520 | odom_2_baselink.transform.translation.z = odometry.pose.pose.position.z; 521 | odom_2_baselink.transform.rotation = odometry.pose.pose.orientation; 522 | //tfOdom2BaseLink.sendTransform(odom_2_baselink); 523 | 524 | } 525 | 526 | 527 | }; 528 | 529 | 530 | int main(int argc, char ** argv) 531 | { 532 | rclcpp::init(argc, argv); 533 | rclcpp::NodeOptions options; 534 | options.use_intra_process_comms(true); 535 | rclcpp::executors::SingleThreadedExecutor exec; 536 | 537 | auto imu_pre = std::make_shared(options); 538 | exec.add_node(imu_pre); 539 | 540 | cout << "\033[1;32m----> IMU Preintegration Started.\033[0m" << endl; 541 | 542 | exec.spin(); 543 | rclcpp::shutdown(); 544 | 545 | return 0; 546 | } 547 | -------------------------------------------------------------------------------- /scanmatcher/src/scanmatcher_component.cpp: -------------------------------------------------------------------------------- 1 | #include "scanmatcher/scanmatcher_component.h" 2 | 3 | #include 4 | 5 | using namespace std::chrono_literals; 6 | 7 | namespace graphslam 8 | { 9 | ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options) 10 | : Node("scan_matcher", options), 11 | clock_(RCL_ROS_TIME), 12 | tfbuffer_(std::make_shared(clock_)), 13 | listener_(tfbuffer_), 14 | broadcaster_(this) 15 | { 16 | std::string registration_method; 17 | double ndt_resolution; 18 | int ndt_num_threads; 19 | double gicp_corr_dist_threshold; 20 | 21 | declare_parameter("global_frame_id", "map"); 22 | get_parameter("global_frame_id", global_frame_id_); 23 | declare_parameter("robot_frame_id", "base_link"); 24 | get_parameter("robot_frame_id", robot_frame_id_); 25 | declare_parameter("odom_frame_id", "odom"); 26 | get_parameter("odom_frame_id", odom_frame_id_); 27 | declare_parameter("registration_method", "NDT"); 28 | get_parameter("registration_method", registration_method); 29 | declare_parameter("ndt_resolution", 5.0); 30 | get_parameter("ndt_resolution", ndt_resolution); 31 | declare_parameter("ndt_num_threads", 0); 32 | get_parameter("ndt_num_threads", ndt_num_threads); 33 | declare_parameter("gicp_corr_dist_threshold", 5.0); 34 | get_parameter("gicp_corr_dist_threshold", gicp_corr_dist_threshold); 35 | declare_parameter("trans_for_mapupdate", 1.5); 36 | get_parameter("trans_for_mapupdate", trans_for_mapupdate_); 37 | declare_parameter("vg_size_for_input", 0.2); 38 | get_parameter("vg_size_for_input", vg_size_for_input_); 39 | declare_parameter("vg_size_for_map", 0.1); 40 | get_parameter("vg_size_for_map", vg_size_for_map_); 41 | declare_parameter("use_min_max_filter", false); 42 | get_parameter("use_min_max_filter", use_min_max_filter_); 43 | declare_parameter("scan_min_range", 0.1); 44 | get_parameter("scan_min_range", scan_min_range_); 45 | declare_parameter("scan_max_range", 100.0); 46 | get_parameter("scan_max_range", scan_max_range_); 47 | declare_parameter("map_publish_period", 15.0); 48 | get_parameter("map_publish_period", map_publish_period_); 49 | declare_parameter("num_targeted_cloud", 10); 50 | get_parameter("num_targeted_cloud", num_targeted_cloud_); 51 | if (num_targeted_cloud_ < 1) { 52 | std::cout << "num_tareged_cloud should be positive" << std::endl; 53 | num_targeted_cloud_ = 1; 54 | } 55 | 56 | declare_parameter("initial_pose_x", 0.0); 57 | get_parameter("initial_pose_x", initial_pose_x_); 58 | declare_parameter("initial_pose_y", 0.0); 59 | get_parameter("initial_pose_y", initial_pose_y_); 60 | declare_parameter("initial_pose_z", 0.0); 61 | get_parameter("initial_pose_z", initial_pose_z_); 62 | declare_parameter("initial_pose_qx", 0.0); 63 | get_parameter("initial_pose_qx", initial_pose_qx_); 64 | declare_parameter("initial_pose_qy", 0.0); 65 | get_parameter("initial_pose_qy", initial_pose_qy_); 66 | declare_parameter("initial_pose_qz", 0.0); 67 | get_parameter("initial_pose_qz", initial_pose_qz_); 68 | declare_parameter("initial_pose_qw", 1.0); 69 | get_parameter("initial_pose_qw", initial_pose_qw_); 70 | 71 | declare_parameter("set_initial_pose", false); 72 | get_parameter("set_initial_pose", set_initial_pose_); 73 | declare_parameter("use_odom", false); 74 | get_parameter("use_odom", use_odom_); 75 | declare_parameter("use_imu", false); 76 | get_parameter("use_imu", use_imu_); 77 | declare_parameter("debug_flag", false); 78 | get_parameter("debug_flag", debug_flag_); 79 | 80 | std::cout << "registration_method:" << registration_method << std::endl; 81 | std::cout << "ndt_resolution[m]:" << ndt_resolution << std::endl; 82 | std::cout << "ndt_num_threads:" << ndt_num_threads << std::endl; 83 | std::cout << "gicp_corr_dist_threshold[m]:" << gicp_corr_dist_threshold << std::endl; 84 | std::cout << "trans_for_mapupdate[m]:" << trans_for_mapupdate_ << std::endl; 85 | std::cout << "vg_size_for_input[m]:" << vg_size_for_input_ << std::endl; 86 | std::cout << "vg_size_for_map[m]:" << vg_size_for_map_ << std::endl; 87 | std::cout << "use_min_max_filter:" << std::boolalpha << use_min_max_filter_ << std::endl; 88 | std::cout << "scan_min_range[m]:" << scan_min_range_ << std::endl; 89 | std::cout << "scan_max_range[m]:" << scan_max_range_ << std::endl; 90 | std::cout << "set_initial_pose:" << std::boolalpha << set_initial_pose_ << std::endl; 91 | std::cout << "use_odom:" << std::boolalpha << use_odom_ << std::endl; 92 | std::cout << "use_imu:" << std::boolalpha << use_imu_ << std::endl; 93 | std::cout << "debug_flag:" << std::boolalpha << debug_flag_ << std::endl; 94 | std::cout << "map_publish_period[sec]:" << map_publish_period_ << std::endl; 95 | std::cout << "num_targeted_cloud:" << num_targeted_cloud_ << std::endl; 96 | std::cout << "------------------" << std::endl; 97 | 98 | if (registration_method == "NDT") { 99 | 100 | pclomp::NormalDistributionsTransform::Ptr 101 | ndt(new pclomp::NormalDistributionsTransform()); 102 | ndt->setResolution(ndt_resolution); 103 | ndt->setTransformationEpsilon(0.01); 104 | // ndt_omp 105 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); 106 | if (ndt_num_threads > 0) {ndt->setNumThreads(ndt_num_threads);} 107 | 108 | registration_ = ndt; 109 | 110 | } else if (registration_method == "GICP") { 111 | boost::shared_ptr> 112 | gicp(new pclomp::GeneralizedIterativeClosestPoint()); 113 | gicp->setMaxCorrespondenceDistance(gicp_corr_dist_threshold); 114 | //gicp->setCorrespondenceRandomness(20); 115 | gicp->setTransformationEpsilon(1e-8); 116 | registration_ = gicp; 117 | } else { 118 | RCLCPP_ERROR(get_logger(), "invalid registration method"); 119 | exit(1); 120 | } 121 | 122 | map_array_msg_.header.frame_id = global_frame_id_; 123 | map_array_msg_.cloud_coordinate = map_array_msg_.LOCAL; 124 | 125 | path_.header.frame_id = global_frame_id_; 126 | 127 | initializePubSub(); 128 | 129 | if (set_initial_pose_) { 130 | auto msg = std::make_shared(); 131 | msg->header.stamp = now(); 132 | msg->header.frame_id = global_frame_id_; 133 | msg->pose.position.x = initial_pose_x_; 134 | msg->pose.position.y = initial_pose_y_; 135 | msg->pose.position.z = initial_pose_z_; 136 | msg->pose.orientation.x = initial_pose_qx_; 137 | msg->pose.orientation.y = initial_pose_qy_; 138 | msg->pose.orientation.z = initial_pose_qz_; 139 | msg->pose.orientation.w = initial_pose_qw_; 140 | corrent_pose_stamped_ = *msg; 141 | pose_pub_->publish(corrent_pose_stamped_); 142 | initial_pose_received_ = true; 143 | 144 | path_.poses.push_back(*msg); 145 | } 146 | 147 | RCLCPP_INFO(get_logger(), "initialization end"); 148 | } 149 | 150 | void ScanMatcherComponent::initializePubSub() 151 | { 152 | RCLCPP_INFO(get_logger(), "initialize Publishers and Subscribers"); 153 | // sub 154 | auto initial_pose_callback = 155 | [this](const typename geometry_msgs::msg::PoseStamped::SharedPtr msg) -> void 156 | { 157 | if (msg->header.frame_id != global_frame_id_) { 158 | RCLCPP_WARN(get_logger(), "This initial_pose is not in the global frame"); 159 | return; 160 | } 161 | RCLCPP_INFO(get_logger(), "initial_pose is received"); 162 | 163 | corrent_pose_stamped_ = *msg; 164 | previous_position_.x() = corrent_pose_stamped_.pose.position.x; 165 | previous_position_.y() = corrent_pose_stamped_.pose.position.y; 166 | previous_position_.z() = corrent_pose_stamped_.pose.position.z; 167 | initial_pose_received_ = true; 168 | 169 | pose_pub_->publish(corrent_pose_stamped_); 170 | }; 171 | 172 | auto cloud_callback = 173 | [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void 174 | { 175 | if (!initial_pose_received_) 176 | { 177 | RCLCPP_WARN(get_logger(), "initial_pose is not received"); 178 | return; 179 | } 180 | sensor_msgs::msg::PointCloud2 transformed_msg; 181 | try { 182 | tf2::TimePoint time_point = tf2::TimePoint( 183 | std::chrono::seconds(msg->header.stamp.sec) + 184 | std::chrono::nanoseconds(msg->header.stamp.nanosec)); 185 | const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform( 186 | robot_frame_id_, msg->header.frame_id, time_point); 187 | tf2::doTransform(*msg, transformed_msg, transform); // TODO:slow now(https://github.com/ros/geometry2/pull/432) 188 | } catch (tf2::TransformException & e) { 189 | RCLCPP_ERROR(this->get_logger(), "%s", e.what()); 190 | return; 191 | } 192 | 193 | pcl::PointCloud::Ptr tmp_ptr(new pcl::PointCloud()); 194 | pcl::fromROSMsg(transformed_msg, *tmp_ptr); 195 | 196 | if (use_min_max_filter_) { 197 | double r; 198 | pcl::PointCloud tmp; 199 | for (const auto & p : tmp_ptr->points) { 200 | r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0)); 201 | if (scan_min_range_ < r && r < scan_max_range_) {tmp.push_back(p);} 202 | } 203 | } 204 | 205 | if (!initial_cloud_received_) { 206 | RCLCPP_INFO(get_logger(), "initial_cloud is received"); 207 | initial_cloud_received_ = true; 208 | initializeMap(tmp_ptr, msg->header); 209 | last_map_time_ = clock_.now(); 210 | } 211 | 212 | if (initial_cloud_received_) {receiveCloud(tmp_ptr, msg->header.stamp);} 213 | 214 | }; 215 | 216 | 217 | auto odom_callback = 218 | [this](const typename nav_msgs::msg::Odometry::SharedPtr msg) -> void 219 | { 220 | if (initial_pose_received_) {receiveOdom(*msg);} 221 | }; 222 | 223 | 224 | initial_pose_sub_ = 225 | create_subscription( 226 | "initial_pose", rclcpp::QoS(10), initial_pose_callback); 227 | 228 | odom_sub_ = 229 | create_subscription( 230 | "preintegrated_odom", rclcpp::SensorDataQoS(), odom_callback); 231 | 232 | input_cloud_sub_ = 233 | create_subscription( 234 | "input_cloud", rclcpp::SensorDataQoS(), cloud_callback); 235 | 236 | // pub 237 | pose_pub_ = create_publisher( 238 | "current_pose", 239 | rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); 240 | map_pub_ = create_publisher( 241 | "map", rclcpp::QoS( 242 | rclcpp::KeepLast( 243 | 1)).reliable()); 244 | map_array_pub_ = 245 | create_publisher( 246 | "map_array", rclcpp::QoS( 247 | rclcpp::KeepLast( 248 | 1)).reliable()); 249 | path_pub_ = create_publisher("path", 1); 250 | odom_pub_ = create_publisher( 251 | "odom", rclcpp::QoS( 252 | rclcpp::KeepLast( 253 | 1)).reliable()); 254 | } 255 | 256 | void ScanMatcherComponent::initializeMap(const pcl::PointCloud ::Ptr & tmp_ptr, const std_msgs::msg::Header & header) 257 | { 258 | RCLCPP_INFO(get_logger(), "create a first map"); 259 | pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); 260 | pcl::VoxelGrid voxel_grid; 261 | voxel_grid.setLeafSize(vg_size_for_map_, vg_size_for_map_, vg_size_for_map_); 262 | voxel_grid.setInputCloud(tmp_ptr); 263 | voxel_grid.filter(*cloud_ptr); 264 | 265 | Eigen::Matrix4f sim_trans = getTransformation(corrent_pose_stamped_.pose); 266 | pcl::PointCloud::Ptr transformed_cloud_ptr( 267 | new pcl::PointCloud()); 268 | pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, sim_trans); 269 | registration_->setInputTarget(transformed_cloud_ptr); 270 | 271 | // map 272 | sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2); 273 | pcl::toROSMsg(*transformed_cloud_ptr, *map_msg_ptr); 274 | 275 | // map array 276 | sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr( 277 | new sensor_msgs::msg::PointCloud2); 278 | pcl::toROSMsg(*cloud_ptr, *cloud_msg_ptr); 279 | lidarslam_msgs::msg::SubMap submap; 280 | submap.header = header; 281 | submap.distance = 0; 282 | submap.pose = corrent_pose_stamped_.pose; 283 | submap.cloud = *cloud_msg_ptr; 284 | map_array_msg_.header = header; 285 | map_array_msg_.submaps.push_back(submap); 286 | 287 | map_pub_->publish(submap.cloud); 288 | } 289 | 290 | void ScanMatcherComponent::receiveCloud( 291 | const pcl::PointCloud::ConstPtr & cloud_ptr, 292 | const rclcpp::Time stamp) 293 | { 294 | if (mapping_flag_ && mapping_future_.valid()) { 295 | auto status = mapping_future_.wait_for(0s); 296 | if (status == std::future_status::ready) { 297 | if (is_map_updated_ == true) { 298 | pcl::PointCloud::Ptr targeted_cloud_ptr(new pcl::PointCloud( 299 | targeted_cloud_)); 300 | registration_->setInputTarget(targeted_cloud_ptr); 301 | is_map_updated_ = false; 302 | } 303 | mapping_flag_ = false; 304 | mapping_thread_.detach(); 305 | } 306 | } 307 | 308 | pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); 309 | pcl::VoxelGrid voxel_grid; 310 | voxel_grid.setLeafSize(vg_size_for_input_, vg_size_for_input_, vg_size_for_input_); 311 | voxel_grid.setInputCloud(cloud_ptr); 312 | voxel_grid.filter(*filtered_cloud_ptr); 313 | registration_->setInputSource(filtered_cloud_ptr); 314 | 315 | Eigen::Matrix4f sim_trans = getTransformation(corrent_pose_stamped_.pose); 316 | 317 | if (use_odom_) { 318 | geometry_msgs::msg::TransformStamped odom_trans; 319 | try { 320 | odom_trans = tfbuffer_.lookupTransform( 321 | odom_frame_id_, robot_frame_id_, tf2_ros::fromMsg( 322 | stamp)); 323 | } catch (tf2::TransformException & e) { 324 | RCLCPP_ERROR(this->get_logger(), "%s", e.what()); 325 | } 326 | Eigen::Affine3d odom_affine = tf2::transformToEigen(odom_trans); 327 | Eigen::Matrix4f odom_mat = odom_affine.matrix().cast(); 328 | if (previous_odom_mat_ != Eigen::Matrix4f::Identity()) { 329 | sim_trans = sim_trans * previous_odom_mat_.inverse() * odom_mat; 330 | } 331 | previous_odom_mat_ = odom_mat; 332 | } 333 | 334 | if (use_imu_ && odom_ptr_last_ != -1) { 335 | int odom_ptr = odom_ptr_front_; 336 | while (odom_ptr != odom_ptr_last_) { 337 | rclcpp::Time odom_stamp = odom_que_[odom_ptr].header.stamp; 338 | if (odom_stamp.nanoseconds() > stamp.nanoseconds()) {break;} 339 | odom_ptr = (odom_ptr + 1) % odom_que_length_; 340 | } 341 | Eigen::Matrix4f odom_position = getTransformation(odom_que_[odom_ptr].pose.pose); 342 | sim_trans = odom_position; 343 | odom_ptr_front_ = odom_ptr; 344 | } 345 | 346 | pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); 347 | rclcpp::Clock system_clock; 348 | rclcpp::Time time_align_start = system_clock.now(); 349 | registration_->align(*output_cloud, sim_trans); 350 | rclcpp::Time time_align_end = system_clock.now(); 351 | 352 | Eigen::Matrix4f final_transformation = registration_->getFinalTransformation(); 353 | 354 | publishMapAndPose(cloud_ptr, final_transformation, stamp); 355 | 356 | if (!debug_flag_) {return;} 357 | 358 | tf2::Quaternion quat_tf; 359 | double roll, pitch, yaw; 360 | tf2::fromMsg(corrent_pose_stamped_.pose.orientation, quat_tf); 361 | tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw); 362 | 363 | std::cout << "---------------------------------------------------------" << std::endl; 364 | std::cout << "nanoseconds: " << stamp.nanoseconds() << std::endl; 365 | std::cout << "trans: " << trans_ << std::endl; 366 | std::cout << "align time:" << time_align_end.seconds() - time_align_start.seconds() << "s" << 367 | std::endl; 368 | std::cout << "number of filtered cloud points: " << filtered_cloud_ptr->size() << std::endl; 369 | std::cout << "initial transformation:" << std::endl; 370 | std::cout << sim_trans << std::endl; 371 | std::cout << "has converged: " << registration_->hasConverged() << std::endl; 372 | std::cout << "fitness score: " << registration_->getFitnessScore() << std::endl; 373 | std::cout << "final transformation:" << std::endl; 374 | std::cout << final_transformation << std::endl; 375 | std::cout << "rpy" << std::endl; 376 | std::cout << "roll:" << roll * 180 / M_PI << "," << 377 | "pitch:" << pitch * 180 / M_PI << "," << 378 | "yaw:" << yaw * 180 / M_PI << std::endl; 379 | int num_submaps = map_array_msg_.submaps.size(); 380 | std::cout << "num_submaps:" << num_submaps << std::endl; 381 | std::cout << "moving distance:" << latest_distance_ << std::endl; 382 | std::cout << "---------------------------------------------------------" << std::endl; 383 | } 384 | 385 | void ScanMatcherComponent::publishMapAndPose( 386 | const pcl::PointCloud::ConstPtr & cloud_ptr, 387 | const Eigen::Matrix4f final_transformation, const rclcpp::Time stamp) 388 | { 389 | 390 | Eigen::Vector3d position = final_transformation.block<3, 1>(0, 3).cast(); 391 | 392 | Eigen::Matrix3d rot_mat = final_transformation.block<3, 3>(0, 0).cast(); 393 | Eigen::Quaterniond quat_eig(rot_mat); 394 | geometry_msgs::msg::Quaternion quat_msg = tf2::toMsg(quat_eig); 395 | 396 | geometry_msgs::msg::TransformStamped transform_stamped; 397 | transform_stamped.header.stamp = stamp; 398 | transform_stamped.header.frame_id = global_frame_id_; 399 | transform_stamped.child_frame_id = robot_frame_id_; 400 | transform_stamped.transform.translation.x = position.x(); 401 | transform_stamped.transform.translation.y = position.y(); 402 | transform_stamped.transform.translation.z = position.z(); 403 | transform_stamped.transform.rotation = quat_msg; 404 | broadcaster_.sendTransform(transform_stamped); 405 | 406 | corrent_pose_stamped_.header.stamp = stamp; 407 | corrent_pose_stamped_.pose.position.x = position.x(); 408 | corrent_pose_stamped_.pose.position.y = position.y(); 409 | corrent_pose_stamped_.pose.position.z = position.z(); 410 | corrent_pose_stamped_.pose.orientation = quat_msg; 411 | pose_pub_->publish(corrent_pose_stamped_); 412 | 413 | nav_msgs::msg::Odometry odom; 414 | odom.header = transform_stamped.header; 415 | odom.pose.pose = corrent_pose_stamped_.pose; 416 | odom.twist = odom_que_[odom_ptr_front_].twist; 417 | odom_pub_->publish(odom); 418 | 419 | path_.poses.push_back(corrent_pose_stamped_); 420 | path_pub_->publish(path_); 421 | 422 | trans_ = (position - previous_position_).norm(); 423 | if (trans_ >= trans_for_mapupdate_ && !mapping_flag_) { 424 | geometry_msgs::msg::PoseStamped corrent_pose_stamped; 425 | corrent_pose_stamped = corrent_pose_stamped_; 426 | previous_position_ = position; 427 | mapping_task_ = 428 | std::packaged_task( 429 | std::bind( 430 | &ScanMatcherComponent::updateMap, this, cloud_ptr, 431 | final_transformation, corrent_pose_stamped)); 432 | mapping_future_ = mapping_task_.get_future(); 433 | mapping_thread_ = std::thread(std::move(std::ref(mapping_task_))); 434 | mapping_flag_ = true; 435 | } 436 | } 437 | 438 | void ScanMatcherComponent::updateMap( 439 | const pcl::PointCloud::ConstPtr cloud_ptr, 440 | const Eigen::Matrix4f final_transformation, 441 | const geometry_msgs::msg::PoseStamped corrent_pose_stamped) 442 | { 443 | pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); 444 | pcl::VoxelGrid voxel_grid; 445 | voxel_grid.setLeafSize(vg_size_for_map_, vg_size_for_map_, vg_size_for_map_); 446 | voxel_grid.setInputCloud(cloud_ptr); 447 | voxel_grid.filter(*filtered_cloud_ptr); 448 | 449 | pcl::PointCloud::Ptr transformed_cloud_ptr(new pcl::PointCloud()); 450 | pcl::transformPointCloud(*filtered_cloud_ptr, *transformed_cloud_ptr, final_transformation); 451 | 452 | targeted_cloud_.clear(); 453 | targeted_cloud_ += *transformed_cloud_ptr; 454 | int num_submaps = map_array_msg_.submaps.size(); 455 | for (int i = 0; i < num_targeted_cloud_ - 1; i++) { 456 | if (num_submaps - 1 - i < 0) {continue;} 457 | pcl::PointCloud::Ptr tmp_ptr(new pcl::PointCloud()); 458 | pcl::fromROSMsg(map_array_msg_.submaps[num_submaps - 1 - i].cloud, *tmp_ptr); 459 | pcl::PointCloud::Ptr transformed_tmp_ptr(new pcl::PointCloud()); 460 | Eigen::Affine3d submap_affine; 461 | tf2::fromMsg(map_array_msg_.submaps[num_submaps - 1 - i].pose, submap_affine); 462 | pcl::transformPointCloud(*tmp_ptr, *transformed_tmp_ptr, submap_affine.matrix()); 463 | targeted_cloud_ += *transformed_tmp_ptr; 464 | } 465 | 466 | /* map array */ 467 | sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr( 468 | new sensor_msgs::msg::PointCloud2); 469 | pcl::toROSMsg(*filtered_cloud_ptr, *cloud_msg_ptr); 470 | 471 | lidarslam_msgs::msg::SubMap submap; 472 | submap.header.frame_id = global_frame_id_; 473 | submap.header.stamp = corrent_pose_stamped.header.stamp; 474 | latest_distance_ += trans_; 475 | submap.distance = latest_distance_; 476 | submap.pose = corrent_pose_stamped.pose; 477 | submap.cloud = *cloud_msg_ptr; 478 | submap.cloud.header.frame_id = global_frame_id_; 479 | map_array_msg_.header.stamp = corrent_pose_stamped.header.stamp; 480 | map_array_msg_.submaps.push_back(submap); 481 | map_array_pub_->publish(map_array_msg_); 482 | 483 | is_map_updated_ = true; 484 | 485 | rclcpp::Time map_time = clock_.now(); 486 | double dt = map_time.seconds() - last_map_time_.seconds(); 487 | if (dt > map_publish_period_) { 488 | publishMap(map_array_msg_, global_frame_id_); 489 | last_map_time_ = map_time; 490 | } 491 | } 492 | 493 | Eigen::Matrix4f ScanMatcherComponent::getTransformation(const geometry_msgs::msg::Pose pose) 494 | { 495 | Eigen::Affine3d affine; 496 | tf2::fromMsg(pose, affine); 497 | Eigen::Matrix4f sim_trans = affine.matrix().cast(); 498 | return sim_trans; 499 | } 500 | 501 | void ScanMatcherComponent::receiveOdom(const nav_msgs::msg::Odometry odom_msg) 502 | { 503 | if (!use_imu_) {return;} 504 | // RCLCPP_WARN(get_logger(), "The odom was received."); 505 | odom_ptr_last_ = (odom_ptr_last_ + 1) % odom_que_length_; 506 | odom_que_[odom_ptr_last_] = odom_msg; 507 | if ((odom_ptr_last_ + 1) % odom_que_length_ == odom_ptr_front_) { 508 | odom_ptr_front_ = (odom_ptr_front_ + 1) % odom_que_length_; 509 | } 510 | } 511 | 512 | void ScanMatcherComponent::publishMap(const lidarslam_msgs::msg::MapArray & map_array_msg , const std::string & map_frame_id) 513 | { 514 | pcl::PointCloud::Ptr map_ptr(new pcl::PointCloud); 515 | for (auto & submap : map_array_msg.submaps) { 516 | pcl::PointCloud::Ptr submap_cloud_ptr(new pcl::PointCloud); 517 | pcl::PointCloud::Ptr transformed_submap_cloud_ptr( 518 | new pcl::PointCloud); 519 | pcl::fromROSMsg(submap.cloud, *submap_cloud_ptr); 520 | 521 | Eigen::Affine3d affine; 522 | tf2::fromMsg(submap.pose, affine); 523 | pcl::transformPointCloud( 524 | *submap_cloud_ptr, *transformed_submap_cloud_ptr, 525 | affine.matrix().cast()); 526 | 527 | *map_ptr += *transformed_submap_cloud_ptr; 528 | } 529 | RCLCPP_INFO(get_logger(), "publish a map, number of points in the map : %ld", map_ptr->size()); 530 | 531 | sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2); 532 | pcl::toROSMsg(*map_ptr, *map_msg_ptr); 533 | map_msg_ptr->header.frame_id = map_frame_id; 534 | map_pub_->publish(*map_msg_ptr); 535 | } 536 | 537 | } // namespace graphslam 538 | 539 | #include 540 | RCLCPP_COMPONENTS_REGISTER_NODE(graphslam::ScanMatcherComponent) 541 | -------------------------------------------------------------------------------- /scanmatcher/src/scanmatcher_node.cpp: -------------------------------------------------------------------------------- 1 | #include "scanmatcher/scanmatcher_component.h" 2 | #include 3 | 4 | int main(int argc, char * argv[]) 5 | { 6 | rclcpp::init(argc, argv); 7 | rclcpp::NodeOptions options; 8 | options.use_intra_process_comms(true); 9 | rclcpp::spin(std::make_shared(options)); 10 | rclcpp::shutdown(); 11 | return 0; 12 | } 13 | --------------------------------------------------------------------------------