├── .clang-format ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── fast_lio_sam_qn ├── CMakeLists.txt ├── config │ ├── config.yaml │ ├── lio_rviz.rviz │ └── sam_rviz.rviz ├── include │ ├── fast_lio_sam_qn.h │ ├── loop_closure.h │ ├── pose_pcd.hpp │ └── utilities.hpp ├── launch │ └── run.launch ├── package.xml └── src │ ├── fast_lio_sam_qn.cpp │ ├── loop_closure.cpp │ └── main.cpp ├── imgs ├── fast1.png ├── fast2.png ├── qn.png ├── qn_side.png ├── sam1.png └── sam2.png └── 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_qn/result.pcd 5 | /fast_lio_sam_qn/sequence*/ 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/FAST_LIO"] 2 | path = third_party/FAST_LIO 3 | url = https://github.com/hku-mars/FAST_LIO 4 | [submodule "third_party/livox_ros_driver"] 5 | path = third_party/livox_ros_driver 6 | url = https://github.com/Livox-SDK/livox_ros_driver 7 | [submodule "third_party/nano_gicp"] 8 | path = third_party/nano_gicp 9 | url = https://github.com/engcang/nano_gicp 10 | [submodule "third_party/Quatro"] 11 | path = third_party/Quatro 12 | url = https://github.com/engcang/Quatro 13 | -------------------------------------------------------------------------------- /.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-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 [Quatro](https://quatro-plusplus.github.io/) and [Nano-GICP module](https://github.com/engcang/nano_gicp) 3 | + [Quatro](https://quatro-plusplus.github.io/) - fast, accurate and robust global registration which provides great initial guess of transform 4 | + [Quatro module](https://github.com/engcang/quatro) - `Quatro` as a module, can be easily used in other packages 5 | + [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) 6 | + Note: similar repositories already exist 7 | + [FAST_LIO_LC](https://github.com/yanliang-wang/FAST_LIO_LC): FAST-LIO2 + SC-A-LOAM based SLAM 8 | + [FAST_LIO_SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM): FAST-LIO2 + ScanContext based SLAM 9 | + [FAST_LIO_SAM](https://github.com/kahowang/FAST_LIO_SAM): FAST-LIO2 + LIO-SAM (not modularized) 10 | + [FAST_LIO_SAM](https://github.com/engcang/FAST-LIO-SAM): FAST-LIO2 + LIO-SAM (modularized) 11 | + Note2: main code (PGO) is modularized and hence can be combined with any other LIO / LO 12 | + This repo is to learn GTSAM myself! 13 | + and as GTSAM tutorial for beginners - [GTSAM 튜토리얼 한글 포스팅](https://engcang.github.io/2023/07/15/gtsam_tutorial.html) 14 | ## Video clip - https://youtu.be/MQ8XxRY472Y 15 |
16 | 17 | ## Computational complexity
in KITTI seq. 05 with i9-10900k CPU 18 | + FAST-LIO-SAM: max 118% CPU usage, 125 times of ICP, 124.9ms consumption on average 19 | + FAST-LIO-SAM-N (only Nano-GICP): max 164% CPU usage, 130 times of ICP, 61.9ms consumption on average 20 | + FAST-LIO-SAM-QN: 21 | + Advanced matching - max 325% CPU usage, 85 times of ICP, 140ms consumption on average 22 | + Optimized matching (with max 200 correspondences downsampling) - max 569% CPU usage, 90 times of ICP, 128.6ms consumption on average 23 | + Note: `loop_timer_func` runs at fixed `basic/loop_update_hz`. So how many times of ICP occurred can be different depending on the speed of matching methods. 24 | 25 |

26 | 27 | 28 | 29 |
30 | KITTI seq 05 top view - (left): FAST-LIO2 (middle): FAST-LIO-SAM (bottom): FAST-LIO-SAM-QN 31 |

32 |

33 | 34 | 35 | 36 |
37 | KITTI seq 05 side view - (top): FAST-LIO2 (middle): FAST-LIO-SAM (bottom): FAST-LIO-SAM-QN 38 |

39 | 40 |
41 | 42 | ## Dependencies 43 | + `C++` >= 17, `OpenMP` >= 4.5, `CMake` >= 3.10.0, `Eigen` >= 3.2, `Boost` >= 1.54 44 | + `ROS` 45 | + [`GTSAM`](https://github.com/borglab/gtsam) 46 | ```shell 47 | wget -O gtsam.zip https://github.com/borglab/gtsam/archive/refs/tags/4.1.1.zip 48 | unzip gtsam.zip 49 | cd gtsam-4.1.1/ 50 | mkdir build && cd build 51 | cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. 52 | sudo make install -j16 53 | ``` 54 | + [`Teaser++`](https://github.com/MIT-SPARK/TEASER-plusplus) 55 | ```shell 56 | git clone https://github.com/MIT-SPARK/TEASER-plusplus.git 57 | cd TEASER-plusplus && mkdir build && cd build 58 | cmake .. -DENABLE_DIAGNOSTIC_PRINT=OFF 59 | sudo make install -j16 60 | sudo ldconfig 61 | ``` 62 | + `tbb` (is used for faster `Quatro`) 63 | ```shell 64 | sudo apt install libtbb-dev 65 | ``` 66 | 67 | ## How to build 68 | + Get the code and then build the main code. 69 | ```shell 70 | cd ~/your_workspace/src 71 | git clone https://github.com/engcang/FAST-LIO-SAM-QN --recursive 72 | 73 | cd ~/your_workspace 74 | # nano_gicp, quatro first 75 | catkin build nano_gicp -DCMAKE_BUILD_TYPE=Release 76 | # Note the option! 77 | catkin build quatro -DCMAKE_BUILD_TYPE=Release -DQUATRO_TBB=ON -DQUATRO_DEBUG=OFF 78 | catkin build -DCMAKE_BUILD_TYPE=Release 79 | . devel/setup.bash 80 | ``` 81 | 82 | ## How to run 83 | + Then run (change config files in third_party/`FAST_LIO`) 84 | ```shell 85 | roslaunch fast_lio_sam_qn run.launch lidar:=ouster 86 | roslaunch fast_lio_sam_qn run.launch lidar:=velodyne 87 | roslaunch fast_lio_sam_qn run.launch lidar:=livox 88 | ``` 89 | * In particular, we provide a preset launch option for specific datasets: 90 | ```shell 91 | roslaunch fast_lio_sam_qn run.launch lidar:=kitti 92 | roslaunch fast_lio_sam_qn run.launch lidar:=mulran 93 | roslaunch fast_lio_sam_qn run.launch lidar:=newer-college20 94 | ``` 95 | 96 | ## Structure 97 | + odomPcdCallback 98 | + pub realtime pose in corrected frame 99 | + keyframe detection -> if keyframe, add to pose graph + save to keyframe queue 100 | + pose graph optimization with iSAM2 101 | + loopTimerFunc 102 | + process a saved keyframe 103 | + detect loop -> if loop, add to pose graph 104 | + visTimerFunc 105 | + visualize all **(Note: global map is only visualized once uncheck/check the mapped_pcd in rviz to save comp.)** 106 | 107 |
108 | 109 | ## Memo 110 | + `Quatro` module fixed for empty matches 111 | + `Quatro` module is updated with `optimizedMatching` which limits the number of correspondences and increased the speed 112 | 113 |
114 | 115 | ## License 116 | Creative Commons License 117 | 118 | This work is licensed under a [Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License](http://creativecommons.org/licenses/by-nc-sa/4.0/) 119 | -------------------------------------------------------------------------------- /fast_lio_sam_qn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(fast_lio_sam_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 | ${catkin_INCLUDE_DIRS} 55 | ${PCL_INCLUDE_DIRS} 56 | ${EIGEN3_INCLUDE_DIR} 57 | ${GTSAM_INCLUDE_DIR} 58 | ) 59 | 60 | 61 | ########### 62 | ## Build ## 63 | ########### 64 | ### main 65 | set(COMMON_LIBS ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${EIGEN3_LIBS} ${OpenMP_LIBS} gtsam) 66 | add_library(${PROJECT_NAME}_modules src/loop_closure.cpp src/fast_lio_sam_qn.cpp) 67 | target_link_libraries(${PROJECT_NAME}_modules ${COMMON_LIBS}) 68 | 69 | add_executable(${PROJECT_NAME}_node src/main.cpp) 70 | target_link_libraries(${PROJECT_NAME}_node ${COMMON_LIBS} ${PROJECT_NAME}_modules) 71 | -------------------------------------------------------------------------------- /fast_lio_sam_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 | loop: 13 | loop_detection_radius: 35.0 # unit [meter] within this radius, check if loop or not 14 | loop_detection_timediff_threshold: 30.0 # unit [sec] check if two keyframes are loop if times are farther than this 15 | 16 | quatro_nano_gicp_voxel_resolution: 0.3 # voxel resolution to use Quatro and Nano-GICP matching 17 | save_voxel_resolution: 0.3 # voxel resolution to save map in .pcd format 18 | 19 | nano_gicp: # all values are from Nano-GICP official github (Direct LiDAR Odometry) 20 | thread_number: 0 # if 0, max number of core 21 | icp_score_threshold: 1.5 # lower is more accurate, for loop detection 22 | correspondences_number: 15 23 | max_iter: 32 24 | transformation_epsilon: 0.01 25 | euclidean_fitness_epsilon: 0.01 26 | ransac: 27 | max_iter: 5 28 | outlier_rejection_threshold: 1.0 29 | 30 | quatro: # all values are from Quatro official github 31 | enable: true # whether or not to use quatro, if false -> FAST-LIO-SAM-N (Nano-GICP only) 32 | optimize_matching: true # whether or not to use Optimized matching 33 | distance_threshold: 35.0 # when optimized matching, use only correspondences within this radius, unit [meter] 34 | max_correspondences: 500 # max correspondences to use for optimized matching 35 | fpfh_normal_radius: 0.9 # It should be 2.5 - 3.0 * `quatro_nano_gicp_voxel_resolution` 36 | fpfh_radius: 1.5 # It should be 5.0 * `quatro_nano_gicp_voxel_resolution` 37 | estimating_scale: false 38 | noise_bound: 0.3 # The magnitude of uncertainty of measurements, the best is within v/2 ~ v (v: voxel resol.) 39 | rotation: 40 | num_max_iter: 50 # Usually, rotation estimation converges within < 20 iterations 41 | gnc_factor: 1.4 # Control the magnitue of the increase in non-linearity. The larger the value, the steeper the increase in nonlinearity. 42 | rot_cost_diff_threshold: 0.0001 # The cost threshold is compared with the difference between costs of consecutive iterations. 43 | # Once the diff. of cost < `rot_cost_diff_threshold`, then the optimization is finished. 44 | 45 | result: 46 | save_map_pcd: true # Save result map in .pcd format, not voxelized and hence file size could be huge 47 | 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) 48 | save_in_kitti_format: true # Save result in KITTI format 49 | seq_name: "sequence" # sequence name for saving scans and corresponding poses 50 | -------------------------------------------------------------------------------- /fast_lio_sam_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_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 | - /mapping1/curr_pcd_stack1 11 | - /debug1 12 | Splitter Ratio: 0.6432291865348816 13 | Tree Height: 441 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 1 41 | Cell Size: 1000 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: false 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 40 56 | Reference Frame: 57 | Value: false 58 | - Class: rviz/Axes 59 | Enabled: false 60 | Length: 5 61 | Name: Axes 62 | Radius: 0.800000011920929 63 | Reference Frame: 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: 37.83192825317383 71 | Min Value: -2.407393217086792 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.5 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.029999999329447746 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 | Shaft Length: 1 163 | Shaft Radius: 0.05000000074505806 164 | Shape: Axes 165 | Topic: /pose_stamped 166 | Unreliable: false 167 | Value: true 168 | - Alpha: 0 169 | Buffer Length: 2 170 | Class: rviz/Path 171 | Color: 0; 155; 255 172 | Enabled: true 173 | Head Diameter: 0 174 | Head Length: 0 175 | Length: 0.30000001192092896 176 | Line Style: Billboards 177 | Line Width: 0.20000000298023224 178 | Name: Path 179 | Offset: 180 | X: 0 181 | Y: 0 182 | Z: 0 183 | Pose Color: 25; 255; 255 184 | Pose Style: None 185 | Radius: 0.029999999329447746 186 | Shaft Diameter: 0.4000000059604645 187 | Shaft Length: 0.4000000059604645 188 | Topic: /ori_path 189 | Unreliable: false 190 | Value: true 191 | - Alpha: 0 192 | Buffer Length: 2 193 | Class: rviz/Path 194 | Color: 138; 226; 52 195 | Enabled: true 196 | Head Diameter: 0 197 | Head Length: 0 198 | Length: 0.30000001192092896 199 | Line Style: Billboards 200 | Line Width: 0.20000000298023224 201 | Name: corrected_Path 202 | Offset: 203 | X: 0 204 | Y: 0 205 | Z: 0 206 | Pose Color: 25; 255; 255 207 | Pose Style: None 208 | Radius: 0.029999999329447746 209 | Shaft Diameter: 0.4000000059604645 210 | Shaft Length: 0.4000000059604645 211 | Topic: /corrected_path 212 | Unreliable: false 213 | Value: true 214 | - Alpha: 1 215 | Autocompute Intensity Bounds: true 216 | Autocompute Value Bounds: 217 | Max Value: 10 218 | Min Value: -10 219 | Value: true 220 | Axis: Z 221 | Channel Name: intensity 222 | Class: rviz/PointCloud2 223 | Color: 0; 59; 255 224 | Color Transformer: FlatColor 225 | Decay Time: 0 226 | Enabled: true 227 | Invert Rainbow: false 228 | Max Color: 255; 255; 255 229 | Min Color: 0; 0; 0 230 | Name: odoms 231 | Position Transformer: XYZ 232 | Queue Size: 10 233 | Selectable: true 234 | Size (Pixels): 3 235 | Size (m): 0.800000011920929 236 | Style: Spheres 237 | Topic: /ori_odom 238 | Unreliable: false 239 | Use Fixed Frame: true 240 | Use rainbow: true 241 | Value: true 242 | - Alpha: 1 243 | Autocompute Intensity Bounds: true 244 | Autocompute Value Bounds: 245 | Max Value: 10 246 | Min Value: -10 247 | Value: true 248 | Axis: Z 249 | Channel Name: intensity 250 | Class: rviz/PointCloud2 251 | Color: 138; 226; 52 252 | Color Transformer: FlatColor 253 | Decay Time: 0 254 | Enabled: true 255 | Invert Rainbow: false 256 | Max Color: 255; 255; 255 257 | Min Color: 0; 0; 0 258 | Name: corrected_odoms 259 | Position Transformer: XYZ 260 | Queue Size: 10 261 | Selectable: true 262 | Size (Pixels): 3 263 | Size (m): 0.800000011920929 264 | Style: Spheres 265 | Topic: /corrected_odom 266 | Unreliable: false 267 | Use Fixed Frame: true 268 | Use rainbow: true 269 | Value: true 270 | - Class: rviz/Marker 271 | Enabled: true 272 | Marker Topic: /loop_detection 273 | Name: loop_constraints 274 | Namespaces: 275 | {} 276 | Queue Size: 100 277 | Value: true 278 | - Class: rviz/Group 279 | Displays: 280 | - Alpha: 1 281 | Autocompute Intensity Bounds: true 282 | Autocompute Value Bounds: 283 | Max Value: 10 284 | Min Value: -10 285 | Value: true 286 | Axis: Z 287 | Channel Name: intensity 288 | Class: rviz/PointCloud2 289 | Color: 255; 0; 0 290 | Color Transformer: FlatColor 291 | Decay Time: 0 292 | Enabled: true 293 | Invert Rainbow: false 294 | Max Color: 255; 255; 255 295 | Min Color: 0; 0; 0 296 | Name: src 297 | Position Transformer: XYZ 298 | Queue Size: 10 299 | Selectable: true 300 | Size (Pixels): 2 301 | Size (m): 0.10000000149011612 302 | Style: Points 303 | Topic: /src 304 | Unreliable: false 305 | Use Fixed Frame: true 306 | Use rainbow: true 307 | Value: true 308 | - Alpha: 1 309 | Autocompute Intensity Bounds: true 310 | Autocompute Value Bounds: 311 | Max Value: 10 312 | Min Value: -10 313 | Value: true 314 | Axis: Z 315 | Channel Name: intensity 316 | Class: rviz/PointCloud2 317 | Color: 0; 101; 255 318 | Color Transformer: FlatColor 319 | Decay Time: 0 320 | Enabled: true 321 | Invert Rainbow: false 322 | Max Color: 255; 255; 255 323 | Min Color: 0; 0; 0 324 | Name: dst 325 | Position Transformer: XYZ 326 | Queue Size: 10 327 | Selectable: true 328 | Size (Pixels): 2 329 | Size (m): 0.10000000149011612 330 | Style: Points 331 | Topic: /dst 332 | Unreliable: false 333 | Use Fixed Frame: true 334 | Use rainbow: true 335 | Value: true 336 | - Alpha: 1 337 | Autocompute Intensity Bounds: true 338 | Autocompute Value Bounds: 339 | Max Value: 10 340 | Min Value: -10 341 | Value: true 342 | Axis: Z 343 | Channel Name: intensity 344 | Class: rviz/PointCloud2 345 | Color: 252; 233; 79 346 | Color Transformer: FlatColor 347 | Decay Time: 0 348 | Enabled: true 349 | Invert Rainbow: false 350 | Max Color: 255; 255; 255 351 | Min Color: 0; 0; 0 352 | Name: coarse_aligned 353 | Position Transformer: XYZ 354 | Queue Size: 10 355 | Selectable: true 356 | Size (Pixels): 2 357 | Size (m): 0.10000000149011612 358 | Style: Points 359 | Topic: /coarse_aligned_quatro 360 | Unreliable: false 361 | Use Fixed Frame: true 362 | Use rainbow: true 363 | Value: true 364 | - Alpha: 1 365 | Autocompute Intensity Bounds: true 366 | Autocompute Value Bounds: 367 | Max Value: 10 368 | Min Value: -10 369 | Value: true 370 | Axis: Z 371 | Channel Name: intensity 372 | Class: rviz/PointCloud2 373 | Color: 0; 255; 0 374 | Color Transformer: FlatColor 375 | Decay Time: 0 376 | Enabled: true 377 | Invert Rainbow: false 378 | Max Color: 255; 255; 255 379 | Min Color: 0; 0; 0 380 | Name: fine_aligned 381 | Position Transformer: XYZ 382 | Queue Size: 10 383 | Selectable: true 384 | Size (Pixels): 2 385 | Size (m): 0.10000000149011612 386 | Style: Points 387 | Topic: /fine_aligned_nano_gicp 388 | Unreliable: false 389 | Use Fixed Frame: true 390 | Use rainbow: true 391 | Value: true 392 | Enabled: true 393 | Name: debug 394 | Enabled: true 395 | Global Options: 396 | Background Color: 0; 0; 0 397 | Default Light: true 398 | Fixed Frame: map 399 | Frame Rate: 10 400 | Name: root 401 | Tools: 402 | - Class: rviz/Interact 403 | Hide Inactive Objects: true 404 | - Class: rviz/MoveCamera 405 | - Class: rviz/Select 406 | - Class: rviz/FocusCamera 407 | - Class: rviz/Measure 408 | - Class: rviz/SetInitialPose 409 | Theta std deviation: 0.2617993950843811 410 | Topic: /initialpose 411 | X std deviation: 0.5 412 | Y std deviation: 0.5 413 | - Class: rviz/SetGoal 414 | Topic: /move_base_simple/goal 415 | - Class: rviz/PublishPoint 416 | Single click: true 417 | Topic: /clicked_point 418 | Value: true 419 | Views: 420 | Current: 421 | Class: rviz/Orbit 422 | Distance: 179.57247924804688 423 | Enable Stereo Rendering: 424 | Stereo Eye Separation: 0.05999999865889549 425 | Stereo Focal Distance: 1 426 | Swap Stereo Eyes: false 427 | Value: false 428 | Focal Point: 429 | X: 0 430 | Y: 0 431 | Z: 0 432 | Focal Shape Fixed Size: false 433 | Focal Shape Size: 0.05000000074505806 434 | Invert Z Axis: false 435 | Name: Current View 436 | Near Clip Distance: 0.009999999776482582 437 | Pitch: 0.7853981852531433 438 | Target Frame: robot 439 | Value: Orbit (rviz) 440 | Yaw: 0.7853981852531433 441 | Saved: ~ 442 | Window Geometry: 443 | Displays: 444 | collapsed: true 445 | Height: 1016 446 | Hide Left Dock: true 447 | Hide Right Dock: true 448 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c7000000000000000000000001000001520000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000071800000052fc0100000002fb0000000800540069006d0065000000000000000718000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000004df0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 449 | Selection: 450 | collapsed: false 451 | Time: 452 | collapsed: false 453 | Tool Properties: 454 | collapsed: false 455 | Views: 456 | collapsed: true 457 | Width: 1247 458 | X: 2393 459 | Y: 335 460 | -------------------------------------------------------------------------------- /fast_lio_sam_qn/include/fast_lio_sam_qn.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_QN_MAIN_H 2 | #define FAST_LIO_SAM_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 FastLioSamQn 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 FastLioSamQn(const ros::NodeHandle &n_private); 104 | ~FastLioSamQn(); 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_qn/include/loop_closure.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_QN_LOOP_CLOSURE_H 2 | #define FAST_LIO_SAM_QN_LOOP_CLOSURE_H 3 | 4 | ///// C++ common headers 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | ///// PCL 11 | #include //pt 12 | #include //cloud 13 | ///// Eigen 14 | #include 15 | ///// Nano-GICP 16 | #include 17 | #include 18 | ///// Quatro 19 | #include 20 | ///// coded headers 21 | #include "pose_pcd.hpp" 22 | #include "utilities.hpp" 23 | using PcdPair = std::tuple, pcl::PointCloud>; 24 | 25 | struct NanoGICPConfig 26 | { 27 | int nano_thread_number_ = 0; 28 | int nano_correspondences_number_ = 15; 29 | int nano_max_iter_ = 32; 30 | int nano_ransac_max_iter_ = 5; 31 | double max_corr_dist_ = 2.0; 32 | double icp_score_thr_ = 10.0; 33 | double transformation_epsilon_ = 0.01; 34 | double euclidean_fitness_epsilon_ = 0.01; 35 | double ransac_outlier_rejection_threshold_ = 1.0; 36 | }; 37 | 38 | struct QuatroConfig 39 | { 40 | bool use_optimized_matching_ = true; 41 | bool estimat_scale_ = false; 42 | int quatro_max_num_corres_ = 500; 43 | int quatro_max_iter_ = 50; 44 | double quatro_distance_threshold_ = 30.0; 45 | double fpfh_normal_radius_ = 0.30; // It should be 2.5 - 3.0 * `voxel_res` 46 | double fpfh_radius_ = 0.50; // It should be 5.0 * `voxel_res` 47 | double noise_bound_ = 0.30; 48 | double rot_gnc_factor_ = 1.40; 49 | double rot_cost_diff_thr_ = 0.0001; 50 | }; 51 | 52 | struct LoopClosureConfig 53 | { 54 | bool enable_quatro_ = true; 55 | bool enable_submap_matching_ = true; 56 | int num_submap_keyframes_ = 10; 57 | double voxel_res_ = 0.1; 58 | double loop_detection_radius_; 59 | double loop_detection_timediff_threshold_; 60 | NanoGICPConfig gicp_config_; 61 | QuatroConfig quatro_config_; 62 | }; 63 | 64 | struct RegistrationOutput 65 | { 66 | bool is_valid_ = false; 67 | bool is_converged_ = false; 68 | double score_ = std::numeric_limits::max(); 69 | Eigen::Matrix4d pose_between_eig_ = Eigen::Matrix4d::Identity(); 70 | }; 71 | 72 | class LoopClosure 73 | { 74 | private: 75 | nano_gicp::NanoGICP nano_gicp_; 76 | std::shared_ptr> quatro_handler_ = nullptr; 77 | int closest_keyframe_idx_ = -1; 78 | pcl::PointCloud::Ptr src_cloud_; 79 | pcl::PointCloud::Ptr dst_cloud_; 80 | pcl::PointCloud coarse_aligned_; 81 | pcl::PointCloud aligned_; 82 | LoopClosureConfig config_; 83 | 84 | public: 85 | explicit LoopClosure(const LoopClosureConfig &config); 86 | ~LoopClosure(); 87 | int fetchClosestKeyframeIdx(const PosePcd &query_keyframe, 88 | const std::vector &keyframes); 89 | PcdPair setSrcAndDstCloud(const std::vector &keyframes, 90 | const int src_idx, 91 | const int dst_idx, 92 | const int submap_range, 93 | const double voxel_res, 94 | const bool enable_quatro, 95 | const bool enable_submap_matching); 96 | RegistrationOutput icpAlignment(const pcl::PointCloud &src, 97 | const pcl::PointCloud &dst); 98 | RegistrationOutput coarseToFineAlignment(const pcl::PointCloud &src, 99 | const pcl::PointCloud &dst); 100 | RegistrationOutput performLoopClosure(const PosePcd &query_keyframe, 101 | const std::vector &keyframes); 102 | RegistrationOutput performLoopClosure(const PosePcd &query_keyframe, 103 | const std::vector &keyframes, 104 | const int closest_keyframe_idx); 105 | pcl::PointCloud getSourceCloud(); 106 | pcl::PointCloud getTargetCloud(); 107 | pcl::PointCloud getCoarseAlignedCloud(); 108 | pcl::PointCloud getFinalAlignedCloud(); 109 | int getClosestKeyframeidx(); 110 | }; 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /fast_lio_sam_qn/include/pose_pcd.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_QN_POSE_PCD_HPP 2 | #define FAST_LIO_SAM_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_qn/include/utilities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_LIO_SAM_QN_UTILITY_HPP 2 | #define FAST_LIO_SAM_QN_UTILITY_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_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_qn/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_lio_sam_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 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_qn/src/fast_lio_sam_qn.cpp: -------------------------------------------------------------------------------- 1 | #include "fast_lio_sam_qn.h" 2 | 3 | FastLioSamQn::FastLioSamQn(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 | /* loop */ 22 | nh_.param("/loop/loop_detection_radius", lc_config.loop_detection_radius_, 15.0); 23 | nh_.param("/loop/loop_detection_timediff_threshold", lc_config.loop_detection_timediff_threshold_, 10.0); 24 | gc.max_corr_dist_ = lc_config.loop_detection_radius_ * 1.5; 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_iter", gc.nano_max_iter_, 32); 30 | nh_.param("/nano_gicp/transformation_epsilon", gc.transformation_epsilon_, 0.01); 31 | nh_.param("/nano_gicp/euclidean_fitness_epsilon", gc.euclidean_fitness_epsilon_, 0.01); 32 | nh_.param("/nano_gicp/ransac/max_iter", gc.nano_ransac_max_iter_, 5); 33 | nh_.param("/nano_gicp/ransac/outlier_rejection_threshold", gc.ransac_outlier_rejection_threshold_, 1.0); 34 | /* quatro (Quatro config) */ 35 | nh_.param("/quatro/enable", lc_config.enable_quatro_, false); 36 | nh_.param("/quatro/optimize_matching", qc.use_optimized_matching_, true); 37 | nh_.param("/quatro/distance_threshold", qc.quatro_distance_threshold_, 30.0); 38 | nh_.param("/quatro/max_nucorrespondences", qc.quatro_max_num_corres_, 200); 39 | nh_.param("/quatro/fpfh_normal_radius", qc.fpfh_normal_radius_, 0.3); 40 | nh_.param("/quatro/fpfh_radius", qc.fpfh_radius_, 0.5); 41 | nh_.param("/quatro/estimating_scale", qc.estimat_scale_, false); 42 | nh_.param("/quatro/noise_bound", qc.noise_bound_, 0.3); 43 | nh_.param("/quatro/rotation/gnc_factor", qc.rot_gnc_factor_, 1.4); 44 | nh_.param("/quatro/rotation/rot_cost_diff_threshold", qc.rot_cost_diff_thr_, 0.0001); 45 | nh_.param("/quatro/rotation/numax_iter", qc.quatro_max_iter_, 50); 46 | /* results */ 47 | nh_.param("/result/save_map_bag", save_map_bag_, false); 48 | nh_.param("/result/save_map_pcd", save_map_pcd_, false); 49 | nh_.param("/result/save_in_kitti_format", save_in_kitti_format_, false); 50 | nh_.param("/result/seq_name", seq_name_, ""); 51 | loop_closure_.reset(new LoopClosure(lc_config)); 52 | /* Initialization of GTSAM */ 53 | gtsam::ISAM2Params isam_params_; 54 | isam_params_.relinearizeThreshold = 0.01; 55 | isam_params_.relinearizeSkip = 1; 56 | isam_handler_ = std::make_shared(isam_params_); 57 | /* ROS things */ 58 | odom_path_.header.frame_id = map_frame_; 59 | corrected_path_.header.frame_id = map_frame_; 60 | package_path_ = ros::package::getPath("fast_lio_sam_qn"); 61 | /* publishers */ 62 | odom_pub_ = nh_.advertise("/ori_odom", 10, true); 63 | path_pub_ = nh_.advertise("/ori_path", 10, true); 64 | corrected_odom_pub_ = nh_.advertise("/corrected_odom", 10, true); 65 | corrected_path_pub_ = nh_.advertise("/corrected_path", 10, true); 66 | corrected_pcd_map_pub_ = nh_.advertise("/corrected_map", 10, true); 67 | corrected_current_pcd_pub_ = nh_.advertise("/corrected_current_pcd", 10, true); 68 | loop_detection_pub_ = nh_.advertise("/loop_detection", 10, true); 69 | realtime_pose_pub_ = nh_.advertise("/pose_stamped", 10); 70 | debug_src_pub_ = nh_.advertise("/src", 10, true); 71 | debug_dst_pub_ = nh_.advertise("/dst", 10, true); 72 | debug_coarse_aligned_pub_ = nh_.advertise("/coarse_aligned_quatro", 10, true); 73 | debug_fine_aligned_pub_ = nh_.advertise("/fine_aligned_nano_gicp", 10, true); 74 | /* subscribers */ 75 | sub_odom_ = std::make_shared>(nh_, "/Odometry", 10); 76 | sub_pcd_ = std::make_shared>(nh_, "/cloud_registered", 10); 77 | sub_odom_pcd_sync_ = std::make_shared>(odom_pcd_sync_pol(10), *sub_odom_, *sub_pcd_); 78 | sub_odom_pcd_sync_->registerCallback(boost::bind(&FastLioSamQn::odomPcdCallback, this, _1, _2)); 79 | sub_save_flag_ = nh_.subscribe("/save_dir", 1, &FastLioSamQn::saveFlagCallback, this); 80 | /* Timers */ 81 | loop_timer_ = nh_.createTimer(ros::Duration(1 / loop_update_hz), &FastLioSamQn::loopTimerFunc, this); 82 | vis_timer_ = nh_.createTimer(ros::Duration(1 / vis_hz), &FastLioSamQn::visTimerFunc, this); 83 | ROS_INFO("Main class, starting node..."); 84 | } 85 | 86 | void FastLioSamQn::odomPcdCallback(const nav_msgs::OdometryConstPtr &odom_msg, 87 | const sensor_msgs::PointCloud2ConstPtr &pcd_msg) 88 | { 89 | Eigen::Matrix4d last_odom_tf; 90 | last_odom_tf = current_frame_.pose_eig_; // to calculate delta 91 | current_frame_ = PosePcd(*odom_msg, *pcd_msg, current_keyframe_idx_); // to be checked if keyframe or not 92 | high_resolution_clock::time_point t1 = high_resolution_clock::now(); 93 | { 94 | //// 1. realtime pose = last corrected odom * delta (last -> current) 95 | std::lock_guard lock(realtime_pose_mutex_); 96 | odom_delta_ = odom_delta_ * last_odom_tf.inverse() * current_frame_.pose_eig_; 97 | current_frame_.pose_corrected_eig_ = last_corrected_pose_ * odom_delta_; 98 | realtime_pose_pub_.publish(poseEigToPoseStamped(current_frame_.pose_corrected_eig_, map_frame_)); 99 | broadcaster_.sendTransform(tf::StampedTransform(poseEigToROSTf(current_frame_.pose_corrected_eig_), 100 | ros::Time::now(), 101 | map_frame_, 102 | "robot")); 103 | } 104 | corrected_current_pcd_pub_.publish(pclToPclRos(transformPcd(current_frame_.pcd_, current_frame_.pose_corrected_eig_), map_frame_)); 105 | 106 | if (!is_initialized_) //// init only once 107 | { 108 | // others 109 | keyframes_.push_back(current_frame_); 110 | updateOdomsAndPaths(current_frame_); 111 | // graph 112 | auto variance_vector = (gtsam::Vector(6) << 1e-4, 1e-4, 1e-4, 1e-2, 1e-2, 1e-2).finished(); // rad*rad, 113 | // meter*meter 114 | gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Variances(variance_vector); 115 | gtsam_graph_.add(gtsam::PriorFactor(0, poseEigToGtsamPose(current_frame_.pose_eig_), prior_noise)); 116 | init_esti_.insert(current_keyframe_idx_, poseEigToGtsamPose(current_frame_.pose_eig_)); 117 | current_keyframe_idx_++; 118 | is_initialized_ = true; 119 | } 120 | else 121 | { 122 | //// 2. check if keyframe 123 | high_resolution_clock::time_point t2 = high_resolution_clock::now(); 124 | if (checkIfKeyframe(current_frame_, keyframes_.back())) 125 | { 126 | // 2-2. if so, save 127 | { 128 | std::lock_guard lock(keyframes_mutex_); 129 | keyframes_.push_back(current_frame_); 130 | } 131 | // 2-3. if so, add to graph 132 | auto variance_vector = (gtsam::Vector(6) << 1e-4, 1e-4, 1e-4, 1e-2, 1e-2, 1e-2).finished(); 133 | gtsam::noiseModel::Diagonal::shared_ptr odom_noise = gtsam::noiseModel::Diagonal::Variances(variance_vector); 134 | gtsam::Pose3 pose_from = poseEigToGtsamPose(keyframes_[current_keyframe_idx_ - 1].pose_corrected_eig_); 135 | gtsam::Pose3 pose_to = poseEigToGtsamPose(current_frame_.pose_corrected_eig_); 136 | { 137 | std::lock_guard lock(graph_mutex_); 138 | gtsam_graph_.add(gtsam::BetweenFactor(current_keyframe_idx_ - 1, 139 | current_keyframe_idx_, 140 | pose_from.between(pose_to), 141 | odom_noise)); 142 | init_esti_.insert(current_keyframe_idx_, pose_to); 143 | } 144 | current_keyframe_idx_++; 145 | 146 | //// 3. vis 147 | high_resolution_clock::time_point t3 = high_resolution_clock::now(); 148 | { 149 | std::lock_guard lock(vis_mutex_); 150 | updateOdomsAndPaths(current_frame_); 151 | } 152 | 153 | //// 4. optimize with graph 154 | high_resolution_clock::time_point t4 = high_resolution_clock::now(); 155 | // m_corrected_esti = gtsam::LevenbergMarquardtOptimizer(m_gtsam_graph, init_esti_).optimize(); // cf. isam.update vs values.LM.optimize 156 | { 157 | std::lock_guard lock(graph_mutex_); 158 | isam_handler_->update(gtsam_graph_, init_esti_); 159 | isam_handler_->update(); 160 | if (loop_added_flag_) // https://github.com/TixiaoShan/LIO-SAM/issues/5#issuecomment-653752936 161 | { 162 | isam_handler_->update(); 163 | isam_handler_->update(); 164 | isam_handler_->update(); 165 | } 166 | gtsam_graph_.resize(0); 167 | init_esti_.clear(); 168 | } 169 | 170 | //// 5. handle corrected results 171 | // get corrected poses and reset odom delta (for realtime pose pub) 172 | high_resolution_clock::time_point t5 = high_resolution_clock::now(); 173 | { 174 | std::lock_guard lock(realtime_pose_mutex_); 175 | corrected_esti_ = isam_handler_->calculateEstimate(); 176 | last_corrected_pose_ = gtsamPoseToPoseEig(corrected_esti_.at(corrected_esti_.size() - 1)); 177 | odom_delta_ = Eigen::Matrix4d::Identity(); 178 | } 179 | // correct poses in keyframes 180 | if (loop_added_flag_) 181 | { 182 | std::lock_guard lock(keyframes_mutex_); 183 | for (size_t i = 0; i < corrected_esti_.size(); ++i) 184 | { 185 | keyframes_[i].pose_corrected_eig_ = gtsamPoseToPoseEig(corrected_esti_.at(i)); 186 | } 187 | loop_added_flag_ = false; 188 | } 189 | high_resolution_clock::time_point t6 = high_resolution_clock::now(); 190 | 191 | ROS_INFO("real: %.1f, key_add: %.1f, vis: %.1f, opt: %.1f, res: %.1f, tot: %.1fms", 192 | duration_cast(t2 - t1).count() / 1e3, 193 | duration_cast(t3 - t2).count() / 1e3, 194 | duration_cast(t4 - t3).count() / 1e3, 195 | duration_cast(t5 - t4).count() / 1e3, 196 | duration_cast(t6 - t5).count() / 1e3, 197 | duration_cast(t6 - t1).count() / 1e3); 198 | } 199 | } 200 | return; 201 | } 202 | 203 | void FastLioSamQn::loopTimerFunc(const ros::TimerEvent &event) 204 | { 205 | auto &latest_keyframe = keyframes_.back(); 206 | if (!is_initialized_ || keyframes_.empty() || latest_keyframe.processed_) 207 | { 208 | return; 209 | } 210 | latest_keyframe.processed_ = true; 211 | 212 | high_resolution_clock::time_point t1 = high_resolution_clock::now(); 213 | const int closest_keyframe_idx = loop_closure_->fetchClosestKeyframeIdx(latest_keyframe, keyframes_); 214 | if (closest_keyframe_idx < 0) 215 | { 216 | return; 217 | } 218 | 219 | const RegistrationOutput ®_output = loop_closure_->performLoopClosure(latest_keyframe, keyframes_, closest_keyframe_idx); 220 | if (reg_output.is_valid_) 221 | { 222 | ROS_INFO("\033[1;32mLoop closure accepted. Score: %.3f\033[0m", reg_output.score_); 223 | const auto &score = reg_output.score_; 224 | gtsam::Pose3 pose_from = poseEigToGtsamPose(reg_output.pose_between_eig_ * latest_keyframe.pose_corrected_eig_); // IMPORTANT: take care of the order 225 | gtsam::Pose3 pose_to = poseEigToGtsamPose(keyframes_[closest_keyframe_idx].pose_corrected_eig_); 226 | auto variance_vector = (gtsam::Vector(6) << score, score, score, score, score, score).finished(); 227 | gtsam::noiseModel::Diagonal::shared_ptr loop_noise = gtsam::noiseModel::Diagonal::Variances(variance_vector); 228 | { 229 | std::lock_guard lock(graph_mutex_); 230 | gtsam_graph_.add(gtsam::BetweenFactor(latest_keyframe.idx_, 231 | closest_keyframe_idx, 232 | pose_from.between(pose_to), 233 | loop_noise)); 234 | } 235 | loop_idx_pairs_.push_back({latest_keyframe.idx_, closest_keyframe_idx}); // for vis 236 | loop_added_flag_vis_ = true; 237 | loop_added_flag_ = true; 238 | } 239 | else 240 | { 241 | ROS_WARN("Loop closure rejected. Score: %.3f", reg_output.score_); 242 | } 243 | high_resolution_clock::time_point t2 = high_resolution_clock::now(); 244 | 245 | debug_src_pub_.publish(pclToPclRos(loop_closure_->getSourceCloud(), map_frame_)); 246 | debug_dst_pub_.publish(pclToPclRos(loop_closure_->getTargetCloud(), map_frame_)); 247 | debug_fine_aligned_pub_.publish(pclToPclRos(loop_closure_->getFinalAlignedCloud(), map_frame_)); 248 | debug_coarse_aligned_pub_.publish(pclToPclRos(loop_closure_->getCoarseAlignedCloud(), map_frame_)); 249 | 250 | ROS_INFO("loop: %.1f", duration_cast(t2 - t1).count() / 1e3); 251 | return; 252 | } 253 | 254 | void FastLioSamQn::visTimerFunc(const ros::TimerEvent &event) 255 | { 256 | if (!is_initialized_) 257 | { 258 | return; 259 | } 260 | 261 | high_resolution_clock::time_point tv1 = high_resolution_clock::now(); 262 | //// 1. if loop closed, correct vis data 263 | if (loop_added_flag_vis_) 264 | // copy and ready 265 | { 266 | gtsam::Values corrected_esti_copied; 267 | pcl::PointCloud corrected_odoms; 268 | nav_msgs::Path corrected_path; 269 | { 270 | std::lock_guard lock(realtime_pose_mutex_); 271 | corrected_esti_copied = corrected_esti_; 272 | } 273 | // correct pose and path 274 | for (size_t i = 0; i < corrected_esti_copied.size(); ++i) 275 | { 276 | gtsam::Pose3 pose_ = corrected_esti_copied.at(i); 277 | corrected_odoms.points.emplace_back(pose_.translation().x(), pose_.translation().y(), pose_.translation().z()); 278 | corrected_path.poses.push_back(gtsamPoseToPoseStamped(pose_, map_frame_)); 279 | } 280 | // update vis of loop constraints 281 | if (!loop_idx_pairs_.empty()) 282 | { 283 | loop_detection_pub_.publish(getLoopMarkers(corrected_esti_copied)); 284 | } 285 | // update with corrected data 286 | { 287 | std::lock_guard lock(vis_mutex_); 288 | corrected_odoms_ = corrected_odoms; 289 | corrected_path_.poses = corrected_path.poses; 290 | } 291 | loop_added_flag_vis_ = false; 292 | } 293 | //// 2. publish odoms, paths 294 | { 295 | std::lock_guard lock(vis_mutex_); 296 | odom_pub_.publish(pclToPclRos(odoms_, map_frame_)); 297 | path_pub_.publish(odom_path_); 298 | corrected_odom_pub_.publish(pclToPclRos(corrected_odoms_, map_frame_)); 299 | corrected_path_pub_.publish(corrected_path_); 300 | } 301 | 302 | //// 3. global map 303 | if (global_map_vis_switch_ && corrected_pcd_map_pub_.getNumSubscribers() > 0) // save time, only once 304 | { 305 | pcl::PointCloud::Ptr corrected_map(new pcl::PointCloud()); 306 | corrected_map->reserve(keyframes_[0].pcd_.size() * keyframes_.size()); // it's an approximated size 307 | { 308 | std::lock_guard lock(keyframes_mutex_); 309 | for (size_t i = 0; i < keyframes_.size(); ++i) 310 | { 311 | *corrected_map += transformPcd(keyframes_[i].pcd_, keyframes_[i].pose_corrected_eig_); 312 | } 313 | } 314 | const auto &voxelized_map = voxelizePcd(corrected_map, voxel_res_); 315 | corrected_pcd_map_pub_.publish(pclToPclRos(*voxelized_map, map_frame_)); 316 | global_map_vis_switch_ = false; 317 | } 318 | if (!global_map_vis_switch_ && corrected_pcd_map_pub_.getNumSubscribers() == 0) 319 | { 320 | global_map_vis_switch_ = true; 321 | } 322 | high_resolution_clock::time_point tv2 = high_resolution_clock::now(); 323 | ROS_INFO("vis: %.1fms", duration_cast(tv2 - tv1).count() / 1e3); 324 | return; 325 | } 326 | 327 | void FastLioSamQn::saveFlagCallback(const std_msgs::String::ConstPtr &msg) 328 | { 329 | std::string save_dir = msg->data != "" ? msg->data : package_path_; 330 | 331 | // save scans as individual pcd files and poses in KITTI format 332 | // Delete the scans folder if it exists and create a new one 333 | std::string seq_directory = save_dir + "/" + seq_name_; 334 | std::string scans_directory = seq_directory + "/scans"; 335 | if (save_in_kitti_format_) 336 | { 337 | ROS_INFO("\033[32;1mScans are saved in %s, following the KITTI and TUM format\033[0m", scans_directory.c_str()); 338 | if (fs::exists(seq_directory)) 339 | { 340 | fs::remove_all(seq_directory); 341 | } 342 | fs::create_directories(scans_directory); 343 | 344 | std::ofstream kitti_pose_file(seq_directory + "/poses_kitti.txt"); 345 | std::ofstream tum_pose_file(seq_directory + "/poses_tum.txt"); 346 | tum_pose_file << "#timestamp x y z qx qy qz qw\n"; 347 | { 348 | std::lock_guard lock(keyframes_mutex_); 349 | for (size_t i = 0; i < keyframes_.size(); ++i) 350 | { 351 | // Save the point cloud 352 | std::stringstream ss_; 353 | ss_ << scans_directory << "/" << std::setw(6) << std::setfill('0') << i << ".pcd"; 354 | ROS_INFO("Saving %s...", ss_.str().c_str()); 355 | pcl::io::savePCDFileASCII(ss_.str(), keyframes_[i].pcd_); 356 | 357 | // Save the pose in KITTI format 358 | const auto &pose_ = keyframes_[i].pose_corrected_eig_; 359 | kitti_pose_file << pose_(0, 0) << " " << pose_(0, 1) << " " << pose_(0, 2) << " " 360 | << pose_(0, 3) << " " << pose_(1, 0) << " " << pose_(1, 1) << " " 361 | << pose_(1, 2) << " " << pose_(1, 3) << " " << pose_(2, 0) << " " 362 | << pose_(2, 1) << " " << pose_(2, 2) << " " << pose_(2, 3) << "\n"; 363 | 364 | const auto &lidar_optim_pose_ = poseEigToPoseStamped(keyframes_[i].pose_corrected_eig_); 365 | tum_pose_file << std::fixed << std::setprecision(8) << keyframes_[i].timestamp_ 366 | << " " << lidar_optim_pose_.pose.position.x << " " 367 | << lidar_optim_pose_.pose.position.y << " " 368 | << lidar_optim_pose_.pose.position.z << " " 369 | << lidar_optim_pose_.pose.orientation.x << " " 370 | << lidar_optim_pose_.pose.orientation.y << " " 371 | << lidar_optim_pose_.pose.orientation.z << " " 372 | << lidar_optim_pose_.pose.orientation.w << "\n"; 373 | } 374 | } 375 | kitti_pose_file.close(); 376 | tum_pose_file.close(); 377 | ROS_INFO("\033[32;1mScans and poses saved in .pcd and KITTI format\033[0m"); 378 | } 379 | 380 | if (save_map_bag_) 381 | { 382 | rosbag::Bag bag; 383 | bag.open(package_path_ + "/result.bag", rosbag::bagmode::Write); 384 | { 385 | std::lock_guard lock(keyframes_mutex_); 386 | for (size_t i = 0; i < keyframes_.size(); ++i) 387 | { 388 | ros::Time time; 389 | time.fromSec(keyframes_[i].timestamp_); 390 | bag.write("/keyframe_pcd", time, pclToPclRos(keyframes_[i].pcd_, map_frame_)); 391 | bag.write("/keyframe_pose", time, poseEigToPoseStamped(keyframes_[i].pose_corrected_eig_)); 392 | } 393 | } 394 | bag.close(); 395 | ROS_INFO("\033[36;1mResult saved in .bag format!!!\033[0m"); 396 | } 397 | 398 | if (save_map_pcd_) 399 | { 400 | pcl::PointCloud::Ptr corrected_map(new pcl::PointCloud()); 401 | corrected_map->reserve(keyframes_[0].pcd_.size() * keyframes_.size()); // it's an approximated size 402 | { 403 | std::lock_guard lock(keyframes_mutex_); 404 | for (size_t i = 0; i < keyframes_.size(); ++i) 405 | { 406 | *corrected_map += transformPcd(keyframes_[i].pcd_, keyframes_[i].pose_corrected_eig_); 407 | } 408 | } 409 | const auto &voxelized_map = voxelizePcd(corrected_map, voxel_res_); 410 | pcl::io::savePCDFileASCII(seq_directory + "/" + seq_name_ + "_map.pcd", *voxelized_map); 411 | ROS_INFO("\033[32;1mAccumulated map cloud saved in .pcd format\033[0m"); 412 | } 413 | } 414 | 415 | FastLioSamQn::~FastLioSamQn() 416 | { 417 | // save map 418 | if (save_map_bag_) 419 | { 420 | rosbag::Bag bag; 421 | bag.open(package_path_ + "/result.bag", rosbag::bagmode::Write); 422 | { 423 | std::lock_guard lock(keyframes_mutex_); 424 | for (size_t i = 0; i < keyframes_.size(); ++i) 425 | { 426 | ros::Time time; 427 | time.fromSec(keyframes_[i].timestamp_); 428 | bag.write("/keyframe_pcd", time, pclToPclRos(keyframes_[i].pcd_, map_frame_)); 429 | bag.write("/keyframe_pose", time, poseEigToPoseStamped(keyframes_[i].pose_corrected_eig_)); 430 | } 431 | } 432 | bag.close(); 433 | ROS_INFO("\033[36;1mResult saved in .bag format!!!\033[0m"); 434 | } 435 | if (save_map_pcd_) 436 | { 437 | pcl::PointCloud::Ptr corrected_map(new pcl::PointCloud()); 438 | corrected_map->reserve(keyframes_[0].pcd_.size() * keyframes_.size()); // it's an approximated size 439 | { 440 | std::lock_guard lock(keyframes_mutex_); 441 | for (size_t i = 0; i < keyframes_.size(); ++i) 442 | { 443 | *corrected_map += transformPcd(keyframes_[i].pcd_, keyframes_[i].pose_corrected_eig_); 444 | } 445 | } 446 | const auto &voxelized_map = voxelizePcd(corrected_map, voxel_res_); 447 | pcl::io::savePCDFileASCII(package_path_ + "/result.pcd", *voxelized_map); 448 | ROS_INFO("\033[32;1mResult saved in .pcd format!!!\033[0m"); 449 | } 450 | } 451 | 452 | void FastLioSamQn::updateOdomsAndPaths(const PosePcd &pose_pcd_in) 453 | { 454 | odoms_.points.emplace_back(pose_pcd_in.pose_eig_(0, 3), 455 | pose_pcd_in.pose_eig_(1, 3), 456 | pose_pcd_in.pose_eig_(2, 3)); 457 | corrected_odoms_.points.emplace_back(pose_pcd_in.pose_corrected_eig_(0, 3), 458 | pose_pcd_in.pose_corrected_eig_(1, 3), 459 | pose_pcd_in.pose_corrected_eig_(2, 3)); 460 | odom_path_.poses.emplace_back(poseEigToPoseStamped(pose_pcd_in.pose_eig_, map_frame_)); 461 | corrected_path_.poses.emplace_back(poseEigToPoseStamped(pose_pcd_in.pose_corrected_eig_, map_frame_)); 462 | return; 463 | } 464 | 465 | visualization_msgs::Marker FastLioSamQn::getLoopMarkers(const gtsam::Values &corrected_esti_in) 466 | { 467 | visualization_msgs::Marker edges; 468 | edges.type = 5u; 469 | edges.scale.x = 0.12f; 470 | edges.header.frame_id = map_frame_; 471 | edges.pose.orientation.w = 1.0f; 472 | edges.color.r = 1.0f; 473 | edges.color.g = 1.0f; 474 | edges.color.b = 1.0f; 475 | edges.color.a = 1.0f; 476 | for (size_t i = 0; i < loop_idx_pairs_.size(); ++i) 477 | { 478 | if (loop_idx_pairs_[i].first >= corrected_esti_in.size() || 479 | loop_idx_pairs_[i].second >= corrected_esti_in.size()) 480 | { 481 | continue; 482 | } 483 | gtsam::Pose3 pose = corrected_esti_in.at(loop_idx_pairs_[i].first); 484 | gtsam::Pose3 pose2 = corrected_esti_in.at(loop_idx_pairs_[i].second); 485 | geometry_msgs::Point p, p2; 486 | p.x = pose.translation().x(); 487 | p.y = pose.translation().y(); 488 | p.z = pose.translation().z(); 489 | p2.x = pose2.translation().x(); 490 | p2.y = pose2.translation().y(); 491 | p2.z = pose2.translation().z(); 492 | edges.points.push_back(p); 493 | edges.points.push_back(p2); 494 | } 495 | return edges; 496 | } 497 | 498 | bool FastLioSamQn::checkIfKeyframe(const PosePcd &pose_pcd_in, const PosePcd &latest_pose_pcd) 499 | { 500 | 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(); 501 | } 502 | -------------------------------------------------------------------------------- /fast_lio_sam_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 | int LoopClosure::fetchClosestKeyframeIdx(const PosePcd &front_keyframe, 35 | const std::vector &keyframes) 36 | { 37 | const auto &loop_det_radi = config_.loop_detection_radius_; 38 | const auto &loop_det_tdiff_thr = config_.loop_detection_timediff_threshold_; 39 | double shortest_distance_ = loop_det_radi * 3.0; 40 | int closest_idx = -1; 41 | for (size_t idx = 0; idx < keyframes.size() - 1; ++idx) 42 | { 43 | // check if potential loop: close enough in distance, far enough in time 44 | double tmp_dist = (keyframes[idx].pose_corrected_eig_.block<3, 1>(0, 3) - front_keyframe.pose_corrected_eig_.block<3, 1>(0, 3)).norm(); 45 | if (loop_det_radi > tmp_dist && 46 | loop_det_tdiff_thr < (front_keyframe.timestamp_ - keyframes[idx].timestamp_)) 47 | { 48 | if (tmp_dist < shortest_distance_) 49 | { 50 | shortest_distance_ = tmp_dist; 51 | closest_idx = keyframes[idx].idx_; 52 | } 53 | } 54 | } 55 | return closest_idx; 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 | { 164 | closest_keyframe_idx_ = fetchClosestKeyframeIdx(query_keyframe, keyframes); 165 | return performLoopClosure(query_keyframe, keyframes, closest_keyframe_idx_); 166 | } 167 | 168 | RegistrationOutput LoopClosure::performLoopClosure(const PosePcd &query_keyframe, 169 | const std::vector &keyframes, 170 | const int closest_keyframe_idx) 171 | { 172 | RegistrationOutput reg_output; 173 | closest_keyframe_idx_ = closest_keyframe_idx; 174 | if (closest_keyframe_idx_ >= 0) 175 | { 176 | // Quatro + NANO-GICP to check loop (from front_keyframe to closest keyframe's neighbor) 177 | const auto &[src_cloud, dst_cloud] = setSrcAndDstCloud(keyframes, 178 | query_keyframe.idx_, 179 | closest_keyframe_idx_, 180 | config_.num_submap_keyframes_, 181 | config_.voxel_res_, 182 | config_.enable_quatro_, 183 | config_.enable_submap_matching_); 184 | // Only for visualization 185 | *src_cloud_ = src_cloud; 186 | *dst_cloud_ = dst_cloud; 187 | 188 | if (config_.enable_quatro_) 189 | { 190 | std::cout << "\033[1;35mExecute coarse-to-fine alignment: " << src_cloud.size() 191 | << " vs " << dst_cloud.size() << "\033[0m\n"; 192 | return coarseToFineAlignment(src_cloud, dst_cloud); 193 | } 194 | else 195 | { 196 | std::cout << "\033[1;35mExecute GICP: " << src_cloud.size() << " vs " 197 | << dst_cloud.size() << "\033[0m\n"; 198 | return icpAlignment(src_cloud, dst_cloud); 199 | } 200 | } 201 | else 202 | { 203 | return reg_output; // dummy output whose `is_valid` is false 204 | } 205 | } 206 | 207 | pcl::PointCloud LoopClosure::getSourceCloud() 208 | { 209 | return *src_cloud_; 210 | } 211 | 212 | pcl::PointCloud LoopClosure::getTargetCloud() 213 | { 214 | return *dst_cloud_; 215 | } 216 | 217 | pcl::PointCloud LoopClosure::getCoarseAlignedCloud() 218 | { 219 | return coarse_aligned_; 220 | } 221 | 222 | // NOTE(hlim): To cover ICP-only mode, I just set `Final`, not `Fine` 223 | pcl::PointCloud LoopClosure::getFinalAlignedCloud() 224 | { 225 | return aligned_; 226 | } 227 | 228 | int LoopClosure::getClosestKeyframeidx() 229 | { 230 | return closest_keyframe_idx_; 231 | } 232 | -------------------------------------------------------------------------------- /fast_lio_sam_qn/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "fast_lio_sam_qn.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "fast_lio_sam_qn_node"); 6 | ros::NodeHandle nh_private("~"); 7 | 8 | FastLioSamQn fast_lio_sam_qn_(nh_private); 9 | 10 | ros::AsyncSpinner spinner(4); // Use multi threads 11 | spinner.start(); 12 | ros::waitForShutdown(); 13 | 14 | fast_lio_sam_qn_.~FastLioSamQn(); // Explicit call of destructor 15 | 16 | return 0; 17 | } 18 | -------------------------------------------------------------------------------- /imgs/fast1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/engcang/FAST-LIO-SAM-QN/c586b8955a1b726243f4c44b56fe9ef9100b0e61/imgs/fast1.png -------------------------------------------------------------------------------- /imgs/fast2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/engcang/FAST-LIO-SAM-QN/c586b8955a1b726243f4c44b56fe9ef9100b0e61/imgs/fast2.png -------------------------------------------------------------------------------- /imgs/qn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/engcang/FAST-LIO-SAM-QN/c586b8955a1b726243f4c44b56fe9ef9100b0e61/imgs/qn.png -------------------------------------------------------------------------------- /imgs/qn_side.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/engcang/FAST-LIO-SAM-QN/c586b8955a1b726243f4c44b56fe9ef9100b0e61/imgs/qn_side.png -------------------------------------------------------------------------------- /imgs/sam1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/engcang/FAST-LIO-SAM-QN/c586b8955a1b726243f4c44b56fe9ef9100b0e61/imgs/sam1.png -------------------------------------------------------------------------------- /imgs/sam2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/engcang/FAST-LIO-SAM-QN/c586b8955a1b726243f4c44b56fe9ef9100b0e61/imgs/sam2.png -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------