├── .clang-format ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── fast_lio_sam_sc_qn ├── CMakeLists.txt ├── config │ ├── config.yaml │ ├── lio_rviz.rviz │ └── sam_rviz.rviz ├── include │ ├── fast_lio_sam_sc_qn.h │ ├── loop_closure.h │ ├── pose_pcd.hpp │ └── utilities.hpp ├── launch │ └── run.launch ├── package.xml └── src │ ├── fast_lio_sam_sc_qn.cpp │ ├── loop_closure.cpp │ └── main.cpp └── third_party └── fastlio_config_launch ├── kimera-multi.launch ├── kimera-multi.yaml ├── kitti.launch ├── kitti.yaml ├── mulran.launch ├── mulran.yaml ├── newer-college2020.launch ├── newer-college2020.yaml ├── vbr-colosseo.launch └── vbr-colosseo.yaml /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: LLVM 4 | AccessModifierOffset: -4 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: true 11 | AllowShortBlocksOnASingleLine: true 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: Inline 14 | AllowShortIfStatementsOnASingleLine: false 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: false 17 | AlwaysBreakBeforeMultilineStrings: false 18 | AlwaysBreakTemplateDeclarations: true 19 | BinPackArguments: false 20 | BinPackParameters: false 21 | BreakBeforeBraces: Allman 22 | BraceWrapping: 23 | AfterFunction: true 24 | AfterControlStatement: true 25 | BreakConstructorInitializers: AfterColon 26 | BreakConstructorInitializersBeforeComma: false 27 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 28 | Cpp11BracedListStyle: true 29 | ExperimentalAutoDetectBinPacking: false 30 | ForEachMacros: 31 | - foreach 32 | - Q_FOREACH 33 | - BOOST_FOREACH 34 | IncludeCategories: 35 | # C system headers 36 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 37 | Priority: 10 38 | # C++ system headers 39 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 40 | Priority: 20 41 | # Other library h files (with angles) 42 | - Regex: "^<" 43 | Priority: 30 44 | # Your project's h files 45 | - Regex: '^"' 46 | Priority: 40 47 | IndentCaseLabels: true 48 | IndentWidth: 4 49 | IndentWrappedFunctionNames: false 50 | KeepEmptyLinesAtTheStartOfBlocks: false 51 | MacroBlockBegin: "" 52 | MacroBlockEnd: "" 53 | ColumnLimit: 0 54 | MaxEmptyLinesToKeep: 2 55 | NamespaceIndentation: All 56 | ObjCSpaceAfterProperty: false 57 | ObjCSpaceBeforeProtocolList: false 58 | PenaltyBreakBeforeFirstCallParameter: 1000 59 | PenaltyBreakAssignment: 1000 60 | PenaltyBreakComment: 1000 61 | PenaltyBreakFirstLessLess: 1000 62 | PenaltyBreakString: 1000 63 | PenaltyExcessCharacter: 100000 64 | PenaltyReturnTypeOnItsOwnLine: 1000 65 | PointerAlignment: Right 66 | ReflowComments: true 67 | SortIncludes: false 68 | SortUsingDeclarations: false 69 | SpaceAfterCStyleCast: false 70 | SpaceAfterLogicalNot: false 71 | SpaceAfterTemplateKeyword: false 72 | SpaceBeforeAssignmentOperators: true 73 | SpaceBeforeCpp11BracedList: false 74 | SpaceBeforeCtorInitializerColon: false 75 | SpaceBeforeInheritanceColon: false 76 | SpaceBeforeParens: ControlStatements 77 | SpaceBeforeRangeBasedForLoopColon: true 78 | SpaceInEmptyParentheses: false 79 | SpacesBeforeTrailingComments: 1 80 | SpacesInAngles: false 81 | SpacesInCStyleCastParentheses: false 82 | SpacesInContainerLiterals: false 83 | SpacesInParentheses: false 84 | SpacesInSquareBrackets: false 85 | Standard: Cpp11 86 | TabWidth: 4 87 | UseTab: Never 88 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | *.bag 3 | fast_lio_sam_qn/*.bag 4 | /fast_lio_sam_sc_qn/result.pcd 5 | /fast_lio_sam_sc_qn/sequence*/ 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/Quatro"] 2 | path = third_party/Quatro 3 | url = https://github.com/engcang/Quatro 4 | [submodule "third_party/FAST_LIO"] 5 | path = third_party/FAST_LIO 6 | url = https://github.com/hku-mars/FAST_LIO 7 | [submodule "third_party/nano_gicp"] 8 | path = third_party/nano_gicp 9 | url = https://github.com/engcang/nano_gicp 10 | [submodule "third_party/livox_ros_driver"] 11 | path = third_party/livox_ros_driver 12 | url = https://github.com/Livox-SDK/livox_ros_driver 13 | [submodule "third_party/scancontext_tro"] 14 | path = third_party/scancontext_tro 15 | url = https://github.com/engcang/scancontext_tro 16 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v5.0.0 4 | hooks: 5 | - id: check-yaml 6 | - id: check-json 7 | - id: check-xml 8 | - id: end-of-file-fixer 9 | - id: trailing-whitespace 10 | args: [--markdown-linebreak-ext=md] 11 | - id: check-added-large-files 12 | - id: check-merge-conflict 13 | # - id: check-ast #python 14 | 15 | # Python 16 | # - repo: https://github.com/psf/black 17 | # rev: "23.7.0" 18 | # hooks: 19 | # - id: black 20 | 21 | # - repo: https://github.com/pylint-dev/pylint 22 | # rev: "v3.0.0a6" 23 | # hooks: 24 | # - id: pylint 25 | 26 | - repo: https://github.com/pre-commit/mirrors-clang-format 27 | rev: "v19.1.2" 28 | hooks: 29 | - id: clang-format 30 | 31 | - repo: https://github.com/cpplint/cpplint 32 | rev: "2.0.0" 33 | hooks: 34 | - id: cpplint 35 | args: 36 | [ 37 | "--filter=-whitespace/indent,-whitespace/braces,-whitespace/comments,-whitespace/line_length,-whitespace/newline, 38 | -build/include_subdir,-build/header_guard,-build/include_order,-build/c++17,-build/namespaces, 39 | -readability/braces,-readability/todo,-legal/copyright,-runtime/references", 40 | ] 41 | files: '\.h$|\.hpp$' 42 | exclude: third_party/ 43 | 44 | - repo: https://github.com/cpplint/cpplint 45 | rev: "2.0.0" 46 | hooks: 47 | - id: cpplint 48 | args: 49 | [ 50 | "--filter=-whitespace/indent,-whitespace/braces,-whitespace/comments,-whitespace/line_length,-whitespace/newline, 51 | -build/include_subdir,-build/header_guard,-build/include_order,-build/c++17,-build/namespaces,-build/include_what_you_use, 52 | -readability/braces,-readability/todo,-legal/copyright,-runtime/references", 53 | ] 54 | files: '\.cpp$' 55 | exclude: third_party/ 56 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2024, EungChang-Mason-Lee 2 | All rights reserved. 3 | 4 | All codes are copyrighted by EungChang-Mason-Lee published under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License. 5 | You must attribute the work in the manner specified by the author. 6 | You may not use the work for commercial purposes, 7 | and you may only distribute the resulting work under the same license if you alter, transform, or create the work. 8 | 9 | Redistribution and use in source and binary forms, ONLY without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | 1. Redistributions of source code must retain the above copyright notice, this 13 | list of conditions and the following disclaimer. 14 | 15 | 2. Redistributions in binary form must reproduce the above copyright notice, 16 | this list of conditions and the following disclaimer in the documentation 17 | and/or other materials provided with the distribution. 18 | 19 | 3. Neither the name of the copyright holder nor the names of its 20 | contributors may be used to endorse or promote products derived from 21 | this software without specific prior written permission. 22 | 23 | 4. Redistributions or use in any forms are NOT for any method of COMMERCIAL use. 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FAST-LIO-SAM-SC-QN 2 | + This repository is a SLAM implementation combining [FAST-LIO2](https://github.com/hku-mars/FAST_LIO) with pose graph optimization and loop closing based on [ScanContext](https://github.com/gisbi-kim/scancontext_tro), [Quatro](https://quatro-plusplus.github.io/), and [Nano-GICP module](https://github.com/engcang/nano_gicp) 3 | + [ScanContext](https://github.com/gisbi-kim/scancontext_tro) - a global descriptor for LiDAR point cloud, here it is used as loop candidate pair detection 4 | + [Quatro](https://quatro-plusplus.github.io/) - fast, accurate and robust global registration which provides great initial guess of transform 5 | + [Quatro module](https://github.com/engcang/quatro) - `Quatro` as a module, can be easily used in other packages 6 | + [Nano-GICP module](https://github.com/engcang/nano_gicp) - fast ICP combining [FastGICP](https://github.com/SMRT-AIST/fast_gicp) + [NanoFLANN](https://github.com/jlblancoc/nanoflann) 7 | + Note: similar repositories already exist 8 | + [FAST_LIO_LC](https://github.com/yanliang-wang/FAST_LIO_LC): FAST-LIO2 + SC-A-LOAM based SLAM 9 | + [FAST_LIO_SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM): FAST-LIO2 + ScanContext based SLAM 10 | + [FAST_LIO_SAM](https://github.com/kahowang/FAST_LIO_SAM): FAST-LIO2 + LIO-SAM (not modularized) 11 | + [FAST_LIO_SAM](https://github.com/engcang/FAST-LIO-SAM): FAST-LIO2 + LIO-SAM (modularized) 12 | + [FAST_LIO_SAM_QN](https://github.com/engcang/FAST-LIO-SAM-QN): FAST-LIO2 + LIO-SAM + Quatro + Nano-GICP (modularized) 13 | + Note2: main code (PGO) is modularized and hence can be combined with any other LIO / LO 14 | + This repo is to learn GTSAM myself! 15 | + and as GTSAM tutorial for beginners - [GTSAM 튜토리얼 한글 포스팅](https://engcang.github.io/2023/07/15/gtsam_tutorial.html) 16 | ## Video clip - https://youtu.be/MQ8XxRY472Y 17 | 18 |
19 | 20 | ## Main difference between [FAST_LIO_SAM_QN](https://github.com/engcang/FAST-LIO-SAM-QN) and FAST_LIO_SAM_SC_QN 21 | + FAST_LIO_SAM_QN sets loop candidate pair as (current keyframe, the closest and old enough keyframe) 22 | + FAST_LIO_SAM_SC_QN gets loop candidate pair from ScanContext 23 | 24 |
25 | 26 | ## Dependencies 27 | + `C++` >= 17, `OpenMP` >= 4.5, `CMake` >= 3.10.0, `Eigen` >= 3.2, `Boost` >= 1.54 28 | + `ROS` 29 | + [`GTSAM`](https://github.com/borglab/gtsam) >= 4.1.1 30 | ```shell 31 | wget -O gtsam.zip https://github.com/borglab/gtsam/archive/refs/tags/4.1.1.zip 32 | unzip gtsam.zip 33 | cd gtsam-4.1.1/ 34 | mkdir build && cd build 35 | cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. 36 | sudo make install -j16 37 | ``` 38 | + [`Teaser++`](https://github.com/MIT-SPARK/TEASER-plusplus) 39 | ```shell 40 | git clone https://github.com/MIT-SPARK/TEASER-plusplus.git 41 | cd TEASER-plusplus && mkdir build && cd build 42 | cmake .. -DENABLE_DIAGNOSTIC_PRINT=OFF 43 | sudo make install -j16 44 | sudo ldconfig 45 | ``` 46 | + `tbb` (is used for faster `Quatro`) 47 | ```shell 48 | sudo apt install libtbb-dev 49 | ``` 50 | 51 | ## How to build 52 | + Get the code and then build the main code. 53 | ```shell 54 | cd ~/your_workspace/src 55 | git clone https://github.com/engcang/FAST-LIO-SAM-SC-QN --recursive 56 | 57 | cd ~/your_workspace 58 | # nano_gicp, quatro first 59 | catkin build nano_gicp -DCMAKE_BUILD_TYPE=Release 60 | # Note the option! 61 | catkin build quatro -DCMAKE_BUILD_TYPE=Release -DQUATRO_TBB=ON -DQUATRO_DEBUG=OFF 62 | catkin build -DCMAKE_BUILD_TYPE=Release 63 | . devel/setup.bash 64 | ``` 65 | 66 | ## How to run 67 | + Then run (change config files in third_party/`FAST_LIO`) 68 | ```shell 69 | roslaunch fast_lio_sam_sc_qn run.launch lidar:=ouster 70 | roslaunch fast_lio_sam_sc_qn run.launch lidar:=velodyne 71 | roslaunch fast_lio_sam_sc_qn run.launch lidar:=livox 72 | ``` 73 | * In particular, we provide a preset launch option for specific datasets: 74 | ```shell 75 | roslaunch fast_lio_sam_sc_qn run.launch lidar:=kitti 76 | roslaunch fast_lio_sam_sc_qn run.launch lidar:=mulran 77 | roslaunch fast_lio_sam_sc_qn run.launch lidar:=newer-college20 78 | ``` 79 | 80 |
81 | 82 | ## Structure 83 | + odomPcdCallback 84 | + pub realtime pose in corrected frame 85 | + keyframe detection -> if keyframe, add to pose graph + save to keyframe queue + generate ScanContext 86 | + pose graph optimization with iSAM2 87 | + loopTimerFunc 88 | + process a saved keyframe 89 | + detect loop -> if loop, add to pose graph 90 | + visTimerFunc 91 | + visualize all **(Note: global map is only visualized once uncheck/check the mapped_pcd in rviz to save comp.)** 92 | 93 |
94 | 95 | ## Memo 96 | + `Quatro` module fixed for empty matches 97 | + `Quatro` module is updated with `optimizedMatching` which limits the number of correspondences and increased the speed 98 | 99 |
100 | 101 | ## License 102 | Creative Commons License 103 | 104 | This work is licensed under a [Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License](http://creativecommons.org/licenses/by-nc-sa/4.0/) 105 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(fast_lio_sam_sc_qn) 3 | 4 | ### set compiler 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS "-std=c++17 -pthread -fexceptions -g -ggdb") #c++17, for parallel programming and for Eigen (see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html) 7 | set(CMAKE_THREAD_PREFER_PTHREAD TRUE) 8 | include(FindOpenMP) #For Nano-GICP, Quatro #The best way to set proper compiler settings for using OpenMP in all platforms 9 | if(OPENMP_FOUND) #For Nano-GICP, Quatro #The best way to set proper compiler settings for using OpenMP in all platforms 10 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 12 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 13 | else(OPENMP_FOUND) 14 | message("ERROR: OpenMP could not be found.") 15 | endif(OPENMP_FOUND) 16 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -Wextra ${CMAKE_CXX_FLAGS}") 17 | 18 | ### get packages 19 | find_package(catkin REQUIRED COMPONENTS 20 | roscpp 21 | roslib 22 | rosbag 23 | std_msgs 24 | geometry_msgs 25 | nav_msgs 26 | sensor_msgs 27 | visualization_msgs 28 | tf 29 | tf_conversions 30 | pcl_ros 31 | pcl_conversions 32 | message_filters 33 | nano_gicp 34 | quatro 35 | ) 36 | find_package(PCL REQUIRED) 37 | find_package(Eigen3 REQUIRED) 38 | find_package(GTSAM REQUIRED) 39 | if(PCL_FOUND) 40 | message(WARNING "PCL_VER: ${PCL_VERSION}, and PCL_DIR: ${PCL_INCLUDE_DIRS}") 41 | endif() 42 | if(Eigen3_FOUND) 43 | message(WARNING "EIGEN_VER: ${EIGEN3_VERSION_STRING}, and Eigen DIR: ${EIGEN3_INCLUDE_DIR}") 44 | endif() 45 | if(GTSAM_FOUND) 46 | message(WARNING "GTSAM_VER: ${GTSAM_VERSION}, and GTSAM DIR: ${GTSAM_INCLUDE_DIR}") 47 | endif() 48 | 49 | catkin_package() #this automatically adds/installs the results of build 50 | 51 | ### get packages' headers 52 | include_directories( 53 | include 54 | ${CMAKE_SOURCE_DIR}/../third_party/scancontext_tro 55 | ${catkin_INCLUDE_DIRS} 56 | ${PCL_INCLUDE_DIRS} 57 | ${EIGEN3_INCLUDE_DIR} 58 | ${GTSAM_INCLUDE_DIR} 59 | ) 60 | 61 | 62 | ########### 63 | ## Build ## 64 | ########### 65 | ### main 66 | set(COMMON_LIBS ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${EIGEN3_LIBS} ${OpenMP_LIBS} gtsam) 67 | add_library(${PROJECT_NAME}_modules ${CMAKE_SOURCE_DIR}/../third_party/scancontext_tro/Scancontext.cpp src/loop_closure.cpp src/fast_lio_sam_sc_qn.cpp) 68 | target_link_libraries(${PROJECT_NAME}_modules ${COMMON_LIBS}) 69 | 70 | add_executable(${PROJECT_NAME}_node src/main.cpp) 71 | target_link_libraries(${PROJECT_NAME}_node ${COMMON_LIBS} ${PROJECT_NAME}_modules) 72 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/config/config.yaml: -------------------------------------------------------------------------------- 1 | basic: 2 | map_frame: "map" # publish frame 3 | loop_update_hz: 2.0 # Loop detection Hz 4 | vis_hz: 1.0 # visualize Hz 5 | 6 | keyframe: 7 | keyframe_threshold: 1.5 # unit [meter] every this distance, generate keyframe 8 | num_submap_keyframes: 10 # how many subkeyframes are merged to ICP with current keyframe (closest-this ~ closest+this, if exists) 9 | enable_submap_matching: false # If it is set to be `true`, submap-level (coarse-to-)fine alignment will be performed 10 | # note: without Quatro, anyway scan-to-submap matching is performed, as that shows better performance 11 | 12 | quatro_nano_gicp_voxel_resolution: 0.3 # voxel resolution to use Quatro and Nano-GICP matching 13 | save_voxel_resolution: 0.3 # voxel resolution to save map in .pcd format 14 | scancontext_max_correspondence_distance: 35.0 # this is used for candidate trimming after ScanContext 15 | 16 | nano_gicp: # all values are from Nano-GICP official github (Direct LiDAR Odometry) 17 | thread_number: 0 # if 0, max number of core 18 | icp_score_threshold: 1.5 # lower is more accurate, for loop detection 19 | correspondences_number: 15 20 | max_correspondence_distance: 35.0 # this is also used for candidate trimming after ScanContext 21 | max_iter: 32 22 | transformation_epsilon: 0.01 23 | euclidean_fitness_epsilon: 0.01 24 | ransac: 25 | max_iter: 5 26 | outlier_rejection_threshold: 1.0 27 | 28 | quatro: # all values are from Quatro official github 29 | enable: true # whether or not to use quatro, if false -> FAST-LIO-SAM-N (Nano-GICP only) 30 | optimize_matching: true # whether or not to use Optimized matching 31 | distance_threshold: 35.0 # when optimized matching, use only correspondences within this radius, unit [meter] 32 | max_correspondences: 500 # max correspondences to use for optimized matching 33 | fpfh_normal_radius: 0.9 # It should be 2.5 - 3.0 * `quatro_nano_gicp_voxel_resolution` 34 | fpfh_radius: 1.5 # It should be 5.0 * `quatro_nano_gicp_voxel_resolution` 35 | estimating_scale: false 36 | noise_bound: 0.3 # The magnitude of uncertainty of measurements, the best is within v/2 ~ v (v: voxel resol.) 37 | rotation: 38 | num_max_iter: 50 # Usually, rotation estimation converges within < 20 iterations 39 | gnc_factor: 1.4 # Control the magnitue of the increase in non-linearity. The larger the value, the steeper the increase in nonlinearity. 40 | rot_cost_diff_threshold: 0.0001 # The cost threshold is compared with the difference between costs of consecutive iterations. 41 | # Once the diff. of cost < `rot_cost_diff_threshold`, then the optimization is finished. 42 | 43 | result: 44 | save_map_pcd: true # Save result map in .pcd format, not voxelized and hence file size could be huge 45 | save_map_bag: true # Save result map in .bag format, NOTE: this is used for FAST-LIO-Localization-QN (https://github.com/engcang/FAST-LIO-Localization-QN) 46 | save_in_kitti_format: true # Save result in KITTI format 47 | seq_name: "sequence" # sequence name for saving scans and corresponding poses 48 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/config/lio_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /mapping1 9 | - /mapping1/currPoints1 10 | - /mapping1/currPoints1/Autocompute Value Bounds1 11 | Splitter Ratio: 0.6432291865348816 12 | Tree Height: 441 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: surround 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 1 40 | Cell Size: 1000 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: false 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 40 55 | Reference Frame: 56 | Value: false 57 | - Class: rviz/Axes 58 | Enabled: true 59 | Length: 0.699999988079071 60 | Name: Axes 61 | Radius: 0.10000000149011612 62 | Reference Frame: 63 | Value: true 64 | - Class: rviz/Group 65 | Displays: 66 | - Alpha: 1 67 | Autocompute Intensity Bounds: true 68 | Autocompute Value Bounds: 69 | Max Value: 10 70 | Min Value: -10 71 | Value: true 72 | Axis: Z 73 | Channel Name: intensity 74 | Class: rviz/PointCloud2 75 | Color: 238; 238; 236 76 | Color Transformer: FlatColor 77 | Decay Time: 0 78 | Enabled: true 79 | Invert Rainbow: false 80 | Max Color: 255; 255; 255 81 | Min Color: 238; 238; 236 82 | Name: surround 83 | Position Transformer: XYZ 84 | Queue Size: 1 85 | Selectable: false 86 | Size (Pixels): 1 87 | Size (m): 0.05000000074505806 88 | Style: Points 89 | Topic: /cloud_registered 90 | Unreliable: false 91 | Use Fixed Frame: true 92 | Use rainbow: true 93 | Value: true 94 | - Alpha: 0.5 95 | Autocompute Intensity Bounds: true 96 | Autocompute Value Bounds: 97 | Max Value: 15 98 | Min Value: -5 99 | Value: false 100 | Axis: Z 101 | Channel Name: intensity 102 | Class: rviz/PointCloud2 103 | Color: 255; 255; 255 104 | Color Transformer: Intensity 105 | Decay Time: 1000 106 | Enabled: true 107 | Invert Rainbow: false 108 | Max Color: 255; 255; 255 109 | Min Color: 0; 0; 0 110 | Name: currPoints 111 | Position Transformer: XYZ 112 | Queue Size: 100000 113 | Selectable: true 114 | Size (Pixels): 1 115 | Size (m): 0.029999999329447746 116 | Style: Flat Squares 117 | Topic: /cloud_registered 118 | Unreliable: false 119 | Use Fixed Frame: true 120 | Use rainbow: true 121 | Value: true 122 | Enabled: true 123 | Name: mapping 124 | - Angle Tolerance: 0.009999999776482582 125 | Class: rviz/Odometry 126 | Covariance: 127 | Orientation: 128 | Alpha: 0.5 129 | Color: 255; 255; 127 130 | Color Style: Unique 131 | Frame: Local 132 | Offset: 1 133 | Scale: 1 134 | Value: true 135 | Position: 136 | Alpha: 0.30000001192092896 137 | Color: 204; 51; 204 138 | Scale: 1 139 | Value: true 140 | Value: true 141 | Enabled: true 142 | Keep: 1 143 | Name: Odometry 144 | Position Tolerance: 0.0010000000474974513 145 | Shape: 146 | Alpha: 1 147 | Axes Length: 1 148 | Axes Radius: 0.20000000298023224 149 | Color: 255; 85; 0 150 | Head Length: 0 151 | Head Radius: 0 152 | Shaft Length: 0.05000000074505806 153 | Shaft Radius: 0.05000000074505806 154 | Value: Axes 155 | Topic: /Odometry 156 | Unreliable: false 157 | Value: true 158 | - Alpha: 0 159 | Buffer Length: 2 160 | Class: rviz/Path 161 | Color: 25; 255; 255 162 | Enabled: true 163 | Head Diameter: 0 164 | Head Length: 0 165 | Length: 0.30000001192092896 166 | Line Style: Billboards 167 | Line Width: 0.10000000149011612 168 | Name: Path 169 | Offset: 170 | X: 0 171 | Y: 0 172 | Z: 0 173 | Pose Color: 25; 255; 255 174 | Pose Style: None 175 | Radius: 0.029999999329447746 176 | Shaft Diameter: 0.4000000059604645 177 | Shaft Length: 0.4000000059604645 178 | Topic: /path 179 | Unreliable: false 180 | Value: true 181 | Enabled: true 182 | Global Options: 183 | Background Color: 0; 0; 0 184 | Default Light: true 185 | Fixed Frame: camera_init 186 | Frame Rate: 10 187 | Name: root 188 | Tools: 189 | - Class: rviz/Interact 190 | Hide Inactive Objects: true 191 | - Class: rviz/MoveCamera 192 | - Class: rviz/Select 193 | - Class: rviz/FocusCamera 194 | - Class: rviz/Measure 195 | - Class: rviz/SetInitialPose 196 | Theta std deviation: 0.2617993950843811 197 | Topic: /initialpose 198 | X std deviation: 0.5 199 | Y std deviation: 0.5 200 | - Class: rviz/SetGoal 201 | Topic: /move_base_simple/goal 202 | - Class: rviz/PublishPoint 203 | Single click: true 204 | Topic: /clicked_point 205 | Value: true 206 | Views: 207 | Current: 208 | Class: rviz/Orbit 209 | Distance: 699.7850341796875 210 | Enable Stereo Rendering: 211 | Stereo Eye Separation: 0.05999999865889549 212 | Stereo Focal Distance: 1 213 | Swap Stereo Eyes: false 214 | Value: false 215 | Focal Point: 216 | X: -27.18464469909668 217 | Y: -81.90044403076172 218 | Z: 0.8559151887893677 219 | Focal Shape Fixed Size: false 220 | Focal Shape Size: 0.05000000074505806 221 | Invert Z Axis: false 222 | Name: Current View 223 | Near Clip Distance: 0.009999999776482582 224 | Pitch: 1.5697963237762451 225 | Target Frame: 226 | Value: Orbit (rviz) 227 | Yaw: 1.6053975820541382 228 | Saved: ~ 229 | Window Geometry: 230 | Displays: 231 | collapsed: true 232 | Height: 1016 233 | Hide Left Dock: true 234 | Hide Right Dock: true 235 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c7000000000000000000000001000001520000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000071800000052fc0100000002fb0000000800540069006d0065000000000000000718000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000004df0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 236 | Selection: 237 | collapsed: false 238 | Time: 239 | collapsed: false 240 | Tool Properties: 241 | collapsed: false 242 | Views: 243 | collapsed: true 244 | Width: 1247 245 | X: 1147 246 | Y: 335 247 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/config/sam_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /mapping1 9 | - /mapping1/curr_pcd1/Autocompute Value Bounds1 10 | - /debug1 11 | Splitter Ratio: 0.6432291865348816 12 | Tree Height: 865 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: curr_pcd_stack 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 1 39 | Cell Size: 1000 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 40 54 | Reference Frame: 55 | Value: false 56 | - Alpha: 1 57 | Class: rviz/Axes 58 | Enabled: false 59 | Length: 5 60 | Name: Axes 61 | Radius: 0.800000011920929 62 | Reference Frame: 63 | Show Trail: false 64 | Value: false 65 | - Class: rviz/Group 66 | Displays: 67 | - Alpha: 0.4000000059604645 68 | Autocompute Intensity Bounds: true 69 | Autocompute Value Bounds: 70 | Max Value: 4.1279520988464355 71 | Min Value: -5.993770599365234 72 | Value: true 73 | Axis: Z 74 | Channel Name: intensity 75 | Class: rviz/PointCloud2 76 | Color: 238; 238; 236 77 | Color Transformer: AxisColor 78 | Decay Time: 0 79 | Enabled: true 80 | Invert Rainbow: false 81 | Max Color: 255; 255; 255 82 | Min Color: 238; 238; 236 83 | Name: mapped_pcd 84 | Position Transformer: XYZ 85 | Queue Size: 1 86 | Selectable: false 87 | Size (Pixels): 1 88 | Size (m): 0.03999999910593033 89 | Style: Flat Squares 90 | Topic: /corrected_map 91 | Unreliable: false 92 | Use Fixed Frame: true 93 | Use rainbow: true 94 | Value: true 95 | - Alpha: 1 96 | Autocompute Intensity Bounds: true 97 | Autocompute Value Bounds: 98 | Max Value: 15 99 | Min Value: -5 100 | Value: false 101 | Axis: Z 102 | Channel Name: intensity 103 | Class: rviz/PointCloud2 104 | Color: 255; 255; 255 105 | Color Transformer: FlatColor 106 | Decay Time: 0 107 | Enabled: false 108 | Invert Rainbow: false 109 | Max Color: 255; 255; 255 110 | Min Color: 0; 0; 0 111 | Name: curr_pcd 112 | Position Transformer: XYZ 113 | Queue Size: 100000 114 | Selectable: true 115 | Size (Pixels): 1 116 | Size (m): 0.009999999776482582 117 | Style: Points 118 | Topic: /corrected_current_pcd 119 | Unreliable: false 120 | Use Fixed Frame: true 121 | Use rainbow: true 122 | Value: false 123 | - Alpha: 0.4000000059604645 124 | Autocompute Intensity Bounds: true 125 | Autocompute Value Bounds: 126 | Max Value: 15 127 | Min Value: -5 128 | Value: false 129 | Axis: Z 130 | Channel Name: intensity 131 | Class: rviz/PointCloud2 132 | Color: 255; 255; 255 133 | Color Transformer: FlatColor 134 | Decay Time: 500 135 | Enabled: true 136 | Invert Rainbow: false 137 | Max Color: 255; 255; 255 138 | Min Color: 0; 0; 0 139 | Name: curr_pcd_stack 140 | Position Transformer: XYZ 141 | Queue Size: 100000 142 | Selectable: true 143 | Size (Pixels): 1 144 | Size (m): 0.019999999552965164 145 | Style: Flat Squares 146 | Topic: /corrected_current_pcd 147 | Unreliable: false 148 | Use Fixed Frame: true 149 | Use rainbow: true 150 | Value: true 151 | Enabled: true 152 | Name: mapping 153 | - Alpha: 1 154 | Axes Length: 5 155 | Axes Radius: 0.800000011920929 156 | Class: rviz/Pose 157 | Color: 255; 25; 0 158 | Enabled: true 159 | Head Length: 0.30000001192092896 160 | Head Radius: 0.10000000149011612 161 | Name: current_corrected_pose 162 | Queue Size: 10 163 | Shaft Length: 1 164 | Shaft Radius: 0.05000000074505806 165 | Shape: Axes 166 | Topic: /pose_stamped 167 | Unreliable: false 168 | Value: true 169 | - Alpha: 0 170 | Buffer Length: 2 171 | Class: rviz/Path 172 | Color: 0; 155; 255 173 | Enabled: true 174 | Head Diameter: 0 175 | Head Length: 0 176 | Length: 0.30000001192092896 177 | Line Style: Billboards 178 | Line Width: 0.20000000298023224 179 | Name: Path 180 | Offset: 181 | X: 0 182 | Y: 0 183 | Z: 0 184 | Pose Color: 25; 255; 255 185 | Pose Style: None 186 | Queue Size: 10 187 | Radius: 0.029999999329447746 188 | Shaft Diameter: 0.4000000059604645 189 | Shaft Length: 0.4000000059604645 190 | Topic: /ori_path 191 | Unreliable: false 192 | Value: true 193 | - Alpha: 0 194 | Buffer Length: 2 195 | Class: rviz/Path 196 | Color: 138; 226; 52 197 | Enabled: true 198 | Head Diameter: 0 199 | Head Length: 0 200 | Length: 0.30000001192092896 201 | Line Style: Billboards 202 | Line Width: 0.20000000298023224 203 | Name: corrected_Path 204 | Offset: 205 | X: 0 206 | Y: 0 207 | Z: 0 208 | Pose Color: 25; 255; 255 209 | Pose Style: None 210 | Queue Size: 10 211 | Radius: 0.029999999329447746 212 | Shaft Diameter: 0.4000000059604645 213 | Shaft Length: 0.4000000059604645 214 | Topic: /corrected_path 215 | Unreliable: false 216 | Value: true 217 | - Alpha: 1 218 | Autocompute Intensity Bounds: true 219 | Autocompute Value Bounds: 220 | Max Value: 10 221 | Min Value: -10 222 | Value: true 223 | Axis: Z 224 | Channel Name: intensity 225 | Class: rviz/PointCloud2 226 | Color: 0; 59; 255 227 | Color Transformer: FlatColor 228 | Decay Time: 0 229 | Enabled: true 230 | Invert Rainbow: false 231 | Max Color: 255; 255; 255 232 | Min Color: 0; 0; 0 233 | Name: odoms 234 | Position Transformer: XYZ 235 | Queue Size: 10 236 | Selectable: true 237 | Size (Pixels): 3 238 | Size (m): 0.800000011920929 239 | Style: Spheres 240 | Topic: /ori_odom 241 | Unreliable: false 242 | Use Fixed Frame: true 243 | Use rainbow: true 244 | Value: true 245 | - Alpha: 1 246 | Autocompute Intensity Bounds: true 247 | Autocompute Value Bounds: 248 | Max Value: 10 249 | Min Value: -10 250 | Value: true 251 | Axis: Z 252 | Channel Name: intensity 253 | Class: rviz/PointCloud2 254 | Color: 138; 226; 52 255 | Color Transformer: FlatColor 256 | Decay Time: 0 257 | Enabled: true 258 | Invert Rainbow: false 259 | Max Color: 255; 255; 255 260 | Min Color: 0; 0; 0 261 | Name: corrected_odoms 262 | Position Transformer: XYZ 263 | Queue Size: 10 264 | Selectable: true 265 | Size (Pixels): 3 266 | Size (m): 0.800000011920929 267 | Style: Spheres 268 | Topic: /corrected_odom 269 | Unreliable: false 270 | Use Fixed Frame: true 271 | Use rainbow: true 272 | Value: true 273 | - Class: rviz/Marker 274 | Enabled: true 275 | Marker Topic: /loop_detection 276 | Name: loop_constraints 277 | Namespaces: 278 | {} 279 | Queue Size: 100 280 | Value: true 281 | - Class: rviz/Group 282 | Displays: 283 | - Alpha: 1 284 | Autocompute Intensity Bounds: true 285 | Autocompute Value Bounds: 286 | Max Value: 10 287 | Min Value: -10 288 | Value: true 289 | Axis: Z 290 | Channel Name: intensity 291 | Class: rviz/PointCloud2 292 | Color: 255; 0; 0 293 | Color Transformer: FlatColor 294 | Decay Time: 0 295 | Enabled: true 296 | Invert Rainbow: false 297 | Max Color: 255; 255; 255 298 | Min Color: 0; 0; 0 299 | Name: src 300 | Position Transformer: XYZ 301 | Queue Size: 10 302 | Selectable: true 303 | Size (Pixels): 2 304 | Size (m): 0.10000000149011612 305 | Style: Points 306 | Topic: /src 307 | Unreliable: false 308 | Use Fixed Frame: true 309 | Use rainbow: true 310 | Value: true 311 | - Alpha: 1 312 | Autocompute Intensity Bounds: true 313 | Autocompute Value Bounds: 314 | Max Value: 10 315 | Min Value: -10 316 | Value: true 317 | Axis: Z 318 | Channel Name: intensity 319 | Class: rviz/PointCloud2 320 | Color: 0; 101; 255 321 | Color Transformer: FlatColor 322 | Decay Time: 0 323 | Enabled: true 324 | Invert Rainbow: false 325 | Max Color: 255; 255; 255 326 | Min Color: 0; 0; 0 327 | Name: dst 328 | Position Transformer: XYZ 329 | Queue Size: 10 330 | Selectable: true 331 | Size (Pixels): 2 332 | Size (m): 0.10000000149011612 333 | Style: Points 334 | Topic: /dst 335 | Unreliable: false 336 | Use Fixed Frame: true 337 | Use rainbow: true 338 | Value: true 339 | - Alpha: 1 340 | Autocompute Intensity Bounds: true 341 | Autocompute Value Bounds: 342 | Max Value: 10 343 | Min Value: -10 344 | Value: true 345 | Axis: Z 346 | Channel Name: intensity 347 | Class: rviz/PointCloud2 348 | Color: 252; 233; 79 349 | Color Transformer: FlatColor 350 | Decay Time: 0 351 | Enabled: true 352 | Invert Rainbow: false 353 | Max Color: 255; 255; 255 354 | Min Color: 0; 0; 0 355 | Name: coarse_aligned 356 | Position Transformer: XYZ 357 | Queue Size: 10 358 | Selectable: true 359 | Size (Pixels): 2 360 | Size (m): 0.10000000149011612 361 | Style: Points 362 | Topic: /coarse_aligned_quatro 363 | Unreliable: false 364 | Use Fixed Frame: true 365 | Use rainbow: true 366 | Value: true 367 | - Alpha: 1 368 | Autocompute Intensity Bounds: true 369 | Autocompute Value Bounds: 370 | Max Value: 10 371 | Min Value: -10 372 | Value: true 373 | Axis: Z 374 | Channel Name: intensity 375 | Class: rviz/PointCloud2 376 | Color: 0; 255; 0 377 | Color Transformer: FlatColor 378 | Decay Time: 0 379 | Enabled: true 380 | Invert Rainbow: false 381 | Max Color: 255; 255; 255 382 | Min Color: 0; 0; 0 383 | Name: fine_aligned 384 | Position Transformer: XYZ 385 | Queue Size: 10 386 | Selectable: true 387 | Size (Pixels): 2 388 | Size (m): 0.10000000149011612 389 | Style: Points 390 | Topic: /fine_aligned_nano_gicp 391 | Unreliable: false 392 | Use Fixed Frame: true 393 | Use rainbow: true 394 | Value: true 395 | Enabled: true 396 | Name: debug 397 | Enabled: true 398 | Global Options: 399 | Background Color: 0; 0; 0 400 | Default Light: true 401 | Fixed Frame: map 402 | Frame Rate: 10 403 | Name: root 404 | Tools: 405 | - Class: rviz/Interact 406 | Hide Inactive Objects: true 407 | - Class: rviz/MoveCamera 408 | - Class: rviz/Select 409 | - Class: rviz/FocusCamera 410 | - Class: rviz/Measure 411 | - Class: rviz/SetInitialPose 412 | Theta std deviation: 0.2617993950843811 413 | Topic: /initialpose 414 | X std deviation: 0.5 415 | Y std deviation: 0.5 416 | - Class: rviz/SetGoal 417 | Topic: /move_base_simple/goal 418 | - Class: rviz/PublishPoint 419 | Single click: true 420 | Topic: /clicked_point 421 | Value: true 422 | Views: 423 | Current: 424 | Class: rviz/Orbit 425 | Distance: 179.57247924804688 426 | Enable Stereo Rendering: 427 | Stereo Eye Separation: 0.05999999865889549 428 | Stereo Focal Distance: 1 429 | Swap Stereo Eyes: false 430 | Value: false 431 | Field of View: 0.7853981852531433 432 | Focal Point: 433 | X: 0 434 | Y: 0 435 | Z: 0 436 | Focal Shape Fixed Size: false 437 | Focal Shape Size: 0.05000000074505806 438 | Invert Z Axis: false 439 | Name: Current View 440 | Near Clip Distance: 0.009999999776482582 441 | Pitch: 0.7853981852531433 442 | Target Frame: robot 443 | Yaw: 0.7853981852531433 444 | Saved: ~ 445 | Window Geometry: 446 | Displays: 447 | collapsed: true 448 | Height: 1016 449 | Hide Left Dock: true 450 | Hide Right Dock: true 451 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c7000000000000000000000001000001520000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000071800000052fc0100000002fb0000000800540069006d00650000000000000007180000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000004df0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 452 | Selection: 453 | collapsed: false 454 | Time: 455 | collapsed: false 456 | Tool Properties: 457 | collapsed: false 458 | Views: 459 | collapsed: true 460 | Width: 1247 461 | X: 673 462 | Y: 1107 463 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/include/fast_lio_sam_sc_qn.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_SC_QN_MAIN_H 2 | #define FAST_LIO_SAM_SC_QN_MAIN_H 3 | 4 | ///// common headers 5 | #include 6 | #include 7 | #include //time check 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include // pair, make_pair 14 | #include 15 | #include 16 | #include 17 | #include 18 | ///// ROS 19 | #include 20 | #include // get package_path 21 | #include // save map 22 | #include // to Quaternion_to_euler 23 | #include // to Quaternion_to_euler 24 | #include // createQuaternionFromRPY 25 | #include // tf <-> eigen 26 | #include // broadcaster 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | ///// GTSAM 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | ///// coded headers 44 | #include "loop_closure.h" 45 | #include "pose_pcd.hpp" 46 | #include "utilities.hpp" 47 | 48 | namespace fs = std::filesystem; 49 | using namespace std::chrono; 50 | typedef message_filters::sync_policies::ApproximateTime odom_pcd_sync_pol; 51 | 52 | //////////////////////////////////////////////////////////////////////////////////////////////////// 53 | class FastLioSamScQn 54 | { 55 | private: 56 | ///// basic params 57 | std::string map_frame_; 58 | std::string package_path_; 59 | std::string seq_name_; 60 | ///// shared data - odom and pcd 61 | std::mutex realtime_pose_mutex_, keyframes_mutex_; 62 | std::mutex graph_mutex_, vis_mutex_; 63 | Eigen::Matrix4d last_corrected_pose_ = Eigen::Matrix4d::Identity(); 64 | Eigen::Matrix4d odom_delta_ = Eigen::Matrix4d::Identity(); 65 | PosePcd current_frame_; 66 | std::vector keyframes_; 67 | int current_keyframe_idx_ = 0; 68 | ///// graph and values 69 | bool is_initialized_ = false; 70 | bool loop_added_flag_ = false; // for opt 71 | bool loop_added_flag_vis_ = false; // for vis 72 | std::shared_ptr isam_handler_ = nullptr; 73 | gtsam::NonlinearFactorGraph gtsam_graph_; 74 | gtsam::Values init_esti_; 75 | gtsam::Values corrected_esti_; 76 | double keyframe_thr_; 77 | double voxel_res_; 78 | int sub_key_num_; 79 | std::vector> loop_idx_pairs_; // for vis 80 | ///// visualize 81 | tf::TransformBroadcaster broadcaster_; 82 | pcl::PointCloud odoms_, corrected_odoms_; 83 | nav_msgs::Path odom_path_, corrected_path_; 84 | bool global_map_vis_switch_ = true; 85 | ///// results 86 | bool save_map_bag_ = false, save_map_pcd_ = false, save_in_kitti_format_ = false; 87 | ///// ros 88 | ros::NodeHandle nh_; 89 | ros::Publisher corrected_odom_pub_, corrected_path_pub_, odom_pub_, path_pub_; 90 | ros::Publisher corrected_current_pcd_pub_, corrected_pcd_map_pub_, loop_detection_pub_; 91 | ros::Publisher realtime_pose_pub_; 92 | ros::Publisher debug_src_pub_, debug_dst_pub_, debug_coarse_aligned_pub_, debug_fine_aligned_pub_; 93 | ros::Subscriber sub_save_flag_; 94 | ros::Timer loop_timer_, vis_timer_; 95 | // odom, pcd sync, and save flag subscribers 96 | std::shared_ptr> sub_odom_pcd_sync_ = nullptr; 97 | std::shared_ptr> sub_odom_ = nullptr; 98 | std::shared_ptr> sub_pcd_ = nullptr; 99 | ///// Loop closure 100 | std::shared_ptr loop_closure_; 101 | 102 | public: 103 | explicit FastLioSamScQn(const ros::NodeHandle &n_private); 104 | ~FastLioSamScQn(); 105 | 106 | private: 107 | // methods 108 | void updateOdomsAndPaths(const PosePcd &pose_pcd_in); 109 | bool checkIfKeyframe(const PosePcd &pose_pcd_in, const PosePcd &latest_pose_pcd); 110 | visualization_msgs::Marker getLoopMarkers(const gtsam::Values &corrected_esti_in); 111 | // cb 112 | void odomPcdCallback(const nav_msgs::OdometryConstPtr &odom_msg, 113 | const sensor_msgs::PointCloud2ConstPtr &pcd_msg); 114 | void saveFlagCallback(const std_msgs::String::ConstPtr &msg); 115 | void loopTimerFunc(const ros::TimerEvent &event); 116 | void visTimerFunc(const ros::TimerEvent &event); 117 | }; 118 | 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/include/loop_closure.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_SC_QN_LOOP_CLOSURE_H 2 | #define FAST_LIO_SAM_SC_QN_LOOP_CLOSURE_H 3 | 4 | ///// C++ common headers 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include // pair 11 | ///// PCL 12 | #include //pt 13 | #include //cloud 14 | ///// Eigen 15 | #include 16 | ///// Nano-GICP 17 | #include 18 | #include 19 | ///// Quatro 20 | #include 21 | ///// ScanContext 22 | #include 23 | ///// coded headers 24 | #include "pose_pcd.hpp" 25 | #include "utilities.hpp" 26 | using PcdPair = std::tuple, pcl::PointCloud>; 27 | 28 | struct NanoGICPConfig 29 | { 30 | int nano_thread_number_ = 0; 31 | int nano_correspondences_number_ = 15; 32 | int nano_max_iter_ = 32; 33 | int nano_ransac_max_iter_ = 5; 34 | double max_corr_dist_ = 2.0; 35 | double icp_score_thr_ = 10.0; 36 | double transformation_epsilon_ = 0.01; 37 | double euclidean_fitness_epsilon_ = 0.01; 38 | double ransac_outlier_rejection_threshold_ = 1.0; 39 | }; 40 | 41 | struct QuatroConfig 42 | { 43 | bool use_optimized_matching_ = true; 44 | bool estimat_scale_ = false; 45 | int quatro_max_num_corres_ = 500; 46 | int quatro_max_iter_ = 50; 47 | double quatro_distance_threshold_ = 30.0; 48 | double fpfh_normal_radius_ = 0.30; // It should be 2.5 - 3.0 * `voxel_res` 49 | double fpfh_radius_ = 0.50; // It should be 5.0 * `voxel_res` 50 | double noise_bound_ = 0.30; 51 | double rot_gnc_factor_ = 1.40; 52 | double rot_cost_diff_thr_ = 0.0001; 53 | }; 54 | 55 | struct LoopClosureConfig 56 | { 57 | bool enable_quatro_ = true; 58 | bool enable_submap_matching_ = true; 59 | int num_submap_keyframes_ = 10; 60 | double voxel_res_ = 0.1; 61 | double scancontext_max_correspondence_distance_; 62 | NanoGICPConfig gicp_config_; 63 | QuatroConfig quatro_config_; 64 | }; 65 | 66 | struct RegistrationOutput 67 | { 68 | bool is_valid_ = false; 69 | bool is_converged_ = false; 70 | double score_ = std::numeric_limits::max(); 71 | Eigen::Matrix4d pose_between_eig_ = Eigen::Matrix4d::Identity(); 72 | }; 73 | 74 | class LoopClosure 75 | { 76 | private: 77 | SCManager sc_manager_; 78 | nano_gicp::NanoGICP nano_gicp_; 79 | std::shared_ptr> quatro_handler_ = nullptr; 80 | int closest_keyframe_idx_ = -1; 81 | pcl::PointCloud::Ptr src_cloud_; 82 | pcl::PointCloud::Ptr dst_cloud_; 83 | pcl::PointCloud coarse_aligned_; 84 | pcl::PointCloud aligned_; 85 | LoopClosureConfig config_; 86 | 87 | public: 88 | explicit LoopClosure(const LoopClosureConfig &config); 89 | ~LoopClosure(); 90 | void updateScancontext(pcl::PointCloud cloud); 91 | int fetchCandidateKeyframeIdx(const PosePcd &query_keyframe, 92 | const std::vector &keyframes); 93 | PcdPair setSrcAndDstCloud(const std::vector &keyframes, 94 | const int src_idx, 95 | const int dst_idx, 96 | const int submap_range, 97 | const double voxel_res, 98 | const bool enable_quatro, 99 | const bool enable_submap_matching); 100 | RegistrationOutput icpAlignment(const pcl::PointCloud &src, 101 | const pcl::PointCloud &dst); 102 | RegistrationOutput coarseToFineAlignment(const pcl::PointCloud &src, 103 | const pcl::PointCloud &dst); 104 | RegistrationOutput performLoopClosure(const PosePcd &query_keyframe, 105 | const std::vector &keyframes, 106 | const int closest_keyframe_idx); 107 | pcl::PointCloud getSourceCloud(); 108 | pcl::PointCloud getTargetCloud(); 109 | pcl::PointCloud getCoarseAlignedCloud(); 110 | pcl::PointCloud getFinalAlignedCloud(); 111 | int getClosestKeyframeidx(); 112 | }; 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/include/pose_pcd.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_SC_QN_POSE_PCD_HPP 2 | #define FAST_LIO_SAM_SC_QN_POSE_PCD_HPP 3 | 4 | ///// coded headers 5 | #include "utilities.hpp" 6 | 7 | struct PosePcd 8 | { 9 | pcl::PointCloud pcd_; 10 | Eigen::Matrix4d pose_eig_ = Eigen::Matrix4d::Identity(); 11 | Eigen::Matrix4d pose_corrected_eig_ = Eigen::Matrix4d::Identity(); 12 | double timestamp_; 13 | int idx_; 14 | bool processed_ = false; 15 | PosePcd() {} 16 | PosePcd(const nav_msgs::Odometry &odom_in, 17 | const sensor_msgs::PointCloud2 &pcd_in, 18 | const int &idx_in); 19 | }; 20 | 21 | inline PosePcd::PosePcd(const nav_msgs::Odometry &odom_in, 22 | const sensor_msgs::PointCloud2 &pcd_in, 23 | const int &idx_in) 24 | { 25 | tf::Quaternion q(odom_in.pose.pose.orientation.x, 26 | odom_in.pose.pose.orientation.y, 27 | odom_in.pose.pose.orientation.z, 28 | odom_in.pose.pose.orientation.w); 29 | tf::Matrix3x3 rot_mat_tf(q); 30 | Eigen::Matrix3d rot_mat_eig; 31 | tf::matrixTFToEigen(rot_mat_tf, rot_mat_eig); 32 | pose_eig_.block<3, 3>(0, 0) = rot_mat_eig; 33 | pose_eig_(0, 3) = odom_in.pose.pose.position.x; 34 | pose_eig_(1, 3) = odom_in.pose.pose.position.y; 35 | pose_eig_(2, 3) = odom_in.pose.pose.position.z; 36 | pose_corrected_eig_ = pose_eig_; 37 | pcl::PointCloud tmp_pcd; 38 | pcl::fromROSMsg(pcd_in, tmp_pcd); 39 | pcd_ = transformPcd(tmp_pcd, pose_eig_.inverse()); // FAST-LIO publish data in world frame, 40 | // so save it in LiDAR frame 41 | timestamp_ = odom_in.header.stamp.toSec(); 42 | idx_ = idx_in; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/include/utilities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_SC_QN_UTILITIES_HPP 2 | #define FAST_LIO_SAM_SC_QN_UTILITIES_HPP 3 | 4 | ///// common headers 5 | #include 6 | ///// ROS 7 | #include 8 | #include // to Quaternion_to_euler 9 | #include // to Quaternion_to_euler 10 | #include // createQuaternionFromRPY 11 | #include // tf <-> eigen 12 | #include 13 | #include 14 | #include 15 | #include 16 | ///// PCL 17 | #include //pt 18 | #include //cloud 19 | #include //ros<->pcl 20 | #include //ros<->pcl 21 | #include 22 | #include //voxelgrid 23 | ///// Eigen 24 | #include // whole Eigen library: Sparse(Linearalgebra) + Dense(Core+Geometry+LU+Cholesky+SVD+QR+Eigenvalues) 25 | ///// GTSAM 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | using PointType = pcl::PointXYZI; 37 | 38 | inline pcl::PointCloud::Ptr voxelizePcd(const pcl::PointCloud &pcd_in, 39 | const float voxel_res) 40 | { 41 | static pcl::VoxelGrid voxelgrid; 42 | voxelgrid.setLeafSize(voxel_res, voxel_res, voxel_res); 43 | pcl::PointCloud::Ptr pcd_in_ptr(new pcl::PointCloud); 44 | pcl::PointCloud::Ptr pcd_out(new pcl::PointCloud); 45 | pcd_in_ptr->reserve(pcd_in.size()); 46 | pcd_out->reserve(pcd_in.size()); 47 | *pcd_in_ptr = pcd_in; 48 | voxelgrid.setInputCloud(pcd_in_ptr); 49 | voxelgrid.filter(*pcd_out); 50 | return pcd_out; 51 | } 52 | 53 | inline pcl::PointCloud::Ptr voxelizePcd(const pcl::PointCloud::Ptr &pcd_in, 54 | const float voxel_res) 55 | { 56 | static pcl::VoxelGrid voxelgrid; 57 | voxelgrid.setLeafSize(voxel_res, voxel_res, voxel_res); 58 | pcl::PointCloud::Ptr pcd_out(new pcl::PointCloud); 59 | pcd_out->reserve(pcd_in->size()); 60 | voxelgrid.setInputCloud(pcd_in); 61 | voxelgrid.filter(*pcd_out); 62 | return pcd_out; 63 | } 64 | 65 | ////////////////////////////////////////////////////////////////////// 66 | ///// conversions 67 | inline gtsam::Pose3 poseEigToGtsamPose(const Eigen::Matrix4d &pose_eig_in) 68 | { 69 | double r, p, y; 70 | tf::Matrix3x3 mat; 71 | tf::matrixEigenToTF(pose_eig_in.block<3, 3>(0, 0), mat); 72 | mat.getRPY(r, p, y); 73 | return gtsam::Pose3(gtsam::Rot3::RzRyRx(r, p, y), 74 | gtsam::Point3(pose_eig_in(0, 3), pose_eig_in(1, 3), pose_eig_in(2, 3))); 75 | } 76 | 77 | inline Eigen::Matrix4d gtsamPoseToPoseEig(const gtsam::Pose3 >sam_pose_in) 78 | { 79 | Eigen::Matrix4d pose_eig_out = Eigen::Matrix4d::Identity(); 80 | tf::Quaternion quat = tf::createQuaternionFromRPY(gtsam_pose_in.rotation().roll(), 81 | gtsam_pose_in.rotation().pitch(), 82 | gtsam_pose_in.rotation().yaw()); 83 | tf::Matrix3x3 mat(quat); 84 | Eigen::Matrix3d tmp_rot_mat; 85 | tf::matrixTFToEigen(mat, tmp_rot_mat); 86 | pose_eig_out.block<3, 3>(0, 0) = tmp_rot_mat; 87 | pose_eig_out(0, 3) = gtsam_pose_in.translation().x(); 88 | pose_eig_out(1, 3) = gtsam_pose_in.translation().y(); 89 | pose_eig_out(2, 3) = gtsam_pose_in.translation().z(); 90 | return pose_eig_out; 91 | } 92 | 93 | inline geometry_msgs::PoseStamped poseEigToPoseStamped(const Eigen::Matrix4d &pose_eig_in, 94 | std::string frame_id = "map") 95 | { 96 | double r, p, y; 97 | tf::Matrix3x3 mat; 98 | tf::matrixEigenToTF(pose_eig_in.block<3, 3>(0, 0), mat); 99 | mat.getRPY(r, p, y); 100 | tf::Quaternion quat = tf::createQuaternionFromRPY(r, p, y); 101 | geometry_msgs::PoseStamped pose; 102 | pose.header.frame_id = frame_id; 103 | pose.pose.position.x = pose_eig_in(0, 3); 104 | pose.pose.position.y = pose_eig_in(1, 3); 105 | pose.pose.position.z = pose_eig_in(2, 3); 106 | pose.pose.orientation.w = quat.getW(); 107 | pose.pose.orientation.x = quat.getX(); 108 | pose.pose.orientation.y = quat.getY(); 109 | pose.pose.orientation.z = quat.getZ(); 110 | return pose; 111 | } 112 | 113 | inline tf::Transform poseEigToROSTf(const Eigen::Matrix4d &pose) 114 | { 115 | Eigen::Quaterniond quat(pose.block<3, 3>(0, 0)); 116 | tf::Transform transform; 117 | transform.setOrigin(tf::Vector3(pose(0, 3), pose(1, 3), pose(2, 3))); 118 | transform.setRotation(tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w())); 119 | return transform; 120 | } 121 | 122 | inline tf::Transform poseStampedToROSTf(const geometry_msgs::PoseStamped &pose) 123 | { 124 | tf::Transform transform; 125 | transform.setOrigin(tf::Vector3(pose.pose.position.x, 126 | pose.pose.position.y, 127 | pose.pose.position.z)); 128 | transform.setRotation(tf::Quaternion(pose.pose.orientation.x, 129 | pose.pose.orientation.y, 130 | pose.pose.orientation.z, 131 | pose.pose.orientation.w)); 132 | return transform; 133 | } 134 | 135 | inline geometry_msgs::PoseStamped gtsamPoseToPoseStamped(const gtsam::Pose3 >sam_pose_in, 136 | std::string frame_id = "map") 137 | { 138 | tf::Quaternion quat = tf::createQuaternionFromRPY(gtsam_pose_in.rotation().roll(), 139 | gtsam_pose_in.rotation().pitch(), 140 | gtsam_pose_in.rotation().yaw()); 141 | geometry_msgs::PoseStamped pose; 142 | pose.header.frame_id = frame_id; 143 | pose.pose.position.x = gtsam_pose_in.translation().x(); 144 | pose.pose.position.y = gtsam_pose_in.translation().y(); 145 | pose.pose.position.z = gtsam_pose_in.translation().z(); 146 | pose.pose.orientation.w = quat.getW(); 147 | pose.pose.orientation.x = quat.getX(); 148 | pose.pose.orientation.y = quat.getY(); 149 | pose.pose.orientation.z = quat.getZ(); 150 | return pose; 151 | } 152 | 153 | template 154 | inline sensor_msgs::PointCloud2 pclToPclRos(pcl::PointCloud cloud, 155 | std::string frame_id = "map") 156 | { 157 | sensor_msgs::PointCloud2 cloud_ROS; 158 | pcl::toROSMsg(cloud, cloud_ROS); 159 | cloud_ROS.header.frame_id = frame_id; 160 | return cloud_ROS; 161 | } 162 | 163 | ///// transformation 164 | template 165 | inline pcl::PointCloud transformPcd(const pcl::PointCloud &cloud_in, 166 | const Eigen::Matrix4d &pose_tf) 167 | { 168 | if (cloud_in.size() == 0) 169 | { 170 | return cloud_in; 171 | } 172 | pcl::PointCloud pcl_out = cloud_in; 173 | pcl::transformPointCloud(cloud_in, pcl_out, pose_tf); 174 | return pcl_out; 175 | } 176 | 177 | #endif 178 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_lio_sam_sc_qn 4 | 0.0.0 5 | 6 | 7 | This is a SLAM implementation combining FAST-LIO2 with pose graph optimization and loop closing based on ScanContext, Quatro, and Nano-GICP 8 | 9 | Eungchang Mason Lee 10 | 11 | TODO 12 | 13 | catkin 14 | roscpp 15 | roslib 16 | rosbag 17 | std_msgs 18 | geometry_msgs 19 | nav_msgs 20 | sensor_msgs 21 | visualization_msgs 22 | tf 23 | tf_conversions 24 | pcl_ros 25 | pcl_conversions 26 | message_filters 27 | nano_gicp 28 | quatro 29 | 30 | roscpp 31 | roslib 32 | rosbag 33 | std_msgs 34 | geometry_msgs 35 | nav_msgs 36 | sensor_msgs 37 | visualization_msgs 38 | tf 39 | tf_conversions 40 | pcl_ros 41 | pcl_conversions 42 | message_filters 43 | nano_gicp 44 | quatro 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/src/fast_lio_sam_sc_qn.cpp: -------------------------------------------------------------------------------- 1 | #include "fast_lio_sam_sc_qn.h" 2 | 3 | FastLioSamScQn::FastLioSamScQn(const ros::NodeHandle &n_private): 4 | nh_(n_private) 5 | { 6 | ////// ROS params 7 | double loop_update_hz, vis_hz; 8 | LoopClosureConfig lc_config; 9 | auto &gc = lc_config.gicp_config_; 10 | auto &qc = lc_config.quatro_config_; 11 | /* basic */ 12 | nh_.param("/basic/map_frame", map_frame_, "map"); 13 | nh_.param("/basic/loop_update_hz", loop_update_hz, 1.0); 14 | nh_.param("/basic/vis_hz", vis_hz, 0.5); 15 | nh_.param("/save_voxel_resolution", voxel_res_, 0.3); 16 | nh_.param("/quatro_nano_gicp_voxel_resolution", lc_config.voxel_res_, 0.3); 17 | /* keyframe */ 18 | nh_.param("/keyframe/keyframe_threshold", keyframe_thr_, 1.0); 19 | nh_.param("/keyframe/nusubmap_keyframes", lc_config.num_submap_keyframes_, 5); 20 | nh_.param("/keyframe/enable_submap_matching", lc_config.enable_submap_matching_, false); 21 | /* ScanContext */ 22 | nh_.param("/scancontext_max_correspondence_distance", 23 | lc_config.scancontext_max_correspondence_distance_, 24 | 35.0); 25 | /* nano (GICP config) */ 26 | nh_.param("/nano_gicp/thread_number", gc.nano_thread_number_, 0); 27 | nh_.param("/nano_gicp/icp_score_threshold", gc.icp_score_thr_, 10.0); 28 | nh_.param("/nano_gicp/correspondences_number", gc.nano_correspondences_number_, 15); 29 | nh_.param("/nano_gicp/max_correspondence_distance", gc.max_corr_dist_, 0.01); 30 | nh_.param("/nano_gicp/max_iter", gc.nano_max_iter_, 32); 31 | nh_.param("/nano_gicp/transformation_epsilon", gc.transformation_epsilon_, 0.01); 32 | nh_.param("/nano_gicp/euclidean_fitness_epsilon", gc.euclidean_fitness_epsilon_, 0.01); 33 | nh_.param("/nano_gicp/ransac/max_iter", gc.nano_ransac_max_iter_, 5); 34 | nh_.param("/nano_gicp/ransac/outlier_rejection_threshold", gc.ransac_outlier_rejection_threshold_, 1.0); 35 | /* quatro (Quatro config) */ 36 | nh_.param("/quatro/enable", lc_config.enable_quatro_, false); 37 | nh_.param("/quatro/optimize_matching", qc.use_optimized_matching_, true); 38 | nh_.param("/quatro/distance_threshold", qc.quatro_distance_threshold_, 30.0); 39 | nh_.param("/quatro/max_nucorrespondences", qc.quatro_max_num_corres_, 200); 40 | nh_.param("/quatro/fpfh_normal_radius", qc.fpfh_normal_radius_, 0.3); 41 | nh_.param("/quatro/fpfh_radius", qc.fpfh_radius_, 0.5); 42 | nh_.param("/quatro/estimating_scale", qc.estimat_scale_, false); 43 | nh_.param("/quatro/noise_bound", qc.noise_bound_, 0.3); 44 | nh_.param("/quatro/rotation/gnc_factor", qc.rot_gnc_factor_, 1.4); 45 | nh_.param("/quatro/rotation/rot_cost_diff_threshold", qc.rot_cost_diff_thr_, 0.0001); 46 | nh_.param("/quatro/rotation/numax_iter", qc.quatro_max_iter_, 50); 47 | /* results */ 48 | nh_.param("/result/save_map_bag", save_map_bag_, false); 49 | nh_.param("/result/save_map_pcd", save_map_pcd_, false); 50 | nh_.param("/result/save_in_kitti_format", save_in_kitti_format_, false); 51 | nh_.param("/result/seq_name", seq_name_, ""); 52 | loop_closure_.reset(new LoopClosure(lc_config)); 53 | /* Initialization of GTSAM */ 54 | gtsam::ISAM2Params isam_params_; 55 | isam_params_.relinearizeThreshold = 0.01; 56 | isam_params_.relinearizeSkip = 1; 57 | isam_handler_ = std::make_shared(isam_params_); 58 | /* ROS things */ 59 | odom_path_.header.frame_id = map_frame_; 60 | corrected_path_.header.frame_id = map_frame_; 61 | package_path_ = ros::package::getPath("fast_lio_sam_sc_qn"); 62 | /* publishers */ 63 | odom_pub_ = nh_.advertise("/ori_odom", 10, true); 64 | path_pub_ = nh_.advertise("/ori_path", 10, true); 65 | corrected_odom_pub_ = nh_.advertise("/corrected_odom", 10, true); 66 | corrected_path_pub_ = nh_.advertise("/corrected_path", 10, true); 67 | corrected_pcd_map_pub_ = nh_.advertise("/corrected_map", 10, true); 68 | corrected_current_pcd_pub_ = nh_.advertise("/corrected_current_pcd", 10, true); 69 | loop_detection_pub_ = nh_.advertise("/loop_detection", 10, true); 70 | realtime_pose_pub_ = nh_.advertise("/pose_stamped", 10); 71 | debug_src_pub_ = nh_.advertise("/src", 10, true); 72 | debug_dst_pub_ = nh_.advertise("/dst", 10, true); 73 | debug_coarse_aligned_pub_ = nh_.advertise("/coarse_aligned_quatro", 10, true); 74 | debug_fine_aligned_pub_ = nh_.advertise("/fine_aligned_nano_gicp", 10, true); 75 | /* subscribers */ 76 | sub_odom_ = std::make_shared>(nh_, "/Odometry", 10); 77 | sub_pcd_ = std::make_shared>(nh_, "/cloud_registered", 10); 78 | sub_odom_pcd_sync_ = std::make_shared>(odom_pcd_sync_pol(10), *sub_odom_, *sub_pcd_); 79 | sub_odom_pcd_sync_->registerCallback(boost::bind(&FastLioSamScQn::odomPcdCallback, this, _1, _2)); 80 | sub_save_flag_ = nh_.subscribe("/save_dir", 1, &FastLioSamScQn::saveFlagCallback, this); 81 | /* Timers */ 82 | loop_timer_ = nh_.createTimer(ros::Duration(1 / loop_update_hz), &FastLioSamScQn::loopTimerFunc, this); 83 | vis_timer_ = nh_.createTimer(ros::Duration(1 / vis_hz), &FastLioSamScQn::visTimerFunc, this); 84 | ROS_INFO("Main class, starting node..."); 85 | } 86 | 87 | void FastLioSamScQn::odomPcdCallback(const nav_msgs::OdometryConstPtr &odom_msg, 88 | const sensor_msgs::PointCloud2ConstPtr &pcd_msg) 89 | { 90 | Eigen::Matrix4d last_odom_tf; 91 | last_odom_tf = current_frame_.pose_eig_; // to calculate delta 92 | current_frame_ = PosePcd(*odom_msg, *pcd_msg, current_keyframe_idx_); // to be checked if keyframe or not 93 | high_resolution_clock::time_point t1 = high_resolution_clock::now(); 94 | { 95 | //// 1. realtime pose = last corrected odom * delta (last -> current) 96 | std::lock_guard lock(realtime_pose_mutex_); 97 | odom_delta_ = odom_delta_ * last_odom_tf.inverse() * current_frame_.pose_eig_; 98 | current_frame_.pose_corrected_eig_ = last_corrected_pose_ * odom_delta_; 99 | realtime_pose_pub_.publish(poseEigToPoseStamped(current_frame_.pose_corrected_eig_, map_frame_)); 100 | broadcaster_.sendTransform(tf::StampedTransform(poseEigToROSTf(current_frame_.pose_corrected_eig_), 101 | ros::Time::now(), 102 | map_frame_, 103 | "robot")); 104 | } 105 | corrected_current_pcd_pub_.publish(pclToPclRos(transformPcd(current_frame_.pcd_, current_frame_.pose_corrected_eig_), map_frame_)); 106 | 107 | if (!is_initialized_) //// init only once 108 | { 109 | // others 110 | keyframes_.push_back(current_frame_); 111 | updateOdomsAndPaths(current_frame_); 112 | // graph 113 | auto variance_vector = (gtsam::Vector(6) << 1e-4, 1e-4, 1e-4, 1e-2, 1e-2, 1e-2).finished(); // rad*rad, 114 | // meter*meter 115 | gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Variances(variance_vector); 116 | gtsam_graph_.add(gtsam::PriorFactor(0, poseEigToGtsamPose(current_frame_.pose_eig_), prior_noise)); 117 | init_esti_.insert(current_keyframe_idx_, poseEigToGtsamPose(current_frame_.pose_eig_)); 118 | current_keyframe_idx_++; 119 | // ScanContext 120 | loop_closure_->updateScancontext(current_frame_.pcd_); 121 | is_initialized_ = true; 122 | } 123 | else 124 | { 125 | //// 2. check if keyframe 126 | high_resolution_clock::time_point t2 = high_resolution_clock::now(); 127 | if (checkIfKeyframe(current_frame_, keyframes_.back())) 128 | { 129 | // 2-2. if so, save 130 | { 131 | std::lock_guard lock(keyframes_mutex_); 132 | keyframes_.push_back(current_frame_); 133 | } 134 | // 2-3. if so, add to graph 135 | auto variance_vector = (gtsam::Vector(6) << 1e-4, 1e-4, 1e-4, 1e-2, 1e-2, 1e-2).finished(); 136 | gtsam::noiseModel::Diagonal::shared_ptr odom_noise = gtsam::noiseModel::Diagonal::Variances(variance_vector); 137 | gtsam::Pose3 pose_from = poseEigToGtsamPose(keyframes_[current_keyframe_idx_ - 1].pose_corrected_eig_); 138 | gtsam::Pose3 pose_to = poseEigToGtsamPose(current_frame_.pose_corrected_eig_); 139 | { 140 | std::lock_guard lock(graph_mutex_); 141 | gtsam_graph_.add(gtsam::BetweenFactor(current_keyframe_idx_ - 1, 142 | current_keyframe_idx_, 143 | pose_from.between(pose_to), 144 | odom_noise)); 145 | init_esti_.insert(current_keyframe_idx_, pose_to); 146 | } 147 | current_keyframe_idx_++; 148 | // 2-4. if so, update ScanContext 149 | loop_closure_->updateScancontext(current_frame_.pcd_); 150 | 151 | //// 3. vis 152 | high_resolution_clock::time_point t3 = high_resolution_clock::now(); 153 | { 154 | std::lock_guard lock(vis_mutex_); 155 | updateOdomsAndPaths(current_frame_); 156 | } 157 | 158 | //// 4. optimize with graph 159 | high_resolution_clock::time_point t4 = high_resolution_clock::now(); 160 | // m_corrected_esti = gtsam::LevenbergMarquardtOptimizer(m_gtsam_graph, init_esti_).optimize(); // cf. isam.update vs values.LM.optimize 161 | { 162 | std::lock_guard lock(graph_mutex_); 163 | isam_handler_->update(gtsam_graph_, init_esti_); 164 | isam_handler_->update(); 165 | if (loop_added_flag_) // https://github.com/TixiaoShan/LIO-SAM/issues/5#issuecomment-653752936 166 | { 167 | isam_handler_->update(); 168 | isam_handler_->update(); 169 | isam_handler_->update(); 170 | } 171 | gtsam_graph_.resize(0); 172 | init_esti_.clear(); 173 | } 174 | 175 | //// 5. handle corrected results 176 | // get corrected poses and reset odom delta (for realtime pose pub) 177 | high_resolution_clock::time_point t5 = high_resolution_clock::now(); 178 | { 179 | std::lock_guard lock(realtime_pose_mutex_); 180 | corrected_esti_ = isam_handler_->calculateEstimate(); 181 | last_corrected_pose_ = gtsamPoseToPoseEig(corrected_esti_.at(corrected_esti_.size() - 1)); 182 | odom_delta_ = Eigen::Matrix4d::Identity(); 183 | } 184 | // correct poses in keyframes 185 | if (loop_added_flag_) 186 | { 187 | std::lock_guard lock(keyframes_mutex_); 188 | for (size_t i = 0; i < corrected_esti_.size(); ++i) 189 | { 190 | keyframes_[i].pose_corrected_eig_ = gtsamPoseToPoseEig(corrected_esti_.at(i)); 191 | } 192 | loop_added_flag_ = false; 193 | } 194 | high_resolution_clock::time_point t6 = high_resolution_clock::now(); 195 | 196 | ROS_INFO("real: %.1f, key_add: %.1f, vis: %.1f, opt: %.1f, res: %.1f, tot: %.1fms", 197 | duration_cast(t2 - t1).count() / 1e3, 198 | duration_cast(t3 - t2).count() / 1e3, 199 | duration_cast(t4 - t3).count() / 1e3, 200 | duration_cast(t5 - t4).count() / 1e3, 201 | duration_cast(t6 - t5).count() / 1e3, 202 | duration_cast(t6 - t1).count() / 1e3); 203 | } 204 | } 205 | return; 206 | } 207 | 208 | void FastLioSamScQn::loopTimerFunc(const ros::TimerEvent &event) 209 | { 210 | auto &latest_keyframe = keyframes_.back(); 211 | if (!is_initialized_ || keyframes_.empty() || latest_keyframe.processed_) 212 | { 213 | return; 214 | } 215 | latest_keyframe.processed_ = true; 216 | 217 | high_resolution_clock::time_point t1 = high_resolution_clock::now(); 218 | const int closest_keyframe_idx = loop_closure_->fetchCandidateKeyframeIdx(latest_keyframe, keyframes_); 219 | if (closest_keyframe_idx < 0) 220 | { 221 | return; 222 | } 223 | 224 | const RegistrationOutput ®_output = loop_closure_->performLoopClosure(latest_keyframe, keyframes_, closest_keyframe_idx); 225 | if (reg_output.is_valid_) 226 | { 227 | ROS_INFO("\033[1;32mLoop closure accepted. Score: %.3f\033[0m", reg_output.score_); 228 | const auto &score = reg_output.score_; 229 | gtsam::Pose3 pose_from = poseEigToGtsamPose(reg_output.pose_between_eig_ * latest_keyframe.pose_corrected_eig_); // IMPORTANT: take care of the order 230 | gtsam::Pose3 pose_to = poseEigToGtsamPose(keyframes_[closest_keyframe_idx].pose_corrected_eig_); 231 | auto variance_vector = (gtsam::Vector(6) << score, score, score, score, score, score).finished(); 232 | gtsam::noiseModel::Diagonal::shared_ptr loop_noise = gtsam::noiseModel::Diagonal::Variances(variance_vector); 233 | { 234 | std::lock_guard lock(graph_mutex_); 235 | gtsam_graph_.add(gtsam::BetweenFactor(latest_keyframe.idx_, 236 | closest_keyframe_idx, 237 | pose_from.between(pose_to), 238 | loop_noise)); 239 | } 240 | loop_idx_pairs_.push_back({latest_keyframe.idx_, closest_keyframe_idx}); // for vis 241 | loop_added_flag_vis_ = true; 242 | loop_added_flag_ = true; 243 | } 244 | else 245 | { 246 | ROS_WARN("Loop closure rejected. Score: %.3f", reg_output.score_); 247 | } 248 | high_resolution_clock::time_point t2 = high_resolution_clock::now(); 249 | 250 | debug_src_pub_.publish(pclToPclRos(loop_closure_->getSourceCloud(), map_frame_)); 251 | debug_dst_pub_.publish(pclToPclRos(loop_closure_->getTargetCloud(), map_frame_)); 252 | debug_fine_aligned_pub_.publish(pclToPclRos(loop_closure_->getFinalAlignedCloud(), map_frame_)); 253 | debug_coarse_aligned_pub_.publish(pclToPclRos(loop_closure_->getCoarseAlignedCloud(), map_frame_)); 254 | 255 | ROS_INFO("loop: %.1f", duration_cast(t2 - t1).count() / 1e3); 256 | return; 257 | } 258 | 259 | void FastLioSamScQn::visTimerFunc(const ros::TimerEvent &event) 260 | { 261 | if (!is_initialized_) 262 | { 263 | return; 264 | } 265 | 266 | high_resolution_clock::time_point tv1 = high_resolution_clock::now(); 267 | //// 1. if loop closed, correct vis data 268 | if (loop_added_flag_vis_) 269 | // copy and ready 270 | { 271 | gtsam::Values corrected_esti_copied; 272 | pcl::PointCloud corrected_odoms; 273 | nav_msgs::Path corrected_path; 274 | { 275 | std::lock_guard lock(realtime_pose_mutex_); 276 | corrected_esti_copied = corrected_esti_; 277 | } 278 | // correct pose and path 279 | for (size_t i = 0; i < corrected_esti_copied.size(); ++i) 280 | { 281 | gtsam::Pose3 pose_ = corrected_esti_copied.at(i); 282 | corrected_odoms.points.emplace_back(pose_.translation().x(), pose_.translation().y(), pose_.translation().z()); 283 | corrected_path.poses.push_back(gtsamPoseToPoseStamped(pose_, map_frame_)); 284 | } 285 | // update vis of loop constraints 286 | if (!loop_idx_pairs_.empty()) 287 | { 288 | loop_detection_pub_.publish(getLoopMarkers(corrected_esti_copied)); 289 | } 290 | // update with corrected data 291 | { 292 | std::lock_guard lock(vis_mutex_); 293 | corrected_odoms_ = corrected_odoms; 294 | corrected_path_.poses = corrected_path.poses; 295 | } 296 | loop_added_flag_vis_ = false; 297 | } 298 | //// 2. publish odoms, paths 299 | { 300 | std::lock_guard lock(vis_mutex_); 301 | odom_pub_.publish(pclToPclRos(odoms_, map_frame_)); 302 | path_pub_.publish(odom_path_); 303 | corrected_odom_pub_.publish(pclToPclRos(corrected_odoms_, map_frame_)); 304 | corrected_path_pub_.publish(corrected_path_); 305 | } 306 | 307 | //// 3. global map 308 | if (global_map_vis_switch_ && corrected_pcd_map_pub_.getNumSubscribers() > 0) // save time, only once 309 | { 310 | pcl::PointCloud::Ptr corrected_map(new pcl::PointCloud()); 311 | corrected_map->reserve(keyframes_[0].pcd_.size() * keyframes_.size()); // it's an approximated size 312 | { 313 | std::lock_guard lock(keyframes_mutex_); 314 | for (size_t i = 0; i < keyframes_.size(); ++i) 315 | { 316 | *corrected_map += transformPcd(keyframes_[i].pcd_, keyframes_[i].pose_corrected_eig_); 317 | } 318 | } 319 | const auto &voxelized_map = voxelizePcd(corrected_map, voxel_res_); 320 | corrected_pcd_map_pub_.publish(pclToPclRos(*voxelized_map, map_frame_)); 321 | global_map_vis_switch_ = false; 322 | } 323 | if (!global_map_vis_switch_ && corrected_pcd_map_pub_.getNumSubscribers() == 0) 324 | { 325 | global_map_vis_switch_ = true; 326 | } 327 | high_resolution_clock::time_point tv2 = high_resolution_clock::now(); 328 | ROS_INFO("vis: %.1fms", duration_cast(tv2 - tv1).count() / 1e3); 329 | return; 330 | } 331 | 332 | void FastLioSamScQn::saveFlagCallback(const std_msgs::String::ConstPtr &msg) 333 | { 334 | std::string save_dir = msg->data != "" ? msg->data : package_path_; 335 | 336 | // save scans as individual pcd files and poses in KITTI format 337 | // Delete the scans folder if it exists and create a new one 338 | std::string seq_directory = save_dir + "/" + seq_name_; 339 | std::string scans_directory = seq_directory + "/scans"; 340 | if (save_in_kitti_format_) 341 | { 342 | ROS_INFO("\033[32;1mScans are saved in %s, following the KITTI and TUM format\033[0m", scans_directory.c_str()); 343 | if (fs::exists(seq_directory)) 344 | { 345 | fs::remove_all(seq_directory); 346 | } 347 | fs::create_directories(scans_directory); 348 | 349 | std::ofstream kitti_pose_file(seq_directory + "/poses_kitti.txt"); 350 | std::ofstream tum_pose_file(seq_directory + "/poses_tum.txt"); 351 | tum_pose_file << "#timestamp x y z qx qy qz qw\n"; 352 | { 353 | std::lock_guard lock(keyframes_mutex_); 354 | for (size_t i = 0; i < keyframes_.size(); ++i) 355 | { 356 | // Save the point cloud 357 | std::stringstream ss_; 358 | ss_ << scans_directory << "/" << std::setw(6) << std::setfill('0') << i << ".pcd"; 359 | ROS_INFO("Saving %s...", ss_.str().c_str()); 360 | pcl::io::savePCDFileASCII(ss_.str(), keyframes_[i].pcd_); 361 | 362 | // Save the pose in KITTI format 363 | const auto &pose_ = keyframes_[i].pose_corrected_eig_; 364 | kitti_pose_file << pose_(0, 0) << " " << pose_(0, 1) << " " << pose_(0, 2) << " " 365 | << pose_(0, 3) << " " << pose_(1, 0) << " " << pose_(1, 1) << " " 366 | << pose_(1, 2) << " " << pose_(1, 3) << " " << pose_(2, 0) << " " 367 | << pose_(2, 1) << " " << pose_(2, 2) << " " << pose_(2, 3) << "\n"; 368 | 369 | const auto &lidar_optim_pose_ = poseEigToPoseStamped(keyframes_[i].pose_corrected_eig_); 370 | tum_pose_file << std::fixed << std::setprecision(8) << keyframes_[i].timestamp_ 371 | << " " << lidar_optim_pose_.pose.position.x << " " 372 | << lidar_optim_pose_.pose.position.y << " " 373 | << lidar_optim_pose_.pose.position.z << " " 374 | << lidar_optim_pose_.pose.orientation.x << " " 375 | << lidar_optim_pose_.pose.orientation.y << " " 376 | << lidar_optim_pose_.pose.orientation.z << " " 377 | << lidar_optim_pose_.pose.orientation.w << "\n"; 378 | } 379 | } 380 | kitti_pose_file.close(); 381 | tum_pose_file.close(); 382 | ROS_INFO("\033[32;1mScans and poses saved in .pcd and KITTI format\033[0m"); 383 | } 384 | 385 | if (save_map_bag_) 386 | { 387 | rosbag::Bag bag; 388 | bag.open(package_path_ + "/result.bag", rosbag::bagmode::Write); 389 | { 390 | std::lock_guard lock(keyframes_mutex_); 391 | for (size_t i = 0; i < keyframes_.size(); ++i) 392 | { 393 | ros::Time time; 394 | time.fromSec(keyframes_[i].timestamp_); 395 | bag.write("/keyframe_pcd", time, pclToPclRos(keyframes_[i].pcd_, map_frame_)); 396 | bag.write("/keyframe_pose", time, poseEigToPoseStamped(keyframes_[i].pose_corrected_eig_)); 397 | } 398 | } 399 | bag.close(); 400 | ROS_INFO("\033[36;1mResult saved in .bag format!!!\033[0m"); 401 | } 402 | 403 | if (save_map_pcd_) 404 | { 405 | pcl::PointCloud::Ptr corrected_map(new pcl::PointCloud()); 406 | corrected_map->reserve(keyframes_[0].pcd_.size() * keyframes_.size()); // it's an approximated size 407 | { 408 | std::lock_guard lock(keyframes_mutex_); 409 | for (size_t i = 0; i < keyframes_.size(); ++i) 410 | { 411 | *corrected_map += transformPcd(keyframes_[i].pcd_, keyframes_[i].pose_corrected_eig_); 412 | } 413 | } 414 | const auto &voxelized_map = voxelizePcd(corrected_map, voxel_res_); 415 | pcl::io::savePCDFileASCII(seq_directory + "/" + seq_name_ + "_map.pcd", *voxelized_map); 416 | ROS_INFO("\033[32;1mAccumulated map cloud saved in .pcd format\033[0m"); 417 | } 418 | } 419 | 420 | FastLioSamScQn::~FastLioSamScQn() 421 | { 422 | // save map 423 | if (save_map_bag_) 424 | { 425 | rosbag::Bag bag; 426 | bag.open(package_path_ + "/result.bag", rosbag::bagmode::Write); 427 | { 428 | std::lock_guard lock(keyframes_mutex_); 429 | for (size_t i = 0; i < keyframes_.size(); ++i) 430 | { 431 | ros::Time time; 432 | time.fromSec(keyframes_[i].timestamp_); 433 | bag.write("/keyframe_pcd", time, pclToPclRos(keyframes_[i].pcd_, map_frame_)); 434 | bag.write("/keyframe_pose", time, poseEigToPoseStamped(keyframes_[i].pose_corrected_eig_)); 435 | } 436 | } 437 | bag.close(); 438 | ROS_INFO("\033[36;1mResult saved in .bag format!!!\033[0m"); 439 | } 440 | if (save_map_pcd_) 441 | { 442 | pcl::PointCloud::Ptr corrected_map(new pcl::PointCloud()); 443 | corrected_map->reserve(keyframes_[0].pcd_.size() * keyframes_.size()); // it's an approximated size 444 | { 445 | std::lock_guard lock(keyframes_mutex_); 446 | for (size_t i = 0; i < keyframes_.size(); ++i) 447 | { 448 | *corrected_map += transformPcd(keyframes_[i].pcd_, keyframes_[i].pose_corrected_eig_); 449 | } 450 | } 451 | const auto &voxelized_map = voxelizePcd(corrected_map, voxel_res_); 452 | pcl::io::savePCDFileASCII(package_path_ + "/result.pcd", *voxelized_map); 453 | ROS_INFO("\033[32;1mResult saved in .pcd format!!!\033[0m"); 454 | } 455 | } 456 | 457 | void FastLioSamScQn::updateOdomsAndPaths(const PosePcd &pose_pcd_in) 458 | { 459 | odoms_.points.emplace_back(pose_pcd_in.pose_eig_(0, 3), 460 | pose_pcd_in.pose_eig_(1, 3), 461 | pose_pcd_in.pose_eig_(2, 3)); 462 | corrected_odoms_.points.emplace_back(pose_pcd_in.pose_corrected_eig_(0, 3), 463 | pose_pcd_in.pose_corrected_eig_(1, 3), 464 | pose_pcd_in.pose_corrected_eig_(2, 3)); 465 | odom_path_.poses.emplace_back(poseEigToPoseStamped(pose_pcd_in.pose_eig_, map_frame_)); 466 | corrected_path_.poses.emplace_back(poseEigToPoseStamped(pose_pcd_in.pose_corrected_eig_, map_frame_)); 467 | return; 468 | } 469 | 470 | visualization_msgs::Marker FastLioSamScQn::getLoopMarkers(const gtsam::Values &corrected_esti_in) 471 | { 472 | visualization_msgs::Marker edges; 473 | edges.type = 5u; 474 | edges.scale.x = 0.12f; 475 | edges.header.frame_id = map_frame_; 476 | edges.pose.orientation.w = 1.0f; 477 | edges.color.r = 1.0f; 478 | edges.color.g = 1.0f; 479 | edges.color.b = 1.0f; 480 | edges.color.a = 1.0f; 481 | for (size_t i = 0; i < loop_idx_pairs_.size(); ++i) 482 | { 483 | if (loop_idx_pairs_[i].first >= corrected_esti_in.size() || 484 | loop_idx_pairs_[i].second >= corrected_esti_in.size()) 485 | { 486 | continue; 487 | } 488 | gtsam::Pose3 pose = corrected_esti_in.at(loop_idx_pairs_[i].first); 489 | gtsam::Pose3 pose2 = corrected_esti_in.at(loop_idx_pairs_[i].second); 490 | geometry_msgs::Point p, p2; 491 | p.x = pose.translation().x(); 492 | p.y = pose.translation().y(); 493 | p.z = pose.translation().z(); 494 | p2.x = pose2.translation().x(); 495 | p2.y = pose2.translation().y(); 496 | p2.z = pose2.translation().z(); 497 | edges.points.push_back(p); 498 | edges.points.push_back(p2); 499 | } 500 | return edges; 501 | } 502 | 503 | bool FastLioSamScQn::checkIfKeyframe(const PosePcd &pose_pcd_in, const PosePcd &latest_pose_pcd) 504 | { 505 | return keyframe_thr_ < (latest_pose_pcd.pose_corrected_eig_.block<3, 1>(0, 3) - pose_pcd_in.pose_corrected_eig_.block<3, 1>(0, 3)).norm(); 506 | } 507 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/src/loop_closure.cpp: -------------------------------------------------------------------------------- 1 | #include "loop_closure.h" 2 | 3 | LoopClosure::LoopClosure(const LoopClosureConfig &config) 4 | { 5 | config_ = config; 6 | const auto &gc = config_.gicp_config_; 7 | const auto &qc = config_.quatro_config_; 8 | ////// nano_gicp init 9 | nano_gicp_.setNumThreads(gc.nano_thread_number_); 10 | nano_gicp_.setCorrespondenceRandomness(gc.nano_correspondences_number_); 11 | nano_gicp_.setMaximumIterations(gc.nano_max_iter_); 12 | nano_gicp_.setRANSACIterations(gc.nano_ransac_max_iter_); 13 | nano_gicp_.setMaxCorrespondenceDistance(gc.max_corr_dist_); 14 | nano_gicp_.setTransformationEpsilon(gc.transformation_epsilon_); 15 | nano_gicp_.setEuclideanFitnessEpsilon(gc.euclidean_fitness_epsilon_); 16 | nano_gicp_.setRANSACOutlierRejectionThreshold(gc.ransac_outlier_rejection_threshold_); 17 | ////// quatro init 18 | quatro_handler_ = std::make_shared>(qc.fpfh_normal_radius_, 19 | qc.fpfh_radius_, 20 | qc.noise_bound_, 21 | qc.rot_gnc_factor_, 22 | qc.rot_cost_diff_thr_, 23 | qc.quatro_max_iter_, 24 | qc.estimat_scale_, 25 | qc.use_optimized_matching_, 26 | qc.quatro_distance_threshold_, 27 | qc.quatro_max_num_corres_); 28 | src_cloud_.reset(new pcl::PointCloud); 29 | dst_cloud_.reset(new pcl::PointCloud); 30 | } 31 | 32 | LoopClosure::~LoopClosure() {} 33 | 34 | void LoopClosure::updateScancontext(pcl::PointCloud cloud) 35 | { 36 | sc_manager_.makeAndSaveScancontextAndKeys(cloud); 37 | } 38 | 39 | int LoopClosure::fetchCandidateKeyframeIdx(const PosePcd &query_keyframe, 40 | const std::vector &keyframes) 41 | { 42 | // from ScanContext, get the loop candidate 43 | std::pair sc_detected_ = sc_manager_.detectLoopClosureIDGivenScan(query_keyframe.pcd_); // int: nearest node index, 44 | // float: relative yaw 45 | int candidate_keyframe_idx = sc_detected_.first; 46 | if (candidate_keyframe_idx >= 0) // if exists 47 | { 48 | // if close enough 49 | if ((keyframes[candidate_keyframe_idx].pose_corrected_eig_.block<3, 1>(0, 3) - query_keyframe.pose_corrected_eig_.block<3, 1>(0, 3)) 50 | .norm() < config_.scancontext_max_correspondence_distance_) 51 | { 52 | return candidate_keyframe_idx; 53 | } 54 | } 55 | return -1; 56 | } 57 | 58 | PcdPair LoopClosure::setSrcAndDstCloud(const std::vector &keyframes, 59 | const int src_idx, 60 | const int dst_idx, 61 | const int submap_range, 62 | const double voxel_res, 63 | const bool enable_quatro, 64 | const bool enable_submap_matching) 65 | { 66 | pcl::PointCloud dst_accum, src_accum; 67 | int num_approx = keyframes[src_idx].pcd_.size() * 2 * submap_range; 68 | src_accum.reserve(num_approx); 69 | dst_accum.reserve(num_approx); 70 | if (enable_submap_matching) 71 | { 72 | for (int i = src_idx - submap_range; i < src_idx + submap_range + 1; ++i) 73 | { 74 | if (i >= 0 && i < static_cast(keyframes.size() - 1)) 75 | { 76 | src_accum += transformPcd(keyframes[i].pcd_, keyframes[i].pose_corrected_eig_); 77 | } 78 | } 79 | for (int i = dst_idx - submap_range; i < dst_idx + submap_range + 1; ++i) 80 | { 81 | if (i >= 0 && i < static_cast(keyframes.size() - 1)) 82 | { 83 | dst_accum += transformPcd(keyframes[i].pcd_, keyframes[i].pose_corrected_eig_); 84 | } 85 | } 86 | } 87 | else 88 | { 89 | src_accum = transformPcd(keyframes[src_idx].pcd_, keyframes[src_idx].pose_corrected_eig_); 90 | if (enable_quatro) 91 | { 92 | dst_accum = transformPcd(keyframes[dst_idx].pcd_, keyframes[dst_idx].pose_corrected_eig_); 93 | } 94 | else 95 | { 96 | // For ICP matching, 97 | // empirically scan-to-submap matching works better 98 | for (int i = dst_idx - submap_range; i < dst_idx + submap_range + 1; ++i) 99 | { 100 | if (i >= 0 && i < static_cast(keyframes.size() - 1)) 101 | { 102 | dst_accum += transformPcd(keyframes[i].pcd_, keyframes[i].pose_corrected_eig_); 103 | } 104 | } 105 | } 106 | } 107 | return {*voxelizePcd(src_accum, voxel_res), *voxelizePcd(dst_accum, voxel_res)}; 108 | } 109 | 110 | RegistrationOutput LoopClosure::icpAlignment(const pcl::PointCloud &src, 111 | const pcl::PointCloud &dst) 112 | { 113 | RegistrationOutput reg_output; 114 | aligned_.clear(); 115 | // merge subkeyframes before ICP 116 | pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud()); 117 | pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud()); 118 | *src_cloud = src; 119 | *dst_cloud = dst; 120 | nano_gicp_.setInputSource(src_cloud); 121 | nano_gicp_.calculateSourceCovariances(); 122 | nano_gicp_.setInputTarget(dst_cloud); 123 | nano_gicp_.calculateTargetCovariances(); 124 | nano_gicp_.align(aligned_); 125 | 126 | // handle results 127 | reg_output.score_ = nano_gicp_.getFitnessScore(); 128 | // if matchness score is lower than threshold, (lower is better) 129 | if (nano_gicp_.hasConverged() && reg_output.score_ < config_.gicp_config_.icp_score_thr_) 130 | { 131 | reg_output.is_valid_ = true; 132 | reg_output.is_converged_ = true; 133 | reg_output.pose_between_eig_ = nano_gicp_.getFinalTransformation().cast(); 134 | } 135 | return reg_output; 136 | } 137 | 138 | RegistrationOutput LoopClosure::coarseToFineAlignment(const pcl::PointCloud &src, 139 | const pcl::PointCloud &dst) 140 | { 141 | RegistrationOutput reg_output; 142 | coarse_aligned_.clear(); 143 | 144 | reg_output.pose_between_eig_ = (quatro_handler_->align(src, dst, reg_output.is_converged_)); 145 | if (!reg_output.is_converged_) 146 | { 147 | return reg_output; 148 | } 149 | else // if valid, 150 | { 151 | // coarse align with the result of Quatro 152 | coarse_aligned_ = transformPcd(src, reg_output.pose_between_eig_); 153 | const auto &fine_output = icpAlignment(coarse_aligned_, dst); 154 | const auto quatro_tf_ = reg_output.pose_between_eig_; 155 | reg_output = fine_output; 156 | reg_output.pose_between_eig_ = fine_output.pose_between_eig_ * quatro_tf_; 157 | } 158 | return reg_output; 159 | } 160 | 161 | RegistrationOutput LoopClosure::performLoopClosure(const PosePcd &query_keyframe, 162 | const std::vector &keyframes, 163 | const int closest_keyframe_idx) 164 | { 165 | RegistrationOutput reg_output; 166 | closest_keyframe_idx_ = closest_keyframe_idx; 167 | if (closest_keyframe_idx_ >= 0) 168 | { 169 | // Quatro + NANO-GICP to check loop (from front_keyframe to closest keyframe's neighbor) 170 | const auto &[src_cloud, dst_cloud] = setSrcAndDstCloud(keyframes, 171 | query_keyframe.idx_, 172 | closest_keyframe_idx_, 173 | config_.num_submap_keyframes_, 174 | config_.voxel_res_, 175 | config_.enable_quatro_, 176 | config_.enable_submap_matching_); 177 | // Only for visualization 178 | *src_cloud_ = src_cloud; 179 | *dst_cloud_ = dst_cloud; 180 | 181 | if (config_.enable_quatro_) 182 | { 183 | std::cout << "\033[1;35mExecute coarse-to-fine alignment: " << src_cloud.size() 184 | << " vs " << dst_cloud.size() << "\033[0m\n"; 185 | return coarseToFineAlignment(src_cloud, dst_cloud); 186 | } 187 | else 188 | { 189 | std::cout << "\033[1;35mExecute GICP: " << src_cloud.size() << " vs " 190 | << dst_cloud.size() << "\033[0m\n"; 191 | return icpAlignment(src_cloud, dst_cloud); 192 | } 193 | } 194 | else 195 | { 196 | return reg_output; // dummy output whose `is_valid` is false 197 | } 198 | } 199 | 200 | pcl::PointCloud LoopClosure::getSourceCloud() 201 | { 202 | return *src_cloud_; 203 | } 204 | 205 | pcl::PointCloud LoopClosure::getTargetCloud() 206 | { 207 | return *dst_cloud_; 208 | } 209 | 210 | pcl::PointCloud LoopClosure::getCoarseAlignedCloud() 211 | { 212 | return coarse_aligned_; 213 | } 214 | 215 | // NOTE(hlim): To cover ICP-only mode, I just set `Final`, not `Fine` 216 | pcl::PointCloud LoopClosure::getFinalAlignedCloud() 217 | { 218 | return aligned_; 219 | } 220 | 221 | int LoopClosure::getClosestKeyframeidx() 222 | { 223 | return closest_keyframe_idx_; 224 | } 225 | -------------------------------------------------------------------------------- /fast_lio_sam_sc_qn/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "fast_lio_sam_sc_qn.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "fast_lio_sam_sc_qn_node"); 6 | ros::NodeHandle nh_private("~"); 7 | 8 | FastLioSamScQn fast_lio_sam_sc_qn_(nh_private); 9 | 10 | ros::AsyncSpinner spinner(4); // Use multi threads 11 | spinner.start(); 12 | ros::waitForShutdown(); 13 | 14 | fast_lio_sam_sc_qn_.~FastLioSamScQn(); // Explicit call of destructor 15 | 16 | return 0; 17 | } 18 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/kimera-multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/kimera-multi.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/acl_jackal2/lidar_points" 3 | imu_topic: "/acl_jackal2/forward/imu" 4 | # imu_topic: "/acl_jackal2/imu/data" 5 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 6 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 7 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 8 | 9 | preprocess: 10 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 11 | scan_line: 16 12 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 13 | timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 14 | blind: 0.3 15 | 16 | mapping: 17 | acc_cov: 0.1 18 | gyr_cov: 0.1 19 | b_acc_cov: 0.01 20 | b_gyr_cov: 0.005 21 | fov_degree: 180 22 | det_range: 100.0 23 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 24 | # When using /acl_jackal2/forward/imu 25 | extrinsic_T: [ 0.07025405, -0.10158666, -0.04942693 ] 26 | extrinsic_R: [-2.9046527369e-02, -9.9957706196e-01, -1.7154151723e-03, 27 | -6.9278006858e-02, 3.7251435690e-03, -9.9759064383e-01, 28 | 9.9717458733e-01, -2.8857692625e-02, -6.9356874944e-02 ] 29 | 30 | # When using /acl_jackal2/imu/data (raw) 31 | #extrinsic_T: [0.13, 0.0, 0.52] 32 | #extrinsic_R: [1.0, 0.0, 0.0, 33 | # 0.0, 1.0, 0.0, 34 | # 0.0, 0.0, 1.0] 35 | # When using /acl_jackal2/imu/data (tuned by Parker) 36 | # extrinsic_T: [ 0.06455307, -0.08247405, 0.47198666] 37 | # extrinsic_R: [ 0.99717459, -0.02885769, -0.06935687, 38 | # 0.02904653, 0.99957706, 0.00171542, 39 | # 0.06927801, -0.00372514, 0.99759064] 40 | 41 | publish: 42 | path_en: true 43 | scan_publish_en: true # false: close all the point cloud output 44 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 45 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 46 | 47 | pcd_save: 48 | pcd_save_en: false 49 | interval: -1 # how many LiDAR frames saved in each pcd file; 50 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 51 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/kitti.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/kitti/velo/pointcloud" 3 | imu_topic: "/kitti/oxts/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 6 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 7 | 8 | preprocess: 9 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | scan_line: 64 11 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 12 | timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 13 | blind: 2 14 | 15 | mapping: 16 | acc_cov: 0.1 17 | gyr_cov: 0.1 18 | b_acc_cov: 0.0001 19 | b_gyr_cov: 0.0001 20 | fov_degree: 180 21 | det_range: 100.0 22 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 23 | extrinsic_T: [ 0.81, -0.32, 0.8] 24 | extrinsic_R: [ 1, 0, 0, 25 | 0, 1, 0, 26 | 0, 0, 1] 27 | 28 | publish: 29 | path_en: false 30 | scan_publish_en: true # false: close all the point cloud output 31 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 32 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 33 | 34 | pcd_save: 35 | pcd_save_en: false 36 | interval: -1 # how many LiDAR frames saved in each pcd file; 37 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 38 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/mulran.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/mulran.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os1_points" 3 | imu_topic: "/imu/data_raw" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 6 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 7 | 8 | preprocess: 9 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | scan_line: 64 11 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 12 | timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 13 | blind: 4 14 | 15 | mapping: 16 | acc_cov: 0.1 17 | gyr_cov: 0.1 18 | b_acc_cov: 0.0001 19 | b_gyr_cov: 0.0001 20 | fov_degree: 180 21 | det_range: 150.0 22 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic 23 | extrinsic_T: [ 1.77, 0.0, -0.05 ] 24 | extrinsic_R: [-1, 0, 0, 25 | 0, -1, 0, 26 | 0, 0, 1] 27 | 28 | publish: 29 | path_en: false 30 | scan_publish_en: true # false: close all the point cloud output 31 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 32 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 33 | 34 | pcd_save: 35 | pcd_save_en: false 36 | interval: -1 # how many LiDAR frames saved in each pcd file; 37 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 38 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/newer-college2020.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/newer-college2020.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os1_cloud_node/points" 3 | imu_topic: "/os1_cloud_node/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 6 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 7 | 8 | preprocess: 9 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | scan_line: 64 11 | timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 12 | blind: 1 13 | 14 | mapping: 15 | acc_cov: 0.1 16 | gyr_cov: 0.1 17 | b_acc_cov: 0.0001 18 | b_gyr_cov: 0.0001 19 | fov_degree: 180 20 | det_range: 150.0 21 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic 22 | extrinsic_T: [ 0.0, 0.0, 0.036 ] 23 | extrinsic_R: [-1, 0, 0, 24 | 0, -1, 0, 25 | 0, 0, 1] 26 | 27 | publish: 28 | path_en: false 29 | scan_publish_en: true # false: close all the point cloud output 30 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 31 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 32 | 33 | pcd_save: 34 | pcd_save_en: true 35 | interval: -1 # how many LiDAR frames saved in each pcd file; 36 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 37 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/vbr-colosseo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /third_party/fastlio_config_launch/vbr-colosseo.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/ouster/points" 3 | imu_topic: "/imu/data" 4 | # imu_topic: "/acl_jackal2/imu/data" 5 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 6 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 7 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 8 | 9 | preprocess: 10 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 11 | scan_line: 64 12 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 13 | timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 14 | blind: 0.3 15 | 16 | mapping: 17 | acc_cov: 0.01 18 | gyr_cov: 0.001 19 | b_acc_cov: 0.001 20 | b_gyr_cov: 0.0005 21 | fov_degree: 180 22 | det_range: 100.0 23 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 24 | extrinsic_T: [0.04943289, 0.01478779, 0.60798871] 25 | extrinsic_R: [ 0.99946541, -0.03200262, 0.00670301, 26 | 0.03194165, 0.99944911, 0.009017, 27 | -0.0069879, -0.00879813, 0.99993691 ] 28 | publish: 29 | path_en: true 30 | scan_publish_en: true # false: close all the point cloud output 31 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 32 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 33 | 34 | pcd_save: 35 | pcd_save_en: false 36 | interval: -1 # how many LiDAR frames saved in each pcd file; 37 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 38 | --------------------------------------------------------------------------------