├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── Log ├── Colmap │ └── sparse │ │ └── 0 │ │ ├── cameras.txt │ │ ├── images.txt │ │ └── points3D.txt ├── PCD │ └── .gitkeep ├── guide.md ├── plot.py └── result │ ├── .gitkeep │ └── ntu_viral │ ├── README.md │ ├── eee_01_gt.txt │ ├── eee_01_prism.txt │ ├── eee_02_gt.txt │ ├── eee_02_prism.txt │ ├── eee_03_gt.txt │ ├── eee_03_prism.txt │ ├── evaluate_viral.py │ ├── nya_01_gt.txt │ ├── nya_01_prism.txt │ ├── nya_02_gt.txt │ ├── nya_02_prism.txt │ ├── nya_03_gt.txt │ ├── nya_03_prism.txt │ ├── sbs_01_gt.txt │ ├── sbs_01_prism.txt │ ├── sbs_02_gt.txt │ ├── sbs_02_prism.txt │ ├── sbs_03_gt.txt │ └── sbs_03_prism.txt ├── README.md ├── Supplementary └── LIVO2_supplementary.pdf ├── config ├── MARS_LVIG.yaml ├── NTU_VIRAL.yaml ├── avia.yaml ├── camera_MARS_LVIG.yaml ├── camera_NTU_VIRAL.yaml └── camera_pinhole.yaml ├── include ├── IMU_Processing.h ├── LIVMapper.h ├── common_lib.h ├── feature.h ├── frame.h ├── livox_ros_driver │ ├── CustomMsg.h │ └── CustomPoint.h ├── preprocess.h ├── utils │ ├── color.h │ ├── so3_math.h │ ├── types.h │ └── utils.h ├── vio.h ├── visual_point.h └── voxel_map.h ├── launch ├── mapping_avia_marslvig.launch.py ├── mapping_aviz.launch.py └── mapping_ouster_ntu.launch.py ├── package.xml ├── pics ├── Framework.png ├── debug_error.png └── rosgraph.png ├── rviz_cfg ├── M300.rviz ├── fast_livo2.rviz └── ntu_viral.rviz ├── scripts ├── colmap_output.sh └── mesh.py └── src ├── IMU_Processing.cpp ├── LIVMapper.cpp ├── frame.cpp ├── main.cpp ├── preprocess.cpp ├── utils.cpp ├── vio.cpp ├── visual_point.cpp └── voxel_map.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | Log/* 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(fast_livo) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}") 6 | 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | # Set common compile options 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") 13 | 14 | # Specific settings for Debug build 15 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -g") 16 | 17 | # Detect CPU architecture 18 | message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") 19 | 20 | # Specific settings for Release build 21 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(arm|aarch64|ARM|AARCH64)") 22 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") 23 | # 64-bit ARM optimizations (e.g., RK3588 and Jetson Orin NX) 24 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -ffast-math") 25 | message(STATUS "Using 64-bit ARM optimizations: -O3 -mcpu=native -mtune=native -ffast-math") 26 | else() 27 | # 32-bit ARM optimizations with NEON support 28 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math") 29 | message(STATUS "Using 32-bit ARM optimizations: -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math") 30 | endif() 31 | add_definitions(-DARM_ARCH) 32 | else() 33 | # x86-64 (Intel/AMD) optimizations 34 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native -mtune=native -funroll-loops") #-flto 35 | message(STATUS "Using general x86 optimizations: -O3 -march=native -mtune=native -funroll-loops") 36 | add_definitions(-DX86_ARCH) 37 | endif() 38 | 39 | # Define project root directory 40 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 41 | 42 | # Detect CPU core count for potential multithreading optimization 43 | include(ProcessorCount) 44 | ProcessorCount(N) 45 | message(STATUS "Processor count: ${N}") 46 | 47 | # Set the number of cores for multithreading 48 | if(N GREATER 4) 49 | math(EXPR PROC_NUM "4") 50 | add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM}) 51 | message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}") 52 | elseif(N GREATER 1) 53 | math(EXPR PROC_NUM "${N}") 54 | add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM}) 55 | message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}") 56 | else() 57 | add_definitions(-DMP_PROC_NUM=1) 58 | message(STATUS "Single core detected. Multithreading disabled.") 59 | endif() 60 | 61 | # Check for OpenMP support 62 | find_package(OpenMP QUIET) 63 | if(OpenMP_CXX_FOUND) 64 | message(STATUS "OpenMP found") 65 | add_compile_options(${OpenMP_CXX_FLAGS}) 66 | else() 67 | message(STATUS "OpenMP not found, proceeding without it") 68 | endif() 69 | 70 | # Check for mimalloc support 71 | find_package(mimalloc QUIET) 72 | if(mimalloc_FOUND) 73 | message(STATUS "mimalloc found") 74 | else() 75 | message(STATUS "mimalloc not found, proceeding without it") 76 | endif() 77 | 78 | # Find ament and required dependencies 79 | find_package(ament_cmake REQUIRED) 80 | find_package(rclcpp REQUIRED) 81 | find_package(rclpy REQUIRED) 82 | find_package(geometry_msgs REQUIRED) 83 | find_package(nav_msgs REQUIRED) 84 | find_package(sensor_msgs REQUIRED) 85 | find_package(visualization_msgs REQUIRED) 86 | find_package(pcl_ros REQUIRED) 87 | find_package(pcl_conversions REQUIRED) 88 | find_package(tf2_ros REQUIRED) 89 | find_package(livox_ros_driver2 REQUIRED) 90 | find_package(vikit_common REQUIRED) 91 | find_package(vikit_ros REQUIRED) 92 | find_package(cv_bridge REQUIRED) 93 | find_package(image_transport REQUIRED) 94 | find_package(Eigen3 REQUIRED) 95 | find_package(PCL REQUIRED) 96 | find_package(OpenCV REQUIRED) 97 | find_package(Sophus REQUIRED) 98 | find_package(Boost REQUIRED COMPONENTS thread) 99 | 100 | # Include directories for dependencies 101 | include_directories( 102 | ${EIGEN3_INCLUDE_DIR} 103 | ${PCL_INCLUDE_DIRS} 104 | ${OpenCV_INCLUDE_DIRS} 105 | ${Sophus_INCLUDE_DIRS} 106 | ${vikit_common_INCLUDE_DIRS} 107 | ${vikit_ros_INCLUDE_DIRS} 108 | include 109 | ) 110 | 111 | set(dependencies 112 | rclcpp 113 | rclpy 114 | geometry_msgs 115 | nav_msgs 116 | sensor_msgs 117 | visualization_msgs 118 | cv_bridge 119 | vikit_common 120 | vikit_ros 121 | image_transport 122 | pcl_ros 123 | pcl_conversions 124 | tf2_ros 125 | livox_ros_driver2 126 | ) 127 | 128 | set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX) 129 | 130 | # link_directories(${COMMON_DEPENDENCIES} 131 | # ${vikit_common_LIBRARIES}/libvikit_common.so 132 | # ${vikit_ros_LIBRARIES}/libvikit_ros.so 133 | # ) 134 | 135 | # Add libraries 136 | add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp) 137 | add_library(lio src/voxel_map.cpp) 138 | add_library(pre src/preprocess.cpp) 139 | add_library(imu_proc src/IMU_Processing.cpp) 140 | add_library(laser_mapping src/LIVMapper.cpp) 141 | add_library(utils src/utils.cpp) 142 | 143 | ament_target_dependencies(vio ${dependencies} ) 144 | ament_target_dependencies(lio ${dependencies}) 145 | ament_target_dependencies(pre ${dependencies}) 146 | ament_target_dependencies(imu_proc ${dependencies}) 147 | ament_target_dependencies(laser_mapping ${dependencies}) 148 | 149 | # linking libraries or executables to public dependencies 150 | target_link_libraries(laser_mapping 151 | ${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so 152 | ${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so 153 | ${COMMON_DEPENDENCIES} 154 | ) 155 | target_link_libraries(vio ${COMMON_DEPENDENCIES}) 156 | target_link_libraries(lio utils ${COMMON_DEPENDENCIES}) 157 | target_link_libraries(pre ${COMMON_DEPENDENCIES}) 158 | target_link_libraries(imu_proc ${COMMON_DEPENDENCIES}) 159 | 160 | # Add the main executable 161 | add_executable(fastlivo_mapping src/main.cpp) 162 | 163 | ament_target_dependencies(fastlivo_mapping ${dependencies}) 164 | 165 | # Link libraries to the executable 166 | target_link_libraries(fastlivo_mapping 167 | laser_mapping 168 | vio 169 | lio 170 | pre 171 | imu_proc 172 | ${PCL_LIBRARIES} 173 | ${OpenCV_LIBRARIES} 174 | ${Sophus_LIBRARIES} 175 | ${Boost_LIBRARIES} 176 | ) 177 | 178 | # Link mimalloc if found 179 | if(mimalloc_FOUND) 180 | target_link_libraries(fastlivo_mapping mimalloc) 181 | endif() 182 | 183 | # Install the executable 184 | install(TARGETS 185 | fastlivo_mapping 186 | DESTINATION lib/${PROJECT_NAME} 187 | ) 188 | 189 | install( 190 | DIRECTORY config launch rviz_cfg 191 | DESTINATION share/${PROJECT_NAME} 192 | ) 193 | 194 | # Export dependencies 195 | ament_export_dependencies( 196 | rclcpp 197 | rclpy 198 | geometry_msgs 199 | nav_msgs 200 | sensor_msgs 201 | pcl_ros 202 | pcl_conversions 203 | tf2_ros 204 | livox_ros_driver2 205 | Eigen3 206 | PCL 207 | OpenCV 208 | Sophus 209 | ) 210 | 211 | ament_package() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | 294 | Copyright (C) 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | , 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /Log/Colmap/sparse/0/cameras.txt: -------------------------------------------------------------------------------- 1 | # Camera list with one line of data per camera: 2 | # CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[] 3 | 1 PINHOLE 640 512 588.143715 588.107927 296.059369 254.543215 4 | -------------------------------------------------------------------------------- /Log/Colmap/sparse/0/images.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/Log/Colmap/sparse/0/images.txt -------------------------------------------------------------------------------- /Log/Colmap/sparse/0/points3D.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/Log/Colmap/sparse/0/points3D.txt -------------------------------------------------------------------------------- /Log/PCD/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/Log/PCD/.gitkeep -------------------------------------------------------------------------------- /Log/guide.md: -------------------------------------------------------------------------------- 1 | Here are the saved debug records, which can be plotted using ./Log/plot.py. 2 | -------------------------------------------------------------------------------- /Log/plot.py: -------------------------------------------------------------------------------- 1 | # import matplotlib 2 | # matplotlib.use('Agg') 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | fig, axs = plt.subplots(3,2) 7 | lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] 8 | lab_out = ['', 'out-x', 'out-y', 'out-z'] 9 | plot_ind = range(7,10) 10 | a_pre=np.loadtxt('mat_pre.txt') 11 | a_out=np.loadtxt('mat_out.txt') 12 | time_pre=a_pre[:,0] 13 | time_out=a_out[:,0] 14 | axs[0,0].set_title('Attitude') 15 | axs[1,0].set_title('Translation') 16 | axs[2,0].set_title('Velocity') 17 | axs[0,1].set_title('bg') 18 | axs[1,1].set_title('ba') 19 | axs[2,1].set_title('ExposureTime') 20 | for i in range(1,4): 21 | for j in range(5): 22 | axs[j%3, j//3].plot(time_pre, a_pre[:,i+j*3],'.-', label=lab_pre[i]) 23 | axs[j%3, j//3].plot(time_out, a_out[:,i+j*3],'.-', label=lab_out[i]) 24 | axs[j%3, j//3].legend() 25 | #for i in range(1,4): 26 | axs[2, 1].plot(time_pre, a_pre[:,1+5*3],'.-', label=lab_pre[0]) 27 | axs[2, 1].plot(time_out, a_out[:,1+5*3],'.-', label=lab_pre[0]) 28 | axs[2, 1].legend(['pre', 'out']) 29 | #axs[2, 1].legend(['pre', 'out']) 30 | 31 | for j in range(6): 32 | # axs[j].set_xlim(386,389) 33 | #if j==5: 34 | #continue 35 | axs[j%3, j//3].grid() 36 | #axs[j%3, j/3].legend() 37 | plt.grid() 38 | 39 | #### Draw IMU data 40 | fig, axs = plt.subplots(2) 41 | imu=np.loadtxt('imu.txt') 42 | time=imu[:,0] 43 | axs[0].set_title('Gyroscope') 44 | axs[1].set_title('Accelerameter') 45 | lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] 46 | lab_2 = ['acc-x', 'acc-y', 'acc-z'] 47 | for i in range(3): 48 | # if i==1: 49 | axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) 50 | axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) 51 | for i in range(2): 52 | # axs[i].set_xlim(386,389) 53 | axs[i].grid() 54 | axs[i].legend() 55 | plt.grid() 56 | 57 | # #### Draw time calculation 58 | # plt.figure(3) 59 | # fig = plt.figure() 60 | # font1 = {'family' : 'Times New Roman', 61 | # 'weight' : 'normal', 62 | # 'size' : 12, 63 | # } 64 | # c="red" 65 | # a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') 66 | # a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') 67 | # a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') 68 | # # n = a_out[:,1].size 69 | # # time_mean = a_out[:,1].mean() 70 | # # time_se = a_out[:,1].std() / np.sqrt(n) 71 | # # time_err = a_out[:,1] - time_mean 72 | # # feat_mean = a_out[:,2].mean() 73 | # # feat_err = a_out[:,2] - feat_mean 74 | # # feat_se = a_out[:,2].std() / np.sqrt(n) 75 | # ax1 = fig.add_subplot(111) 76 | # ax1.set_ylabel('Effective Feature Numbers',font1) 77 | # ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) 78 | # ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) 79 | # ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) 80 | # ax1.set_ylim([0, 3000]) 81 | 82 | # ax2 = ax1.twinx() 83 | # ax2.spines['right'].set_color('red') 84 | # ax2.set_ylabel('Compute Time (ms)',font1) 85 | # ax2.yaxis.label.set_color('red') 86 | # ax2.tick_params(axis='y', colors='red') 87 | # ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 88 | # ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 89 | # ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 90 | # ax2.set_xlim([0.5, 3.5]) 91 | # ax2.set_ylim([0, 100]) 92 | 93 | # plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) 94 | # # # print(time_se) 95 | # # # print(a_out3[:,2]) 96 | # plt.grid() 97 | # plt.savefig("time.pdf", dpi=1200) 98 | plt.show() 99 | -------------------------------------------------------------------------------- /Log/result/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/Log/result/.gitkeep -------------------------------------------------------------------------------- /Log/result/ntu_viral/README.md: -------------------------------------------------------------------------------- 1 | ## Usage Instructions 2 | The files in this folder follow the naming convention: `xxx_xx_*` 3 | 4 | - `xxx`: Scene name (e.g., eee, nya, sbs). 5 | - `xx`: Sequence number (e.g., 01, 02, 03) for the same scene. 6 | - `xxx_xx_prism.txt`: Estimated poses from FAST-LIVO2 converted to the PRISM coordinate system. 7 | - `xxx_xx_gt.txt`: Ground truth trajectories converted to the TUM format. 8 | 9 | To compute RMSE between ground truth and estimated trajectories, run: 10 | ```bash 11 | evo_ape tum eee_01_gt.txt eee_01_prism.txt -a --plot --plot_mode xyz 12 | ``` 13 | Results below: 14 | | Scene | Sequence | RMSE (cm) | 15 | |--------|----------|-----------| 16 | | eee | 01 | 2.71 | 17 | | eee | 02 | 2.11 | 18 | | eee | 03 | 2.61 | 19 | | nya | 01 | 3.56 | 20 | | nya | 02 | 3.39 | 21 | | nya | 03 | 3.52 | 22 | | sbs | 01 | 2.34 | 23 | | sbs | 02 | 2.83 | 24 | | sbs | 03 | 3.11 | 25 | 26 | **Note:** The provided files are results (*_prism.txt, *_gt.txt) from my runs. To compare your own FAST-LIVO2 pose outputs with ground truth, you must first run `evaluate_viral.py` to: 27 | - Convert the official [leica_pose.csv](https://github.com/ntu-aris/viral_eval) file to TUM format. 28 | - Transform SLAM trajectories from the **IMU frame** to the **PRISM coordinate system**. -------------------------------------------------------------------------------- /Log/result/ntu_viral/evaluate_viral.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import pandas as pd 4 | from scipy.spatial.transform import Rotation 5 | 6 | def convert_slam_to_prism(slam_tum_file, output_file): 7 | """ 8 | Convert SLAM trajectory in TUM format to prism coordinate system 9 | :param slam_tum_file: SLAM estimated trajectory file (TUM format, quaternion order x y z w) 10 | :param output_file: Output file path 11 | """ 12 | try: 13 | # Check file existence 14 | if not os.path.exists(slam_tum_file): 15 | raise FileNotFoundError(f"SLAM file not found: {slam_tum_file}") 16 | 17 | # Read SLAM trajectory data 18 | slam_data = pd.read_csv(slam_tum_file, sep=' ', header=None) 19 | timestamps = slam_data.iloc[:, 0].values 20 | positions = slam_data.iloc[:, 1:4].values 21 | quaternions = slam_data.iloc[:, 4:8].values # Quaternion order: x y z w 22 | 23 | # Static transformation vector from body to prism coordinates 24 | trans_B2prism = np.array([-0.293656, -0.012288, -0.273095]) 25 | 26 | # Convert positions to prism coordinate system 27 | positions_prism = positions + quat_rotate_vector(quaternions, trans_B2prism) 28 | 29 | # Save in TUM format 30 | output_data = np.column_stack((timestamps, positions_prism, quaternions)) 31 | np.savetxt(output_file, output_data, fmt='%.6f', delimiter=' ') 32 | print(f"Successfully processed SLAM data: {output_file}") 33 | 34 | except Exception as e: 35 | print(f"Error processing SLAM data: {str(e)}") 36 | raise 37 | 38 | def convert_leica_to_tum(leica_file, output_file): 39 | """ 40 | Convert leica_pose.csv to TUM format 41 | :param leica_file: leica_pose.csv file path 42 | :param output_file: Output file path 43 | """ 44 | try: 45 | # Check file existence 46 | if not os.path.exists(leica_file): 47 | raise FileNotFoundError(f"Leica file not found: {leica_file}") 48 | 49 | # Read Leica data 50 | leica_data = pd.read_csv(leica_file, skiprows=1, header=None).values 51 | timestamps = leica_data[:, 0] / 1e9 # Convert nanoseconds to seconds 52 | positions = leica_data[:, 3:6] 53 | quaternions = np.zeros((len(positions), 4)) # Assume identity rotation 54 | quaternions[:, 3] = 1 # w-component of quaternion 55 | 56 | # Save in TUM format 57 | output_data = np.column_stack((timestamps, positions, quaternions)) 58 | np.savetxt(output_file, output_data, fmt='%.6f', delimiter=' ') 59 | print(f"Successfully processed Leica data: {output_file}") 60 | 61 | except Exception as e: 62 | print(f"Error processing Leica data: {str(e)}") 63 | raise 64 | 65 | def quat_rotate_vector(quat, vec): 66 | """ 67 | Rotate vector using quaternion(s) (quaternion order: x y z w) 68 | :param quat: Quaternion array (N x 4, order [x, y, z, w]) 69 | :param vec: 3D vector to rotate 70 | :return: Rotated vectors (N x 3) 71 | """ 72 | # Create rotation object using x y z w order 73 | rot = Rotation.from_quat(quat[:, :4]) 74 | return rot.apply(vec) 75 | 76 | if __name__ == '__main__': 77 | # File paths 78 | input_files = { 79 | 'slam': 'eee_01.txt', 80 | 'leica': 'leica_pose.csv' 81 | } 82 | 83 | output_files = { 84 | 'slam': 'eee_01_prism.txt', 85 | 'leica': 'eee_01_gt.txt' 86 | } 87 | 88 | # Process SLAM data 89 | try: 90 | convert_slam_to_prism(input_files['slam'], output_files['slam']) 91 | except: 92 | print("Skipping SLAM processing") 93 | 94 | # Process Leica data 95 | try: 96 | convert_leica_to_tum(input_files['leica'], output_files['leica']) 97 | except: 98 | print("Skipping Leica processing") 99 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FAST-LIVO2 ROS2 HUMBLE 2 | 3 | ## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry 4 | 5 | Thanks to hku mars lab chunran zheng for the open source excellent work 6 | 7 | ### 📢 News 8 | 9 | - 🔓 **2025-01-23**: Code released! 10 | - 🎉 **2024-10-01**: Accepted by **T-RO '24**! 11 | - 🚀 **2024-07-02**: Conditionally accepted. 12 | 13 | ### 📬 Contact 14 | 15 | If you have any questions, please feel free to contact: Chunran Zheng [zhengcr@connect.hku.hk](mailto:zhengcr@connect.hku.hk). 16 | 17 | ## 1. Introduction 18 | 19 | FAST-LIVO2 is an efficient and accurate LiDAR-inertial-visual fusion localization and mapping system, demonstrating significant potential for real-time 3D reconstruction and onboard robotic localization in severely degraded environments. 20 | 21 |
22 | 23 |
24 | 25 | ### 1.1 Related video 26 | 27 | Our accompanying video is now available on [**Bilibili**](https://www.bilibili.com/video/BV1Ezxge7EEi) and [**YouTube**](https://youtu.be/6dF2DzgbtlY). 28 | 29 | ### 1.2 Related paper 30 | 31 | [FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry](https://arxiv.org/pdf/2408.14035) 32 | 33 | [FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry](https://arxiv.org/pdf/2203.00893) 34 | 35 | ### 1.3 Our hard-synchronized equipment 36 | 37 | We open-source our handheld device, including CAD files, synchronization scheme, STM32 source code, wiring instructions, and sensor ROS driver. Access these resources at this repository: [**LIV_handhold**](https://github.com/xuankuzcr/LIV_handhold). 38 | 39 | ### 1.4 Our associate dataset: FAST-LIVO2-Dataset 40 | Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z) used for evaluation is also available online. 41 | 42 | ### MARS-LVIG dataset 43 | [**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)：A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion 44 | 45 | ### 1.5 Our calibration method 46 | Stay tuned... 47 | 48 | ## 2. Prerequisited 49 | 50 | ### 2.1 Ubuntu and ROS 51 | 52 | Ubuntu 22.04. [ROS Installation](http://wiki.ros.org/ROS/Installation). 53 | 54 | ### 2.2 PCL && Eigen && OpenCV 55 | 56 | PCL>=1.8, Follow [PCL Installation](https://pointclouds.org/). 57 | 58 | Eigen>=3.3.4, Follow [Eigen Installation](https://eigen.tuxfamily.org/index.php?title=Main_Page). 59 | 60 | OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/). 61 | 62 | ### 2.3 Sophus 63 | 64 | #### Binary installation 65 | ```bash 66 | sudo apt install ros-$ROS_DISTRO-sophus 67 | ``` 68 | 69 | #### Building from source 70 | Sophus Installation for the non-templated/double-only version. 71 | 72 | ```bash 73 | git clone https://github.com/strasdat/Sophus.git 74 | cd Sophus 75 | git checkout a621ff 76 | mkdir build && cd build && cmake .. 77 | make 78 | sudo make install 79 | ``` 80 | 81 | if build fails due to `so2.cpp:32:26: error: lvalue required as left operand of assignment`, modify the code as follows: 82 | 83 | **so2.cpp** 84 | ```diff 85 | namespace Sophus 86 | { 87 | 88 | SO2::SO2() 89 | { 90 | - unit_complex_.real() = 1.; 91 | - unit_complex_.imag() = 0.; 92 | + unit_complex_.real(1.); 93 | + unit_complex_.imag(0.); 94 | } 95 | ``` 96 | 97 | ### 2.4 Vikit 98 | 99 | Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder. 100 | 101 | For well-known reasons, ROS2 does not have a direct global parameter server and a simple method to obtain the corresponding parameters. For details, please refer to https://discourse.ros.org/t/ros2-global-parameter-server-status/10114/11. I use a special way to get camera parameters in Vikit. While the method I've provided so far is quite simple and not perfect, it meets my needs. More contributions to improve `rpg_vikit` are hoped. 102 | 103 | ```bash 104 | # Different from the one used in fast-livo1 105 | cd fast_ws/src 106 | git clone https://github.com/Robotic-Developer-Road/rpg_vikit.git 107 | ``` 108 | 109 | Thanks to the following repositories for the code reference: 110 | 111 | - [uzh-rpg/rpg_vikit](https://github.com/uzh-rpg/rpg_vikit) 112 | - [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit) 113 | - [uavfly/vikit](https://github.com/uavfly/vikit) 114 | 115 | ### 2.5 **livox_ros_driver2** 116 | 117 | Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2). 118 | 119 | why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient. 120 | 121 | ## 3. Build 122 | 123 | Clone the repository and colcon build: 124 | 125 | ``` 126 | cd ~/fast_ws/src 127 | git clone https://github.com/Robotic-Developer-Road/FAST-LIVO2.git 128 | cd ../ 129 | colcon build --symlink-install --continue-on-error 130 | source ~/fast_ws/install/setup.bash 131 | ``` 132 | 133 | ## 4. Run our examples 134 | 135 | Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). 136 | 137 | ### convert rosbag 138 | 139 | convert ROS1 rosbag to ROS2 rosbag 140 | ```bash 141 | pip install rosbags 142 | rosbags-convert --src Retail_Street.bag --dst Retail_Street 143 | ``` 144 | - [gitlab rosbags](https://gitlab.com/ternaris/rosbags) 145 | - [pypi rosbags](https://pypi.org/project/rosbags/) 146 | 147 | ### change the msg type on rosbag 148 | 149 | Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file. 150 | 1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2. 151 | 2. change the msg type of msg type in **metadata.yaml** as follows: 152 | 153 | **metadata.yaml** 154 | ```diff 155 | rosbag2_bagfile_information: 156 | compression_format: '' 157 | compression_mode: '' 158 | custom_data: {} 159 | duration: 160 | nanoseconds: 135470252209 161 | files: 162 | - duration: 163 | nanoseconds: 135470252209 164 | message_count: 30157 165 | path: Retail_Street.db3 166 | .............. 167 | topic_metadata: 168 | name: /livox/lidar 169 | offered_qos_profiles: '' 170 | serialization_format: cdr 171 | - type: livox_ros_driver/msg/CustomMsg 172 | + type: livox_ros_driver2/msg/CustomMsg 173 | type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d 174 | ............... 175 | ..... 176 | ``` 177 | 178 | ### Run the demo 179 | 180 | Do not forget to `source` your ROS2 workspace before running the following command. 181 | 182 | ```bash 183 | ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True 184 | ros2 bag play -p Retail_Street # space bar controls play/pause 185 | ``` 186 | 187 | ## 5. License 188 | 189 | The source code of this package is released under the [**GPLv2**](http://www.gnu.org/licenses/) license. For commercial use, please contact me at and Prof. Fu Zhang at to discuss an alternative license. -------------------------------------------------------------------------------- /Supplementary/LIVO2_supplementary.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/Supplementary/LIVO2_supplementary.pdf -------------------------------------------------------------------------------- /config/MARS_LVIG.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | common: 4 | img_topic: "/left_camera/image" 5 | lid_topic: "/livox/lidar" 6 | imu_topic: "/livox/imu" 7 | img_en: 1 8 | lidar_en: 1 9 | ros_driver_bug_fix: false 10 | 11 | extrin_calib: 12 | extrinsic_T: [0.04165, 0.02326, -0.0284] 13 | extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 14 | # MARS_LVIG HKisland HKairport 15 | Rcl: [0.00438814,-0.999807,-0.0191582, 16 | -0.00978695,0.0191145,-0.999769, 17 | 0.999942,0.00457463,-0.00970118] 18 | Pcl: [0.016069, 0.0871753, -0.0718021] 19 | # MARS_LVIG AMtown AMvalley 20 | # Rcl: [ -0.0022464, -0.9997299, -0.0231319, 21 | # -0.0084211, 0.0231501, -0.9996966, 22 | # 0.9999620, -0.0020509, -0.0084708] 23 | # Pcl: [-0.0025563, 0.0567484, -0.0512149] 24 | 25 | time_offset: 26 | imu_time_offset: 0.0 27 | img_time_offset: -0.1 28 | exposure_time_init: 0.0 29 | # ╔═══════════════════════════════════════════════════════════════════════════════════════╗ 30 | # ║ Configuration Settings ║ 31 | # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ 32 | # ║ Series │ ID │ img_time_offset │ exposure_time_init │ -s (start hover) ║ 33 | # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ 34 | # ║ HKairport │ HKairport01 │ 0.1 │ 0.0 │ 75 ║ 35 | # ║ │ HKairport02 │ -0.1 │ 0.0 │ 60 ║ 36 | # ║ │ HKairport03 │ -0.1 │ 0.0 │ 62 ║ 37 | # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ 38 | # ║ HKisland │ HKisland01 │ 0.0 │ 0.0 │ 118 ║ 39 | # ║ │ HKisland02 │ 0.1 │ 0.0 │ 80 ║ 40 | # ║ │ HKisland03 │ -0.1 │ 0.0 │ 72 ║ 41 | # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ 42 | # ║ AMtown │ AMtown01 │ -0.1 │ 0.0285 │ 75 ║ 43 | # ║ │ AMtown02 │ -0.1 │ 0.0285 │ 50 ║ 44 | # ║ │ AMtown03 │ -0.1 │ 0.0285 │ 106 ║ 45 | # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ 46 | # ║ AMvalley │ AMvalley01 │ -0.1 │ 0.0132 │ 70 ║ 47 | # ║ │ AMvalley02 │ -0.1 │ 0.0132 │ 65 ║ 48 | # ║ │ AMvalley03 │ -0.1 │ 0.0132 │ 68 ║ 49 | # ╚═══════════════════════════════════════════════════════════════════════════════════════╝ 50 | preprocess: 51 | point_filter_num: 1 52 | filter_size_surf: 0.1 53 | lidar_type: 1 # Livox Avia LiDAR 54 | scan_line: 6 55 | blind: 0.8 56 | 57 | vio: 58 | max_iterations: 5 59 | outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite 60 | img_point_cov: 1000 # 100 1000 61 | patch_size: 8 62 | patch_pyrimid_level: 4 63 | normal_en: true 64 | raycast_en: false 65 | inverse_composition_en: false 66 | exposure_estimate_en: true 67 | inv_expo_cov: 0.1 68 | 69 | imu: 70 | imu_en: true 71 | imu_int_frame: 30 72 | acc_cov: 2.0 # 0.5 73 | gyr_cov: 0.1 # 0.3 74 | b_acc_cov: 0.0001 # 0.1 75 | b_gyr_cov: 0.0001 # 0.1 76 | 77 | lio: 78 | max_iterations: 5 79 | dept_err: 0.02 80 | beam_err: 0.05 81 | min_eigen_value: 0.005 82 | voxel_size: 2.0 # 1.0 83 | max_layer: 2 84 | max_points_num: 50 85 | layer_init_num: [5, 5, 5, 5, 5] 86 | 87 | local_map: 88 | map_sliding_en: false 89 | half_map_size: 100 90 | sliding_thresh: 8.0 91 | 92 | uav: 93 | imu_rate_odom: false 94 | gravity_align_en: false 95 | 96 | publish: 97 | dense_map_en: true 98 | pub_effect_point_en: false 99 | pub_plane_en: false 100 | pub_scan_num: 1 101 | blind_rgb_points: 0.0 102 | 103 | evo: 104 | seq_name: "HKisland03" 105 | pose_output_en: false 106 | 107 | pcd_save: 108 | pcd_save_en: false 109 | colmap_output_en: false # need to set interval = -1 110 | filter_size_pcd: 0.15 111 | interval: -1 112 | # how many LiDAR frames saved in each pcd file; 113 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. -------------------------------------------------------------------------------- /config/NTU_VIRAL.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | common: 4 | img_topic: "/left/image_raw" 5 | lid_topic: "/os1_cloud_node1/points" 6 | imu_topic: "/imu/imu" 7 | img_en: 1 8 | lidar_en: 1 9 | ros_driver_bug_fix: false 10 | 11 | extrin_calib: 12 | extrinsic_T: [-0.050, 0.000, 0.055] 13 | extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 14 | # NTU_VIRAL 15 | Rcl: [0.0218308, 0.99976, -0.00201407, 16 | -0.0131205, 0.00230088, 0.999911, 17 | 0.999676, -0.0218025, 0.0131676] 18 | Pcl: [0.122993, 0.0398643, -0.0577101] 19 | 20 | time_offset: 21 | lidar_time_offset: -0.1 22 | imu_time_offset: 0.0 23 | img_time_offset: 0.0 24 | exposure_time_init: 0.0 25 | 26 | preprocess: 27 | point_filter_num: 3 28 | filter_size_surf: 0.1 29 | lidar_type: 3 # Ouster 30 | scan_line: 16 31 | blind: 1.0 32 | 33 | vio: 34 | max_iterations: 5 35 | outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite 36 | img_point_cov: 100 # 100 1000 37 | patch_size: 8 38 | patch_pyrimid_level: 3 39 | normal_en: true 40 | raycast_en: false 41 | inverse_composition_en: false 42 | exposure_estimate_en: true 43 | inv_expo_cov: 0.1 44 | 45 | imu: 46 | imu_en: true 47 | imu_int_frame: 30 48 | acc_cov: 0.5 # 0.2 49 | gyr_cov: 0.3 # 0.5 50 | b_acc_cov: 0.0001 # 0.1 51 | b_gyr_cov: 0.0001 # 0.1 52 | 53 | lio: 54 | max_iterations: 5 55 | dept_err: 0.02 56 | beam_err: 0.01 57 | min_eigen_value: 0.0025 # 0.0025 58 | voxel_size: 0.5 59 | max_layer: 2 60 | max_points_num: 50 61 | layer_init_num: [5, 5, 5, 5, 5] 62 | 63 | local_map: 64 | map_sliding_en: false 65 | half_map_size: 100 66 | sliding_thresh: 8.0 67 | 68 | uav: 69 | imu_rate_odom: false 70 | gravity_align_en: false 71 | 72 | publish: 73 | dense_map_en: true 74 | pub_effect_point_en: false 75 | pub_plane_en: false 76 | pub_scan_num: 1 77 | blind_rgb_points: 0.0 78 | 79 | evo: 80 | seq_name: "eee_01" 81 | pose_output_en: true 82 | 83 | pcd_save: 84 | pcd_save_en: false 85 | colmap_output_en: false # need to set interval = -1 86 | filter_size_pcd: 0.15 87 | interval: -1 88 | # how many LiDAR frames saved in each pcd file; 89 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. -------------------------------------------------------------------------------- /config/avia.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | common: 4 | img_topic: "/left_camera/image" 5 | lid_topic: "/livox/lidar" 6 | imu_topic: "/livox/imu" 7 | img_en: 1 8 | lidar_en: 1 9 | ros_driver_bug_fix: false 10 | 11 | extrin_calib: 12 | extrinsic_T: [0.04165, 0.02326, -0.0284] 13 | extrinsic_R: [1.0, 0.0, 0.0, 14 | 0.0, 1.0, 0.0, 15 | 0.0, 0.0, 1.0] 16 | Rcl: [0.00610193,-0.999863,-0.0154172, 17 | -0.00615449,0.0153796,-0.999863, 18 | 0.999962,0.00619598,-0.0060598] 19 | Pcl: [0.0194384, 0.104689,-0.0251952] 20 | 21 | time_offset: 22 | imu_time_offset: 0.0 23 | img_time_offset: 0.1 24 | exposure_time_init: 0.0 25 | 26 | preprocess: 27 | point_filter_num: 1 28 | filter_size_surf: 0.1 29 | lidar_type: 1 # Livox Avia LiDAR 30 | scan_line: 6 31 | blind: 0.8 32 | 33 | vio: 34 | max_iterations: 5 35 | outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite 36 | img_point_cov: 100 # 100 1000 37 | patch_size: 8 38 | patch_pyrimid_level: 4 39 | normal_en: true 40 | raycast_en: false 41 | inverse_composition_en: false 42 | exposure_estimate_en: true 43 | inv_expo_cov: 0.1 44 | 45 | imu: 46 | imu_en: true 47 | imu_int_frame: 30 48 | acc_cov: 0.5 # 0.2 49 | gyr_cov: 0.3 # 0.5 50 | b_acc_cov: 0.0001 # 0.1 51 | b_gyr_cov: 0.0001 # 0.1 52 | 53 | lio: 54 | max_iterations: 5 55 | dept_err: 0.02 56 | beam_err: 0.05 57 | min_eigen_value: 0.0025 # 0.005 58 | voxel_size: 0.5 59 | max_layer: 2 60 | max_points_num: 50 61 | layer_init_num: [5, 5, 5, 5, 5] 62 | 63 | local_map: 64 | map_sliding_en: false 65 | half_map_size: 100 66 | sliding_thresh: 8.0 67 | 68 | uav: 69 | imu_rate_odom: false 70 | gravity_align_en: false 71 | 72 | publish: 73 | dense_map_en: true 74 | pub_effect_point_en: false 75 | pub_plane_en: false 76 | pub_scan_num: 1 77 | blind_rgb_points: 0.0 78 | 79 | evo: 80 | seq_name: "CBD_Building_01" 81 | pose_output_en: false 82 | 83 | pcd_save: 84 | pcd_save_en: false 85 | colmap_output_en: false # need to set interval = -1 86 | filter_size_pcd: 0.15 87 | interval: -1 88 | # how many LiDAR frames saved in each pcd file; 89 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 90 | -------------------------------------------------------------------------------- /config/camera_MARS_LVIG.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | cam_model: Pinhole 4 | # HKisland HKairport 5 | cam_width: 2448 6 | cam_height: 2048 7 | scale: 0.25 8 | cam_fx: 1444.431662789634 9 | cam_fy: 1444.343536688358 10 | cam_cx: 1177.801079401826 11 | cam_cy: 1043.601026568268 12 | cam_d0: -0.05729528706141188 13 | cam_d1: 0.1210407244166642 14 | cam_d2: 0.001274128378760289 15 | cam_d3: 0.0004389741530109464 16 | 17 | # AMtown AMvalley 18 | # cam_width: 2448 19 | # cam_height: 2048 20 | # scale: 0.25 21 | # cam_fx: 1453.88 22 | # cam_fy: 1452.85 23 | # cam_cx: 1182.53 24 | # cam_cy: 1045.82 25 | # cam_d0: -0.052 26 | # cam_d1: 0.1168 27 | # cam_d2: 0.0015 28 | # cam_d3: 0.00013 -------------------------------------------------------------------------------- /config/camera_NTU_VIRAL.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | cam_model: Pinhole 4 | cam_width: 752 5 | cam_height: 480 6 | scale: 1.0 7 | cam_fx: 4.250258563372763e+02 8 | cam_fy: 4.267976260903337e+02 9 | cam_cx: 3.860151866550880e+02 10 | cam_cy: 2.419130336743440e+02 11 | cam_d0: -0.288105327549552 12 | cam_d1: 0.074578284234601 13 | cam_d2: 7.784489598138802e-04 14 | cam_d3: -2.277853975035461e-04 -------------------------------------------------------------------------------- /config/camera_pinhole.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | cam_model: Pinhole 4 | cam_width: 1280 5 | cam_height: 1024 6 | scale: 0.5 7 | cam_fx: 1293.56944 8 | cam_fy: 1293.3155 9 | cam_cx: 626.91359 10 | cam_cy: 522.799224 11 | cam_d0: -0.076160 12 | cam_d1: 0.123001 13 | cam_d2: -0.00113 14 | cam_d3: 0.000251 -------------------------------------------------------------------------------- /include/IMU_Processing.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef IMU_PROCESSING_H 14 | #define IMU_PROCESSING_H 15 | 16 | #include 17 | #include 18 | #include "common_lib.h" 19 | #include 20 | #include 21 | #include 22 | 23 | const bool time_list(PointType &x, 24 | PointType &y); //{return (x.curvature < y.curvature);}; 25 | 26 | /// *************IMU Process and undistortion 27 | class ImuProcess 28 | { 29 | public: 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 31 | 32 | ImuProcess(); 33 | ~ImuProcess(); 34 | 35 | void Reset(); 36 | void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu); 37 | void set_extrinsic(const V3D &transl, const M3D &rot); 38 | void set_extrinsic(const V3D &transl); 39 | void set_extrinsic(const MD(4, 4) & T); 40 | void set_gyr_cov_scale(const V3D &scaler); 41 | void set_acc_cov_scale(const V3D &scaler); 42 | void set_gyr_bias_cov(const V3D &b_g); 43 | void set_acc_bias_cov(const V3D &b_a); 44 | void set_inv_expo_cov(const double &inv_expo); 45 | void set_imu_init_frame_num(const int &num); 46 | void disable_imu(); 47 | void disable_gravity_est(); 48 | void disable_bias_est(); 49 | void disable_exposure_est(); 50 | void Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_); 51 | void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); 52 | 53 | ofstream fout_imu; 54 | double IMU_mean_acc_norm; 55 | V3D unbiased_gyr; 56 | 57 | V3D cov_acc; 58 | V3D cov_gyr; 59 | V3D cov_bias_gyr; 60 | V3D cov_bias_acc; 61 | double cov_inv_expo; 62 | double first_lidar_time; 63 | bool imu_time_init = false; 64 | bool imu_need_init = true; 65 | M3D Eye3d; 66 | V3D Zero3d; 67 | int lidar_type; 68 | 69 | private: 70 | void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N); 71 | void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); 72 | PointCloudXYZI pcl_wait_proc; 73 | sensor_msgs::msg::Imu::ConstSharedPtr last_imu; 74 | PointCloudXYZI::Ptr cur_pcl_un_; 75 | vector IMUpose; 76 | M3D Lid_rot_to_IMU; 77 | V3D Lid_offset_to_IMU; 78 | V3D mean_acc; 79 | V3D mean_gyr; 80 | V3D angvel_last; 81 | V3D acc_s_last; 82 | double last_prop_end_time; 83 | double time_last_scan; 84 | int init_iter_num = 1, MAX_INI_COUNT = 20; 85 | bool b_first_frame = true; 86 | bool imu_en = true; 87 | bool gravity_est_en = true; 88 | bool ba_bg_est_en = true; 89 | bool exposure_estimate_en = true; 90 | }; 91 | typedef std::shared_ptr ImuProcessPtr; 92 | #endif -------------------------------------------------------------------------------- /include/LIVMapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef LIV_MAPPER_H 14 | #define LIV_MAPPER_H 15 | 16 | #include "IMU_Processing.h" 17 | #include "vio.h" 18 | #include "preprocess.h" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | class LIVMapper 27 | { 28 | public: 29 | LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name); 30 | ~LIVMapper(); 31 | void initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &nh, image_transport::ImageTransport &it_); 32 | void initializeComponents(rclcpp::Node::SharedPtr &node); 33 | void initializeFiles(); 34 | void run(rclcpp::Node::SharedPtr &node); 35 | void gravityAlignment(); 36 | void handleFirstFrame(); 37 | void stateEstimationAndMapping(); 38 | void handleVIO(); 39 | void handleLIO(); 40 | void savePCD(); 41 | void processImu(); 42 | 43 | bool sync_packages(LidarMeasureGroup &meas); 44 | void prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr); 45 | void imu_prop_callback(); 46 | void transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud); 47 | void pointBodyToWorld(const PointType &pi, PointType &po); 48 | 49 | void RGBpointBodyToWorld(PointType const *const pi, PointType *const po); 50 | void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 51 | void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in); 52 | void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in); 53 | void img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in); 54 | void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager); 55 | void publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager); 56 | void publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap); 57 | void publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list); 58 | void publish_odometry(const rclcpp::Publisher::SharedPtr &pmavros_pose_publisherubOdomAftMapped); 59 | void publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher); 60 | void publish_path(const rclcpp::Publisher::SharedPtr &pubPath); 61 | void readParameters(rclcpp::Node::SharedPtr &node); 62 | template void set_posestamp(T &out); 63 | template void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po); 64 | template Eigen::Matrix pointBodyToWorld(const Eigen::Matrix &pi); 65 | cv::Mat getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg); 66 | 67 | std::mutex mtx_buffer, mtx_buffer_imu_prop; 68 | std::condition_variable sig_buffer; 69 | 70 | SLAM_MODE slam_mode_; 71 | std::unordered_map voxel_map; 72 | 73 | string root_dir; 74 | string lid_topic, imu_topic, seq_name, img_topic; 75 | V3D extT; 76 | M3D extR; 77 | 78 | int feats_down_size = 0, max_iterations = 0; 79 | 80 | double res_mean_last = 0.05; 81 | double gyr_cov = 0, acc_cov = 0, inv_expo_cov = 0; 82 | double blind_rgb_points = 0.0; 83 | double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0, last_timestamp_img = -1.0; 84 | double filter_size_surf_min = 0; 85 | double filter_size_pcd = 0; 86 | double _first_lidar_time = 0.0; 87 | double match_time = 0, solve_time = 0, solve_const_H_time = 0; 88 | 89 | bool lidar_map_inited = false, pcd_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false; 90 | int pcd_save_interval = -1, pcd_index = 0; 91 | int pub_scan_num = 1; 92 | 93 | StatesGroup imu_propagate, latest_ekf_state; 94 | 95 | bool new_imu = false, state_update_flg = false, imu_prop_enable = true, ekf_finish_once = false; 96 | deque prop_imu_buffer; 97 | sensor_msgs::msg::Imu newest_imu; 98 | double latest_ekf_time; 99 | nav_msgs::msg::Odometry imu_prop_odom; 100 | rclcpp::Publisher::SharedPtr pubImuPropOdom; 101 | double imu_time_offset = 0.0; 102 | double lidar_time_offset = 0.0; 103 | 104 | bool gravity_align_en = false, gravity_align_finished = false; 105 | 106 | bool sync_jump_flag = false; 107 | 108 | bool lidar_pushed = false, imu_en, gravity_est_en, flg_reset = false, ba_bg_est_en = true; 109 | bool dense_map_en = false; 110 | int img_en = 1, imu_int_frame = 3; 111 | bool normal_en = true; 112 | bool exposure_estimate_en = false; 113 | double exposure_time_init = 0.0; 114 | bool inverse_composition_en = false; 115 | bool raycast_en = false; 116 | int lidar_en = 1; 117 | bool is_first_frame = false; 118 | int grid_size, patch_size, grid_n_width, grid_n_height, patch_pyrimid_level; 119 | int outlier_threshold; 120 | double plot_time; 121 | int frame_cnt; 122 | double img_time_offset = 0.0; 123 | deque lid_raw_data_buffer; 124 | deque lid_header_time_buffer; 125 | deque imu_buffer; 126 | deque img_buffer; 127 | deque img_time_buffer; 128 | vector _pv_list; 129 | vector extrinT; 130 | vector extrinR; 131 | vector cameraextrinT; 132 | vector cameraextrinR; 133 | int IMG_POINT_COV; 134 | 135 | PointCloudXYZI::Ptr visual_sub_map; 136 | PointCloudXYZI::Ptr feats_undistort; 137 | PointCloudXYZI::Ptr feats_down_body; 138 | PointCloudXYZI::Ptr feats_down_world; 139 | PointCloudXYZI::Ptr pcl_w_wait_pub; 140 | PointCloudXYZI::Ptr pcl_wait_pub; 141 | PointCloudXYZRGB::Ptr pcl_wait_save; 142 | PointCloudXYZI::Ptr pcl_wait_save_intensity; 143 | 144 | ofstream fout_pre, fout_out, fout_pcd_pos, fout_points; 145 | 146 | pcl::VoxelGrid downSizeFilterSurf; 147 | 148 | V3D euler_cur; 149 | 150 | LidarMeasureGroup LidarMeasures; 151 | StatesGroup _state; 152 | StatesGroup state_propagat; 153 | 154 | nav_msgs::msg::Path path; 155 | nav_msgs::msg::Odometry odomAftMapped; 156 | geometry_msgs::msg::Quaternion geoQuat; 157 | geometry_msgs::msg::PoseStamped msg_body_pose; 158 | 159 | PreprocessPtr p_pre; 160 | ImuProcessPtr p_imu; 161 | VoxelMapManagerPtr voxelmap_manager; 162 | VIOManagerPtr vio_manager; 163 | 164 | rclcpp::Publisher::SharedPtr plane_pub; 165 | rclcpp::Publisher::SharedPtr voxel_pub; 166 | std::shared_ptr sub_pcl; 167 | rclcpp::Subscription::SharedPtr sub_imu; 168 | rclcpp::Subscription::SharedPtr sub_img; 169 | rclcpp::Publisher::SharedPtr pubLaserCloudFullRes; 170 | rclcpp::Publisher::SharedPtr pubNormal; 171 | rclcpp::Publisher::SharedPtr pubSubVisualMap; 172 | rclcpp::Publisher::SharedPtr pubLaserCloudEffect; 173 | rclcpp::Publisher::SharedPtr pubLaserCloudMap; 174 | rclcpp::Publisher::SharedPtr pubOdomAftMapped; 175 | rclcpp::Publisher::SharedPtr pubPath; 176 | rclcpp::Publisher::SharedPtr pubLaserCloudDyn; 177 | rclcpp::Publisher::SharedPtr pubLaserCloudDynRmed; 178 | rclcpp::Publisher::SharedPtr pubLaserCloudDynDbg; 179 | image_transport::Publisher pubImage; 180 | rclcpp::Publisher::SharedPtr mavros_pose_publisher; 181 | rclcpp::TimerBase::SharedPtr imu_prop_timer; 182 | rclcpp::Node::SharedPtr node; 183 | 184 | int frame_num = 0; 185 | double aver_time_consu = 0; 186 | double aver_time_icp = 0; 187 | double aver_time_map_inre = 0; 188 | bool colmap_output_en = false; 189 | }; 190 | #endif -------------------------------------------------------------------------------- /include/common_lib.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef COMMON_LIB_H 14 | #define COMMON_LIB_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | // using namespace Eigen; // avoid cmake error: reference to ‘Matrix’ is ambiguous 29 | using namespace Sophus; 30 | 31 | #define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl; 32 | #define G_m_s2 (9.81) // Gravaty const in GuangDong/China 33 | #define DIM_STATE (19) // Dimension of states (Let Dim(SO(3)) = 3) 34 | #define INIT_COV (0.01) 35 | #define SIZE_LARGE (500) 36 | #define SIZE_SMALL (100) 37 | #define VEC_FROM_ARRAY(v) v[0], v[1], v[2] 38 | #define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8] 39 | #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name)) 40 | 41 | enum LID_TYPE 42 | { 43 | AVIA = 1, 44 | VELO16 = 2, 45 | OUST64 = 3, 46 | L515 = 4, 47 | XT32 = 5, 48 | PANDAR128 = 6 49 | }; 50 | enum SLAM_MODE 51 | { 52 | ONLY_LO = 0, 53 | ONLY_LIO = 1, 54 | LIVO = 2 55 | }; 56 | enum EKF_STATE 57 | { 58 | WAIT = 0, 59 | VIO = 1, 60 | LIO = 2, 61 | LO = 3 62 | }; 63 | 64 | struct MeasureGroup 65 | { 66 | double vio_time; 67 | double lio_time; 68 | deque imu; 69 | cv::Mat img; 70 | MeasureGroup() 71 | { 72 | vio_time = 0.0; 73 | lio_time = 0.0; 74 | }; 75 | }; 76 | 77 | struct LidarMeasureGroup 78 | { 79 | double lidar_frame_beg_time; 80 | double lidar_frame_end_time; 81 | double last_lio_update_time; 82 | PointCloudXYZI::Ptr lidar; 83 | PointCloudXYZI::Ptr pcl_proc_cur; 84 | PointCloudXYZI::Ptr pcl_proc_next; 85 | deque measures; 86 | EKF_STATE lio_vio_flg; 87 | int lidar_scan_index_now; 88 | 89 | LidarMeasureGroup() 90 | { 91 | lidar_frame_beg_time = -0.0; 92 | lidar_frame_end_time = 0.0; 93 | last_lio_update_time = -1.0; 94 | lio_vio_flg = WAIT; 95 | this->lidar.reset(new PointCloudXYZI()); 96 | this->pcl_proc_cur.reset(new PointCloudXYZI()); 97 | this->pcl_proc_next.reset(new PointCloudXYZI()); 98 | this->measures.clear(); 99 | lidar_scan_index_now = 0; 100 | last_lio_update_time = -1.0; 101 | }; 102 | }; 103 | 104 | typedef struct pointWithVar 105 | { 106 | Eigen::Vector3d point_b; // point in the lidar body frame 107 | Eigen::Vector3d point_i; // point in the imu body frame 108 | Eigen::Vector3d point_w; // point in the world frame 109 | Eigen::Matrix3d var_nostate; // the var removed the state covarience 110 | Eigen::Matrix3d body_var; 111 | Eigen::Matrix3d var; 112 | Eigen::Matrix3d point_crossmat; 113 | Eigen::Vector3d normal; 114 | pointWithVar() 115 | { 116 | var_nostate = Eigen::Matrix3d::Zero(); 117 | var = Eigen::Matrix3d::Zero(); 118 | body_var = Eigen::Matrix3d::Zero(); 119 | point_crossmat = Eigen::Matrix3d::Zero(); 120 | point_b = Eigen::Vector3d::Zero(); 121 | point_i = Eigen::Vector3d::Zero(); 122 | point_w = Eigen::Vector3d::Zero(); 123 | normal = Eigen::Vector3d::Zero(); 124 | }; 125 | } pointWithVar; 126 | 127 | 128 | struct StatesGroup 129 | { 130 | StatesGroup() 131 | { 132 | this->rot_end = M3D::Identity(); 133 | this->pos_end = V3D::Zero(); 134 | this->vel_end = V3D::Zero(); 135 | this->bias_g = V3D::Zero(); 136 | this->bias_a = V3D::Zero(); 137 | this->gravity = V3D::Zero(); 138 | this->inv_expo_time = 1.0; 139 | this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV; 140 | this->cov(6, 6) = 0.00001; 141 | this->cov.block<9, 9>(10, 10) = MD(9, 9)::Identity() * 0.00001; 142 | }; 143 | 144 | StatesGroup(const StatesGroup &b) 145 | { 146 | this->rot_end = b.rot_end; 147 | this->pos_end = b.pos_end; 148 | this->vel_end = b.vel_end; 149 | this->bias_g = b.bias_g; 150 | this->bias_a = b.bias_a; 151 | this->gravity = b.gravity; 152 | this->inv_expo_time = b.inv_expo_time; 153 | this->cov = b.cov; 154 | }; 155 | 156 | StatesGroup &operator=(const StatesGroup &b) 157 | { 158 | this->rot_end = b.rot_end; 159 | this->pos_end = b.pos_end; 160 | this->vel_end = b.vel_end; 161 | this->bias_g = b.bias_g; 162 | this->bias_a = b.bias_a; 163 | this->gravity = b.gravity; 164 | this->inv_expo_time = b.inv_expo_time; 165 | this->cov = b.cov; 166 | return *this; 167 | }; 168 | 169 | StatesGroup operator+(const Matrix &state_add) 170 | { 171 | StatesGroup a; 172 | a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); 173 | a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0); 174 | a.inv_expo_time = this->inv_expo_time + state_add(6, 0); 175 | a.vel_end = this->vel_end + state_add.block<3, 1>(7, 0); 176 | a.bias_g = this->bias_g + state_add.block<3, 1>(10, 0); 177 | a.bias_a = this->bias_a + state_add.block<3, 1>(13, 0); 178 | a.gravity = this->gravity + state_add.block<3, 1>(16, 0); 179 | 180 | a.cov = this->cov; 181 | return a; 182 | }; 183 | 184 | StatesGroup &operator+=(const Matrix &state_add) 185 | { 186 | this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); 187 | this->pos_end += state_add.block<3, 1>(3, 0); 188 | this->inv_expo_time += state_add(6, 0); 189 | this->vel_end += state_add.block<3, 1>(7, 0); 190 | this->bias_g += state_add.block<3, 1>(10, 0); 191 | this->bias_a += state_add.block<3, 1>(13, 0); 192 | this->gravity += state_add.block<3, 1>(16, 0); 193 | return *this; 194 | }; 195 | 196 | Matrix operator-(const StatesGroup &b) 197 | { 198 | Matrix a; 199 | M3D rotd(b.rot_end.transpose() * this->rot_end); 200 | a.block<3, 1>(0, 0) = Log(rotd); 201 | a.block<3, 1>(3, 0) = this->pos_end - b.pos_end; 202 | a(6, 0) = this->inv_expo_time - b.inv_expo_time; 203 | a.block<3, 1>(7, 0) = this->vel_end - b.vel_end; 204 | a.block<3, 1>(10, 0) = this->bias_g - b.bias_g; 205 | a.block<3, 1>(13, 0) = this->bias_a - b.bias_a; 206 | a.block<3, 1>(16, 0) = this->gravity - b.gravity; 207 | return a; 208 | }; 209 | 210 | void resetpose() 211 | { 212 | this->rot_end = M3D::Identity(); 213 | this->pos_end = V3D::Zero(); 214 | this->vel_end = V3D::Zero(); 215 | } 216 | 217 | M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point 218 | V3D pos_end; // the estimated position at the end lidar point (world frame) 219 | V3D vel_end; // the estimated velocity at the end lidar point (world frame) 220 | double inv_expo_time; // the estimated inverse exposure time (no scale) 221 | V3D bias_g; // gyroscope bias 222 | V3D bias_a; // accelerator bias 223 | V3D gravity; // the estimated gravity acceleration 224 | Matrix cov; // states covariance 225 | }; 226 | 227 | template 228 | auto set_pose6d(const double t, const Matrix &a, const Matrix &g, const Matrix &v, const Matrix &p, 229 | const Matrix &R) 230 | { 231 | Pose6D rot_kp; 232 | rot_kp.offset_time = t; 233 | for (int i = 0; i < 3; i++) 234 | { 235 | rot_kp.acc[i] = a(i); 236 | rot_kp.gyr[i] = g(i); 237 | rot_kp.vel[i] = v(i); 238 | rot_kp.pos[i] = p(i); 239 | for (int j = 0; j < 3; j++) 240 | rot_kp.rot[i * 3 + j] = R(i, j); 241 | } 242 | // Map(rot_kp.rot, 3,3) = R; 243 | return move(rot_kp); 244 | } 245 | 246 | #endif -------------------------------------------------------------------------------- /include/feature.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef LIVO_FEATURE_H_ 14 | #define LIVO_FEATURE_H_ 15 | 16 | #include "visual_point.h" 17 | 18 | // A salient image region that is tracked across frames. 19 | struct Feature 20 | { 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | enum FeatureType 24 | { 25 | CORNER, 26 | EDGELET 27 | }; 28 | int id_; 29 | FeatureType type_; //!< Type can be corner or edgelet. 30 | cv::Mat img_; //!< Image associated with the patch feature 31 | Vector2d px_; //!< Coordinates in pixels on pyramid level 0. 32 | Vector3d f_; //!< Unit-bearing vector of the patch feature. 33 | int level_; //!< Image pyramid level where patch feature was extracted. 34 | VisualPoint *point_; //!< Pointer to 3D point which corresponds to the patch feature. 35 | Vector2d grad_; //!< Dominant gradient direction for edglets, normalized. 36 | SE3 T_f_w_; //!< Pose of the frame where the patch feature was extracted. 37 | float *patch_; //!< Pointer to the image patch data. 38 | float score_; //!< Score of the patch feature. 39 | float mean_; //!< Mean intensity of the image patch feature, used for normalization. 40 | double inv_expo_time_; //!< Inverse exposure time of the image where the patch feature was extracted. 41 | 42 | Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3 &_T_f_w, int _level) 43 | : type_(CORNER), px_(_px), f_(_f), T_f_w_(_T_f_w), mean_(0), score_(0), level_(_level), patch_(_patch), point_(_point) 44 | { 45 | } 46 | 47 | inline Vector3d pos() const { return T_f_w_.inverse().translation(); } 48 | 49 | ~Feature() 50 | { 51 | // ROS_WARN("The feature %d has been destructed.", id_); 52 | delete[] patch_; 53 | } 54 | }; 55 | 56 | #endif // LIVO_FEATURE_H_ 57 | -------------------------------------------------------------------------------- /include/frame.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef LIVO_FRAME_H_ 14 | #define LIVO_FRAME_H_ 15 | 16 | #include 17 | #include 18 | 19 | class VisualPoint; 20 | struct Feature; 21 | 22 | typedef list Features; 23 | typedef vector ImgPyr; 24 | 25 | /// A frame saves the image, the associated features and the estimated pose. 26 | class Frame : boost::noncopyable 27 | { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | 31 | static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. 32 | int id_; //!< Unique id of the frame. 33 | vk::AbstractCamera *cam_; //!< Camera model. 34 | SE3 T_f_w_; //!< Transform (f)rame from (w)orld. 35 | SE3 T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior. 36 | cv::Mat img_; //!< Image of the frame. 37 | Features fts_; //!< List of features in the image. 38 | 39 | Frame(vk::AbstractCamera *cam, const cv::Mat &img); 40 | ~Frame(); 41 | 42 | /// Initialize new frame and create image pyramid. 43 | void initFrame(const cv::Mat &img); 44 | 45 | /// Return number of point observations. 46 | inline size_t nObs() const { return fts_.size(); } 47 | 48 | /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). 49 | inline Vector2d w2c(const Vector3d &xyz_w) const { return cam_->world2cam(T_f_w_ * xyz_w); } 50 | 51 | /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c) using the IMU prior pose. 52 | inline Vector2d w2c_prior(const Vector3d &xyz_w) const { return cam_->world2cam(T_f_w_prior_ * xyz_w); } 53 | 54 | /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). 55 | inline Vector3d c2f(const Vector2d &px) const { return cam_->cam2world(px[0], px[1]); } 56 | 57 | /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). 58 | inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); } 59 | 60 | /// Transforms point coordinates in world-frame (w) to camera-frams (f). 61 | inline Vector3d w2f(const Vector3d &xyz_w) const { return T_f_w_ * xyz_w; } 62 | 63 | /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). 64 | inline Vector3d f2w(const Vector3d &f) const { return T_f_w_.inverse() * f; } 65 | 66 | /// Projects Point from unit sphere (f) in camera pixels (c). 67 | inline Vector2d f2c(const Vector3d &f) const { return cam_->world2cam(f); } 68 | 69 | /// Return the pose of the frame in the (w)orld coordinate frame. 70 | inline Vector3d pos() const { return T_f_w_.inverse().translation(); } 71 | }; 72 | 73 | typedef std::unique_ptr FramePtr; 74 | 75 | /// Some helper functions for the frame object. 76 | namespace frame_utils 77 | { 78 | 79 | /// Creates an image pyramid of half-sampled images. 80 | void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr); 81 | 82 | } // namespace frame_utils 83 | 84 | #endif // LIVO_FRAME_H_ 85 | -------------------------------------------------------------------------------- /include/livox_ros_driver/CustomMsg.h: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file livox_ros_driver/CustomMsg.msg 2 | // DO NOT EDIT! 3 | 4 | 5 | #ifndef LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H 6 | #define LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace livox_ros_driver 22 | { 23 | template 24 | struct CustomMsg_ 25 | { 26 | typedef CustomMsg_ Type; 27 | 28 | CustomMsg_() 29 | : header() 30 | , timebase(0) 31 | , point_num(0) 32 | , lidar_id(0) 33 | , rsvd() 34 | , points() { 35 | rsvd.assign(0); 36 | } 37 | CustomMsg_(const ContainerAllocator& _alloc) 38 | : header(_alloc) 39 | , timebase(0) 40 | , point_num(0) 41 | , lidar_id(0) 42 | , rsvd() 43 | , points(_alloc) { 44 | (void)_alloc; 45 | rsvd.assign(0); 46 | } 47 | 48 | 49 | 50 | typedef ::std_msgs::Header_ _header_type; 51 | _header_type header; 52 | 53 | typedef uint64_t _timebase_type; 54 | _timebase_type timebase; 55 | 56 | typedef uint32_t _point_num_type; 57 | _point_num_type point_num; 58 | 59 | typedef uint8_t _lidar_id_type; 60 | _lidar_id_type lidar_id; 61 | 62 | typedef boost::array _rsvd_type; 63 | _rsvd_type rsvd; 64 | 65 | typedef std::vector< ::livox_ros_driver::CustomPoint_ , typename std::allocator_traits::template rebind_alloc< ::livox_ros_driver::CustomPoint_ >> _points_type; 66 | _points_type points; 67 | 68 | 69 | 70 | 71 | 72 | typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_ > Ptr; 73 | typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_ const> ConstPtr; 74 | 75 | }; // struct CustomMsg_ 76 | 77 | typedef ::livox_ros_driver::CustomMsg_ > CustomMsg; 78 | 79 | typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg > CustomMsgPtr; 80 | typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg const> CustomMsgConstPtr; 81 | 82 | // constants requiring out of line definition 83 | 84 | 85 | 86 | template 87 | std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomMsg_ & v) 88 | { 89 | ros::message_operations::Printer< ::livox_ros_driver::CustomMsg_ >::stream(s, "", v); 90 | return s; 91 | } 92 | 93 | 94 | template 95 | bool operator==(const ::livox_ros_driver::CustomMsg_ & lhs, const ::livox_ros_driver::CustomMsg_ & rhs) 96 | { 97 | return lhs.header == rhs.header && 98 | lhs.timebase == rhs.timebase && 99 | lhs.point_num == rhs.point_num && 100 | lhs.lidar_id == rhs.lidar_id && 101 | lhs.rsvd == rhs.rsvd && 102 | lhs.points == rhs.points; 103 | } 104 | 105 | template 106 | bool operator!=(const ::livox_ros_driver::CustomMsg_ & lhs, const ::livox_ros_driver::CustomMsg_ & rhs) 107 | { 108 | return !(lhs == rhs); 109 | } 110 | 111 | 112 | } // namespace livox_ros_driver 113 | 114 | namespace ros 115 | { 116 | namespace message_traits 117 | { 118 | 119 | 120 | 121 | 122 | 123 | template 124 | struct IsMessage< ::livox_ros_driver::CustomMsg_ > 125 | : TrueType 126 | { }; 127 | 128 | template 129 | struct IsMessage< ::livox_ros_driver::CustomMsg_ const> 130 | : TrueType 131 | { }; 132 | 133 | template 134 | struct IsFixedSize< ::livox_ros_driver::CustomMsg_ > 135 | : FalseType 136 | { }; 137 | 138 | template 139 | struct IsFixedSize< ::livox_ros_driver::CustomMsg_ const> 140 | : FalseType 141 | { }; 142 | 143 | template 144 | struct HasHeader< ::livox_ros_driver::CustomMsg_ > 145 | : TrueType 146 | { }; 147 | 148 | template 149 | struct HasHeader< ::livox_ros_driver::CustomMsg_ const> 150 | : TrueType 151 | { }; 152 | 153 | 154 | template 155 | struct MD5Sum< ::livox_ros_driver::CustomMsg_ > 156 | { 157 | static const char* value() 158 | { 159 | return "e4d6829bdfe657cb6c21a746c86b21a6"; 160 | } 161 | 162 | static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } 163 | static const uint64_t static_value1 = 0xe4d6829bdfe657cbULL; 164 | static const uint64_t static_value2 = 0x6c21a746c86b21a6ULL; 165 | }; 166 | 167 | template 168 | struct DataType< ::livox_ros_driver::CustomMsg_ > 169 | { 170 | static const char* value() 171 | { 172 | return "livox_ros_driver/CustomMsg"; 173 | } 174 | 175 | static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } 176 | }; 177 | 178 | template 179 | struct Definition< ::livox_ros_driver::CustomMsg_ > 180 | { 181 | static const char* value() 182 | { 183 | return "# Livox publish pointcloud msg format.\n" 184 | "\n" 185 | "Header header # ROS standard message header\n" 186 | "uint64 timebase # The time of first point\n" 187 | "uint32 point_num # Total number of pointclouds\n" 188 | "uint8 lidar_id # Lidar device id number\n" 189 | "uint8[3] rsvd # Reserved use\n" 190 | "CustomPoint[] points # Pointcloud data\n" 191 | "\n" 192 | "\n" 193 | "================================================================================\n" 194 | "MSG: std_msgs/Header\n" 195 | "# Standard metadata for higher-level stamped data types.\n" 196 | "# This is generally used to communicate timestamped data \n" 197 | "# in a particular coordinate frame.\n" 198 | "# \n" 199 | "# sequence ID: consecutively increasing ID \n" 200 | "uint32 seq\n" 201 | "#Two-integer timestamp that is expressed as:\n" 202 | "# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" 203 | "# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" 204 | "# time-handling sugar is provided by the client library\n" 205 | "time stamp\n" 206 | "#Frame this data is associated with\n" 207 | "string frame_id\n" 208 | "\n" 209 | "================================================================================\n" 210 | "MSG: livox_ros_driver/CustomPoint\n" 211 | "# Livox costom pointcloud format.\n" 212 | "\n" 213 | "uint32 offset_time # offset time relative to the base time\n" 214 | "float32 x # X axis, unit:m\n" 215 | "float32 y # Y axis, unit:m\n" 216 | "float32 z # Z axis, unit:m\n" 217 | "uint8 reflectivity # reflectivity, 0~255\n" 218 | "uint8 tag # livox tag\n" 219 | "uint8 line # laser number in lidar\n" 220 | "\n" 221 | ; 222 | } 223 | 224 | static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } 225 | }; 226 | 227 | } // namespace message_traits 228 | } // namespace ros 229 | 230 | namespace ros 231 | { 232 | namespace serialization 233 | { 234 | 235 | template struct Serializer< ::livox_ros_driver::CustomMsg_ > 236 | { 237 | template inline static void allInOne(Stream& stream, T m) 238 | { 239 | stream.next(m.header); 240 | stream.next(m.timebase); 241 | stream.next(m.point_num); 242 | stream.next(m.lidar_id); 243 | stream.next(m.rsvd); 244 | stream.next(m.points); 245 | } 246 | 247 | ROS_DECLARE_ALLINONE_SERIALIZER 248 | }; // struct CustomMsg_ 249 | 250 | } // namespace serialization 251 | } // namespace ros 252 | 253 | namespace ros 254 | { 255 | namespace message_operations 256 | { 257 | 258 | template 259 | struct Printer< ::livox_ros_driver::CustomMsg_ > 260 | { 261 | template static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomMsg_& v) 262 | { 263 | s << indent << "header: "; 264 | s << std::endl; 265 | Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); 266 | s << indent << "timebase: "; 267 | Printer::stream(s, indent + " ", v.timebase); 268 | s << indent << "point_num: "; 269 | Printer::stream(s, indent + " ", v.point_num); 270 | s << indent << "lidar_id: "; 271 | Printer::stream(s, indent + " ", v.lidar_id); 272 | s << indent << "rsvd[]" << std::endl; 273 | for (size_t i = 0; i < v.rsvd.size(); ++i) 274 | { 275 | s << indent << " rsvd[" << i << "]: "; 276 | Printer::stream(s, indent + " ", v.rsvd[i]); 277 | } 278 | s << indent << "points[]" << std::endl; 279 | for (size_t i = 0; i < v.points.size(); ++i) 280 | { 281 | s << indent << " points[" << i << "]: "; 282 | s << std::endl; 283 | s << indent; 284 | Printer< ::livox_ros_driver::CustomPoint_ >::stream(s, indent + " ", v.points[i]); 285 | } 286 | } 287 | }; 288 | 289 | } // namespace message_operations 290 | } // namespace ros 291 | 292 | #endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H 293 | -------------------------------------------------------------------------------- /include/livox_ros_driver/CustomPoint.h: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file livox_ros_driver/CustomPoint.msg 2 | // DO NOT EDIT! 3 | 4 | 5 | #ifndef LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H 6 | #define LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | namespace livox_ros_driver 20 | { 21 | template 22 | struct CustomPoint_ 23 | { 24 | typedef CustomPoint_ Type; 25 | 26 | CustomPoint_() 27 | : offset_time(0) 28 | , x(0.0) 29 | , y(0.0) 30 | , z(0.0) 31 | , reflectivity(0) 32 | , tag(0) 33 | , line(0) { 34 | } 35 | CustomPoint_(const ContainerAllocator& _alloc) 36 | : offset_time(0) 37 | , x(0.0) 38 | , y(0.0) 39 | , z(0.0) 40 | , reflectivity(0) 41 | , tag(0) 42 | , line(0) { 43 | (void)_alloc; 44 | } 45 | 46 | 47 | 48 | typedef uint32_t _offset_time_type; 49 | _offset_time_type offset_time; 50 | 51 | typedef float _x_type; 52 | _x_type x; 53 | 54 | typedef float _y_type; 55 | _y_type y; 56 | 57 | typedef float _z_type; 58 | _z_type z; 59 | 60 | typedef uint8_t _reflectivity_type; 61 | _reflectivity_type reflectivity; 62 | 63 | typedef uint8_t _tag_type; 64 | _tag_type tag; 65 | 66 | typedef uint8_t _line_type; 67 | _line_type line; 68 | 69 | 70 | 71 | 72 | 73 | typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_ > Ptr; 74 | typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_ const> ConstPtr; 75 | 76 | }; // struct CustomPoint_ 77 | 78 | typedef ::livox_ros_driver::CustomPoint_ > CustomPoint; 79 | 80 | typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint > CustomPointPtr; 81 | typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint const> CustomPointConstPtr; 82 | 83 | // constants requiring out of line definition 84 | 85 | 86 | 87 | template 88 | std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomPoint_ & v) 89 | { 90 | ros::message_operations::Printer< ::livox_ros_driver::CustomPoint_ >::stream(s, "", v); 91 | return s; 92 | } 93 | 94 | 95 | template 96 | bool operator==(const ::livox_ros_driver::CustomPoint_ & lhs, const ::livox_ros_driver::CustomPoint_ & rhs) 97 | { 98 | return lhs.offset_time == rhs.offset_time && 99 | lhs.x == rhs.x && 100 | lhs.y == rhs.y && 101 | lhs.z == rhs.z && 102 | lhs.reflectivity == rhs.reflectivity && 103 | lhs.tag == rhs.tag && 104 | lhs.line == rhs.line; 105 | } 106 | 107 | template 108 | bool operator!=(const ::livox_ros_driver::CustomPoint_ & lhs, const ::livox_ros_driver::CustomPoint_ & rhs) 109 | { 110 | return !(lhs == rhs); 111 | } 112 | 113 | 114 | } // namespace livox_ros_driver 115 | 116 | namespace ros 117 | { 118 | namespace message_traits 119 | { 120 | 121 | 122 | 123 | 124 | 125 | template 126 | struct IsMessage< ::livox_ros_driver::CustomPoint_ > 127 | : TrueType 128 | { }; 129 | 130 | template 131 | struct IsMessage< ::livox_ros_driver::CustomPoint_ const> 132 | : TrueType 133 | { }; 134 | 135 | template 136 | struct IsFixedSize< ::livox_ros_driver::CustomPoint_ > 137 | : TrueType 138 | { }; 139 | 140 | template 141 | struct IsFixedSize< ::livox_ros_driver::CustomPoint_ const> 142 | : TrueType 143 | { }; 144 | 145 | template 146 | struct HasHeader< ::livox_ros_driver::CustomPoint_ > 147 | : FalseType 148 | { }; 149 | 150 | template 151 | struct HasHeader< ::livox_ros_driver::CustomPoint_ const> 152 | : FalseType 153 | { }; 154 | 155 | 156 | template 157 | struct MD5Sum< ::livox_ros_driver::CustomPoint_ > 158 | { 159 | static const char* value() 160 | { 161 | return "109a3cc548bb1f96626be89a5008bd6d"; 162 | } 163 | 164 | static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } 165 | static const uint64_t static_value1 = 0x109a3cc548bb1f96ULL; 166 | static const uint64_t static_value2 = 0x626be89a5008bd6dULL; 167 | }; 168 | 169 | template 170 | struct DataType< ::livox_ros_driver::CustomPoint_ > 171 | { 172 | static const char* value() 173 | { 174 | return "livox_ros_driver/CustomPoint"; 175 | } 176 | 177 | static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } 178 | }; 179 | 180 | template 181 | struct Definition< ::livox_ros_driver::CustomPoint_ > 182 | { 183 | static const char* value() 184 | { 185 | return "# Livox costom pointcloud format.\n" 186 | "\n" 187 | "uint32 offset_time # offset time relative to the base time\n" 188 | "float32 x # X axis, unit:m\n" 189 | "float32 y # Y axis, unit:m\n" 190 | "float32 z # Z axis, unit:m\n" 191 | "uint8 reflectivity # reflectivity, 0~255\n" 192 | "uint8 tag # livox tag\n" 193 | "uint8 line # laser number in lidar\n" 194 | "\n" 195 | ; 196 | } 197 | 198 | static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } 199 | }; 200 | 201 | } // namespace message_traits 202 | } // namespace ros 203 | 204 | namespace ros 205 | { 206 | namespace serialization 207 | { 208 | 209 | template struct Serializer< ::livox_ros_driver::CustomPoint_ > 210 | { 211 | template inline static void allInOne(Stream& stream, T m) 212 | { 213 | stream.next(m.offset_time); 214 | stream.next(m.x); 215 | stream.next(m.y); 216 | stream.next(m.z); 217 | stream.next(m.reflectivity); 218 | stream.next(m.tag); 219 | stream.next(m.line); 220 | } 221 | 222 | ROS_DECLARE_ALLINONE_SERIALIZER 223 | }; // struct CustomPoint_ 224 | 225 | } // namespace serialization 226 | } // namespace ros 227 | 228 | namespace ros 229 | { 230 | namespace message_operations 231 | { 232 | 233 | template 234 | struct Printer< ::livox_ros_driver::CustomPoint_ > 235 | { 236 | template static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomPoint_& v) 237 | { 238 | s << indent << "offset_time: "; 239 | Printer::stream(s, indent + " ", v.offset_time); 240 | s << indent << "x: "; 241 | Printer::stream(s, indent + " ", v.x); 242 | s << indent << "y: "; 243 | Printer::stream(s, indent + " ", v.y); 244 | s << indent << "z: "; 245 | Printer::stream(s, indent + " ", v.z); 246 | s << indent << "reflectivity: "; 247 | Printer::stream(s, indent + " ", v.reflectivity); 248 | s << indent << "tag: "; 249 | Printer::stream(s, indent + " ", v.tag); 250 | s << indent << "line: "; 251 | Printer::stream(s, indent + " ", v.line); 252 | } 253 | }; 254 | 255 | } // namespace message_operations 256 | } // namespace ros 257 | 258 | #endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H 259 | -------------------------------------------------------------------------------- /include/preprocess.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef PREPROCESS_H_ 14 | #define PREPROCESS_H_ 15 | 16 | #include "common_lib.h" 17 | #include 18 | #include 19 | 20 | using namespace std; 21 | 22 | #define IS_VALID(a) ((abs(a) > 1e8) ? true : false) 23 | 24 | enum LiDARFeature 25 | { 26 | Nor, 27 | Poss_Plane, 28 | Real_Plane, 29 | Edge_Jump, 30 | Edge_Plane, 31 | Wire, 32 | ZeroPoint 33 | }; 34 | enum Surround 35 | { 36 | Prev, 37 | Next 38 | }; 39 | enum E_jump 40 | { 41 | Nr_nor, 42 | Nr_zero, 43 | Nr_180, 44 | Nr_inf, 45 | Nr_blind 46 | }; 47 | 48 | struct orgtype 49 | { 50 | double range; 51 | double dista; 52 | double angle[2]; 53 | double intersect; 54 | E_jump edj[2]; 55 | LiDARFeature ftype; 56 | orgtype() 57 | { 58 | range = 0; 59 | edj[Prev] = Nr_nor; 60 | edj[Next] = Nr_nor; 61 | ftype = Nor; 62 | intersect = 2; 63 | } 64 | }; 65 | 66 | /*** Velodyne ***/ 67 | namespace velodyne_ros 68 | { 69 | struct EIGEN_ALIGN16 Point 70 | { 71 | PCL_ADD_POINT4D; 72 | float intensity; 73 | std::uint32_t t; 74 | std::uint16_t ring; 75 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 76 | }; 77 | } // namespace velodyne_ros 78 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 79 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, t, t)(std::uint16_t, ring, ring)) 80 | /****************/ 81 | 82 | /*** Ouster ***/ 83 | namespace ouster_ros 84 | { 85 | struct EIGEN_ALIGN16 Point 86 | { 87 | PCL_ADD_POINT4D; 88 | float intensity; 89 | std::uint32_t t; 90 | std::uint16_t reflectivity; 91 | uint8_t ring; 92 | std::uint16_t ambient; 93 | std::uint32_t range; 94 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 95 | }; 96 | } // namespace ouster_ros 97 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) 98 | (std::uint32_t, t, t)(std::uint16_t, reflectivity, 99 | reflectivity)(std::uint8_t, ring, ring)(std::uint16_t, ambient, ambient)(std::uint32_t, range, range)) 100 | /****************/ 101 | 102 | /*** Hesai_XT32 ***/ 103 | namespace xt32_ros 104 | { 105 | struct EIGEN_ALIGN16 Point 106 | { 107 | PCL_ADD_POINT4D; 108 | float intensity; 109 | double timestamp; 110 | std::uint16_t ring; 111 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 112 | }; 113 | } // namespace xt32_ros 114 | POINT_CLOUD_REGISTER_POINT_STRUCT(xt32_ros::Point, 115 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) 116 | /*****************/ 117 | 118 | /*** Hesai_Pandar128 ***/ 119 | namespace Pandar128_ros 120 | { 121 | struct EIGEN_ALIGN16 Point 122 | { 123 | PCL_ADD_POINT4D; 124 | float timestamp; 125 | uint8_t ring; 126 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 127 | }; 128 | } // namespace Pandar128_ros 129 | POINT_CLOUD_REGISTER_POINT_STRUCT(Pandar128_ros::Point, 130 | (float, x, x)(float, y, y)(float, z, z)(float, timestamp, timestamp)) 131 | /*****************/ 132 | 133 | class Preprocess 134 | { 135 | public: 136 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 137 | 138 | Preprocess(); 139 | ~Preprocess(); 140 | 141 | void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); 142 | void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); 143 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 144 | 145 | // sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud; 146 | PointCloudXYZI pl_full, pl_corn, pl_surf; 147 | PointCloudXYZI pl_buff[128]; // maximum 128 line lidar 148 | vector typess[128]; // maximum 128 line lidar 149 | int lidar_type, point_filter_num, N_SCANS; 150 | 151 | double blind, blind_sqr; 152 | bool feature_enabled, given_offset_time; 153 | std::shared_ptr> pub_full; 154 | std::shared_ptr> pub_surf; 155 | std::shared_ptr> pub_corn; 156 | 157 | private: 158 | void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg); 159 | void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 160 | void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 161 | void xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 162 | void Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 163 | void l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 164 | void give_feature(PointCloudXYZI &pl, vector &types); 165 | void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct); 166 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); 167 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); 168 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); 169 | 170 | int group_size; 171 | double disA, disB, inf_bound; 172 | double limit_maxmid, limit_midmin, limit_maxmin; 173 | double p2l_ratio; 174 | double jump_up_limit, jump_down_limit; 175 | double cos160; 176 | double edgea, edgeb; 177 | double smallp_intersect, smallp_ratio; 178 | double vx, vy, vz; 179 | }; 180 | typedef std::shared_ptr PreprocessPtr; 181 | 182 | #endif // PREPROCESS_H_ -------------------------------------------------------------------------------- /include/utils/color.h: -------------------------------------------------------------------------------- 1 | #ifndef COLOR_H 2 | #define COLOR_H 3 | 4 | #define RESET "\033[0m" 5 | #define BLACK "\033[30m" /* Black */ 6 | #define RED "\033[31m" /* Red */ 7 | #define GREEN "\033[32m" /* Green */ 8 | #define YELLOW "\033[33m" /* Yellow */ 9 | #define BLUE "\033[34m" /* Blue */ 10 | #define MAGENTA "\033[35m" /* Magenta */ 11 | #define CYAN "\033[36m" /* Cyan */ 12 | #define WHITE "\033[37m" /* White */ 13 | #define REDPURPLE "\033[95m" /* Red Purple */ 14 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 15 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 16 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 17 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 18 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 19 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 20 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 21 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 22 | #define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */ 23 | 24 | #endif // COLOR_H 25 | -------------------------------------------------------------------------------- /include/utils/so3_math.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_MATH_H 2 | #define SO3_MATH_H 3 | 4 | #include 5 | #include 6 | 7 | #define SKEW_SYM_MATRX(v) 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0 8 | 9 | template Eigen::Matrix Exp(const Eigen::Matrix &&ang) 10 | { 11 | T ang_norm = ang.norm(); 12 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 13 | if (ang_norm > 0.0000001) 14 | { 15 | Eigen::Matrix r_axis = ang / ang_norm; 16 | Eigen::Matrix K; 17 | K << SKEW_SYM_MATRX(r_axis); 18 | /// Roderigous Tranformation 19 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 20 | } 21 | else { return Eye3; } 22 | } 23 | 24 | template Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 25 | { 26 | T ang_vel_norm = ang_vel.norm(); 27 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 28 | 29 | if (ang_vel_norm > 0.0000001) 30 | { 31 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 32 | Eigen::Matrix K; 33 | 34 | K << SKEW_SYM_MATRX(r_axis); 35 | 36 | T r_ang = ang_vel_norm * dt; 37 | 38 | /// Roderigous Tranformation 39 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 40 | } 41 | else { return Eye3; } 42 | } 43 | 44 | template Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 45 | { 46 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 47 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 48 | if (norm > 0.00001) 49 | { 50 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 51 | Eigen::Matrix K; 52 | K << SKEW_SYM_MATRX(r_ang); 53 | 54 | /// Roderigous Tranformation 55 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 56 | } 57 | else { return Eye3; } 58 | } 59 | 60 | /* Logrithm of a Rotation Matrix */ 61 | template Eigen::Matrix Log(const Eigen::Matrix &R) 62 | { 63 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); 64 | Eigen::Matrix K(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); 65 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 66 | } 67 | 68 | template Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) 69 | { 70 | T sy = sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)); 71 | bool singular = sy < 1e-6; 72 | T x, y, z; 73 | if (!singular) 74 | { 75 | x = atan2(rot(2, 1), rot(2, 2)); 76 | y = atan2(-rot(2, 0), sy); 77 | z = atan2(rot(1, 0), rot(0, 0)); 78 | } 79 | else 80 | { 81 | x = atan2(-rot(1, 2), rot(1, 1)); 82 | y = atan2(-rot(2, 0), sy); 83 | z = 0; 84 | } 85 | Eigen::Matrix ang(x, y, z); 86 | return ang; 87 | } 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /include/utils/types.h: -------------------------------------------------------------------------------- 1 | #ifndef TYPES_H 2 | #define TYPES_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | typedef pcl::PointXYZINormal PointType; 9 | typedef pcl::PointXYZRGB PointTypeRGB; 10 | typedef pcl::PointXYZRGBA PointTypeRGBA; 11 | typedef pcl::PointCloud PointCloudXYZI; 12 | typedef std::vector> PointVector; 13 | typedef pcl::PointCloud PointCloudXYZRGB; 14 | typedef pcl::PointCloud PointCloudXYZRGBA; 15 | 16 | typedef Eigen::Vector2f V2F; 17 | typedef Eigen::Vector2d V2D; 18 | typedef Eigen::Vector3d V3D; 19 | typedef Eigen::Matrix3d M3D; 20 | typedef Eigen::Vector3f V3F; 21 | typedef Eigen::Matrix3f M3F; 22 | 23 | #define MD(a, b) Eigen::Matrix 24 | #define VD(a) Eigen::Matrix 25 | #define MF(a, b) Eigen::Matrix 26 | #define VF(a) Eigen::Matrix 27 | 28 | struct Pose6D 29 | { 30 | /*** the preintegrated Lidar states at the time of IMU measurements in a frame ***/ 31 | double offset_time; // the offset time of IMU measurement w.r.t the first lidar point 32 | double acc[3]; // the preintegrated total acceleration (global frame) at the Lidar origin 33 | double gyr[3]; // the unbiased angular velocity (body frame) at the Lidar origin 34 | double vel[3]; // the preintegrated velocity (global frame) at the Lidar origin 35 | double pos[3]; // the preintegrated position (global frame) at the Lidar origin 36 | double rot[9]; // the preintegrated rotation (global frame) at the Lidar origin 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /include/utils/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | 4 | #include 5 | #include // for int64_t 6 | #include // for std::numeric_limits 7 | #include // for std::out_of_range 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | std::vector convertToIntVectorSafe(const std::vector& int64_vector); 16 | 17 | inline double stamp2Sec(const builtin_interfaces::msg::Time& stamp) 18 | { 19 | return rclcpp::Time(stamp).seconds(); 20 | } 21 | 22 | inline rclcpp::Time sec2Stamp(double timestamp) 23 | { 24 | int32_t sec = std::floor(timestamp); 25 | auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9; 26 | uint32_t nanosec = nanosec_d; 27 | return rclcpp::Time(sec, nanosec); 28 | } 29 | 30 | namespace tf 31 | { 32 | 33 | inline geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) 34 | { 35 | tf2::Quaternion q; 36 | q.setRPY(0, 0, yaw); 37 | return tf2::toMsg(q); 38 | } 39 | 40 | inline geometry_msgs::msg::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw) 41 | { 42 | tf2::Quaternion q; 43 | q.setRPY(roll, pitch, yaw); 44 | return tf2::toMsg(q); 45 | } 46 | 47 | inline tf2::Quaternion createQuaternionFromYaw(double yaw) 48 | { 49 | tf2::Quaternion q; 50 | q.setRPY(0, 0, yaw); 51 | return q; 52 | } 53 | 54 | inline tf2::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw) 55 | { 56 | tf2::Quaternion q; 57 | q.setRPY(roll, pitch, yaw); 58 | return q; 59 | } 60 | } 61 | 62 | inline geometry_msgs::msg::TransformStamped createTransformStamped( 63 | const tf2::Transform &transform, 64 | const builtin_interfaces::msg::Time &stamp, 65 | const std::string &frame_id, 66 | const std::string &child_frame_id) 67 | { 68 | geometry_msgs::msg::TransformStamped transform_stamped; 69 | transform_stamped.header.stamp = stamp; 70 | transform_stamped.header.frame_id = frame_id; 71 | transform_stamped.child_frame_id = child_frame_id; 72 | transform_stamped.transform = tf2::toMsg(transform); 73 | return transform_stamped; 74 | } 75 | 76 | #endif // UTILS_H -------------------------------------------------------------------------------- /include/vio.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef VIO_H_ 14 | #define VIO_H_ 15 | 16 | #include "voxel_map.h" 17 | #include "feature.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | struct SubSparseMap 27 | { 28 | vector propa_errors; 29 | vector errors; 30 | vector> warp_patch; 31 | vector search_levels; 32 | vector voxel_points; 33 | vector inv_expo_list; 34 | vector add_from_voxel_map; 35 | 36 | SubSparseMap() 37 | { 38 | propa_errors.reserve(SIZE_LARGE); 39 | errors.reserve(SIZE_LARGE); 40 | warp_patch.reserve(SIZE_LARGE); 41 | search_levels.reserve(SIZE_LARGE); 42 | voxel_points.reserve(SIZE_LARGE); 43 | inv_expo_list.reserve(SIZE_LARGE); 44 | add_from_voxel_map.reserve(SIZE_SMALL); 45 | }; 46 | 47 | void reset() 48 | { 49 | propa_errors.clear(); 50 | errors.clear(); 51 | warp_patch.clear(); 52 | search_levels.clear(); 53 | voxel_points.clear(); 54 | inv_expo_list.clear(); 55 | add_from_voxel_map.clear(); 56 | } 57 | }; 58 | 59 | class Warp 60 | { 61 | public: 62 | Matrix2d A_cur_ref; 63 | int search_level; 64 | Warp(int level, Matrix2d warp_matrix) : search_level(level), A_cur_ref(warp_matrix) {} 65 | ~Warp() {} 66 | }; 67 | 68 | class VOXEL_POINTS 69 | { 70 | public: 71 | std::vector voxel_points; 72 | int count; 73 | VOXEL_POINTS(int num) : count(num) {} 74 | ~VOXEL_POINTS() 75 | { 76 | for (VisualPoint* vp : voxel_points) 77 | { 78 | if (vp != nullptr) { delete vp; vp = nullptr; } 79 | } 80 | } 81 | }; 82 | 83 | class VIOManager 84 | { 85 | public: 86 | int grid_size; 87 | vk::AbstractCamera *cam; 88 | vk::PinholeCamera *pinhole_cam; 89 | StatesGroup *state; 90 | StatesGroup *state_propagat; 91 | M3D Rli, Rci, Rcl, Rcw, Jdphi_dR, Jdp_dt, Jdp_dR; 92 | V3D Pli, Pci, Pcl, Pcw; 93 | vector grid_num; 94 | vector map_index; 95 | vector border_flag; 96 | vector update_flag; 97 | vector map_dist; 98 | vector scan_value; 99 | vector patch_buffer; 100 | bool normal_en, inverse_composition_en, exposure_estimate_en, raycast_en, has_ref_patch_cache; 101 | bool ncc_en = false, colmap_output_en = false; 102 | 103 | int width, height, grid_n_width, grid_n_height, length; 104 | double image_resize_factor; 105 | double fx, fy, cx, cy; 106 | int patch_pyrimid_level, patch_size, patch_size_total, patch_size_half, border, warp_len; 107 | int max_iterations, total_points; 108 | 109 | double img_point_cov, outlier_threshold, ncc_thre; 110 | 111 | SubSparseMap *visual_submap; 112 | std::vector> rays_with_sample_points; 113 | 114 | double compute_jacobian_time, update_ekf_time; 115 | double ave_total = 0; 116 | // double ave_build_residual_time = 0; 117 | // double ave_ekf_time = 0; 118 | 119 | int frame_count = 0; 120 | bool plot_flag; 121 | 122 | Eigen::Matrix G, H_T_H; 123 | Eigen::MatrixXd K, H_sub_inv; 124 | 125 | ofstream fout_camera, fout_colmap; 126 | unordered_map feat_map; 127 | unordered_map sub_feat_map; 128 | unordered_map warp_map; 129 | vector retrieve_voxel_points; 130 | vector append_voxel_points; 131 | FramePtr new_frame_; 132 | cv::Mat img_cp, img_rgb, img_test; 133 | 134 | enum CellType 135 | { 136 | TYPE_MAP = 1, 137 | TYPE_POINTCLOUD, 138 | TYPE_UNKNOWN 139 | }; 140 | 141 | VIOManager(); 142 | ~VIOManager(); 143 | void updateStateInverse(cv::Mat img, int level); 144 | void updateState(cv::Mat img, int level); 145 | void processFrame(cv::Mat &img, vector &pg, const unordered_map &feat_map, double img_time); 146 | void retrieveFromVisualSparseMap(cv::Mat img, vector &pg, const unordered_map &plane_map); 147 | void generateVisualMapPoints(cv::Mat img, vector &pg); 148 | void setImuToLidarExtrinsic(const V3D &transl, const M3D &rot); 149 | void setLidarToCameraExtrinsic(vector &R, vector &P); 150 | void initializeVIO(); 151 | void getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level); 152 | void computeProjectionJacobian(V3D p, MD(2, 3) & J); 153 | void computeJacobianAndUpdateEKF(cv::Mat img); 154 | void resetGrid(); 155 | void updateVisualMapPoints(cv::Mat img); 156 | void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref, 157 | const int level_ref, 158 | const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref); 159 | void getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, 160 | const V3D &xyz_ref, const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref); 161 | void warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level, 162 | const int pyramid_level, const int halfpatch_size, float *patch); 163 | void insertPointIntoVoxelMap(VisualPoint *pt_new); 164 | void plotTrackedPoints(); 165 | void updateFrameState(StatesGroup state); 166 | void projectPatchFromRefToCur(const unordered_map &plane_map); 167 | void updateReferencePatch(const unordered_map &plane_map); 168 | void precomputeReferencePatches(int level); 169 | void dumpDataForColmap(); 170 | double calculateNCC(float *ref_patch, float *cur_patch, int patch_size); 171 | int getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level); 172 | V3F getInterpolatedPixel(cv::Mat img, V2D pc); 173 | 174 | // void resetRvizDisplay(); 175 | // deque map_cur_frame; 176 | // deque sub_map_ray; 177 | // deque sub_map_ray_fov; 178 | // deque visual_sub_map_cur; 179 | // deque visual_converged_point; 180 | // std::vector> sample_points; 181 | 182 | // PointCloudXYZI::Ptr pg_down; 183 | // pcl::VoxelGrid downSizeFilter; 184 | }; 185 | typedef std::shared_ptr VIOManagerPtr; 186 | 187 | #endif // VIO_H_ -------------------------------------------------------------------------------- /include/visual_point.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef LIVO_POINT_H_ 14 | #define LIVO_POINT_H_ 15 | 16 | #include 17 | #include "common_lib.h" 18 | #include "frame.h" 19 | 20 | class Feature; 21 | 22 | /// A visual map point on the surface of the scene. 23 | class VisualPoint : boost::noncopyable 24 | { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. 29 | Vector3d normal_; //!< Surface normal at point. 30 | Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. 31 | Vector3d previous_normal_; //!< Last updated normal vector. 32 | list obs_; //!< Reference patches which observe the point. 33 | Eigen::Matrix3d covariance_; //!< Covariance of the point. 34 | bool is_converged_; //!< True if the point is converged. 35 | bool is_normal_initialized_; //!< True if the normal is initialized. 36 | bool has_ref_patch_; //!< True if the point has a reference patch. 37 | Feature *ref_patch; //!< Reference patch of the point. 38 | 39 | VisualPoint(const Vector3d &pos); 40 | ~VisualPoint(); 41 | void findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const; 42 | void deleteNonRefPatchFeatures(); 43 | void deleteFeatureRef(Feature *ftr); 44 | void addFrameRef(Feature *ftr); 45 | bool getCloseViewObs(const Vector3d &pos, Feature *&obs, const Vector2d &cur_px) const; 46 | }; 47 | 48 | #endif // LIVO_POINT_H_ 49 | -------------------------------------------------------------------------------- /include/voxel_map.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #ifndef VOXEL_MAP_H_ 14 | #define VOXEL_MAP_H_ 15 | 16 | #include "common_lib.h" 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #define VOXELMAP_HASH_P 116101 31 | #define VOXELMAP_MAX_N 10000000000 32 | 33 | static int voxel_plane_id = 0; 34 | 35 | typedef struct VoxelMapConfig 36 | { 37 | double max_voxel_size_; 38 | int max_layer_; 39 | int max_iterations_; 40 | std::vector layer_init_num_; 41 | int max_points_num_; 42 | double planner_threshold_; 43 | double beam_err_; 44 | double dept_err_; 45 | double sigma_num_; 46 | bool is_pub_plane_map_; 47 | 48 | // config of local map sliding 49 | double sliding_thresh; 50 | bool map_sliding_en; 51 | int half_map_size; 52 | } VoxelMapConfig; 53 | 54 | typedef struct PointToPlane 55 | { 56 | Eigen::Vector3d point_b_; 57 | Eigen::Vector3d point_w_; 58 | Eigen::Vector3d normal_; 59 | Eigen::Vector3d center_; 60 | Eigen::Matrix plane_var_; 61 | M3D body_cov_; 62 | int layer_; 63 | double d_; 64 | double eigen_value_; 65 | bool is_valid_; 66 | float dis_to_plane_; 67 | } PointToPlane; 68 | 69 | typedef struct VoxelPlane 70 | { 71 | Eigen::Vector3d center_; 72 | Eigen::Vector3d normal_; 73 | Eigen::Vector3d y_normal_; 74 | Eigen::Vector3d x_normal_; 75 | Eigen::Matrix3d covariance_; 76 | Eigen::Matrix plane_var_; 77 | float radius_ = 0; 78 | float min_eigen_value_ = 1; 79 | float mid_eigen_value_ = 1; 80 | float max_eigen_value_ = 1; 81 | float d_ = 0; 82 | int points_size_ = 0; 83 | bool is_plane_ = false; 84 | bool is_init_ = false; 85 | int id_ = 0; 86 | bool is_update_ = false; 87 | VoxelPlane() 88 | { 89 | plane_var_ = Eigen::Matrix::Zero(); 90 | covariance_ = Eigen::Matrix3d::Zero(); 91 | center_ = Eigen::Vector3d::Zero(); 92 | normal_ = Eigen::Vector3d::Zero(); 93 | } 94 | } VoxelPlane; 95 | 96 | class VOXEL_LOCATION 97 | { 98 | public: 99 | int64_t x, y, z; 100 | 101 | VOXEL_LOCATION(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) : x(vx), y(vy), z(vz) {} 102 | 103 | bool operator==(const VOXEL_LOCATION &other) const { return (x == other.x && y == other.y && z == other.z); } 104 | }; 105 | 106 | // Hash value 107 | namespace std 108 | { 109 | template <> struct hash 110 | { 111 | int64_t operator()(const VOXEL_LOCATION &s) const 112 | { 113 | using std::hash; 114 | using std::size_t; 115 | return ((((s.z) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.y)) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.x); 116 | } 117 | }; 118 | } // namespace std 119 | 120 | struct DS_POINT 121 | { 122 | float xyz[3]; 123 | float intensity; 124 | int count = 0; 125 | }; 126 | 127 | void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov); 128 | 129 | class VoxelOctoTree 130 | { 131 | 132 | public: 133 | VoxelOctoTree() = default; 134 | std::vector temp_points_; 135 | VoxelPlane *plane_ptr_; 136 | int layer_; 137 | int octo_state_; // 0 is end of tree, 1 is not 138 | VoxelOctoTree *leaves_[8]; 139 | double voxel_center_[3]; // x, y, z 140 | std::vector layer_init_num_; 141 | float quater_length_; 142 | float planer_threshold_; 143 | int points_size_threshold_; 144 | int update_size_threshold_; 145 | int max_points_num_; 146 | int max_layer_; 147 | int new_points_; 148 | bool init_octo_; 149 | bool update_enable_; 150 | 151 | VoxelOctoTree(int max_layer, int layer, int points_size_threshold, int max_points_num, float planer_threshold) 152 | : max_layer_(max_layer), layer_(layer), points_size_threshold_(points_size_threshold), max_points_num_(max_points_num), 153 | planer_threshold_(planer_threshold) 154 | { 155 | temp_points_.clear(); 156 | octo_state_ = 0; 157 | new_points_ = 0; 158 | update_size_threshold_ = 5; 159 | init_octo_ = false; 160 | update_enable_ = true; 161 | for (int i = 0; i < 8; i++) 162 | { 163 | leaves_[i] = nullptr; 164 | } 165 | plane_ptr_ = new VoxelPlane; 166 | } 167 | 168 | ~VoxelOctoTree() 169 | { 170 | for (int i = 0; i < 8; i++) 171 | { 172 | delete leaves_[i]; 173 | } 174 | delete plane_ptr_; 175 | } 176 | void init_plane(const std::vector &points, VoxelPlane *plane); 177 | void init_octo_tree(); 178 | void cut_octo_tree(); 179 | void UpdateOctoTree(const pointWithVar &pv); 180 | 181 | VoxelOctoTree *find_correspond(Eigen::Vector3d pw); 182 | VoxelOctoTree *Insert(const pointWithVar &pv); 183 | }; 184 | 185 | void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config); 186 | 187 | class VoxelMapManager 188 | { 189 | public: 190 | VoxelMapManager() = default; 191 | VoxelMapConfig config_setting_; 192 | int current_frame_id_ = 0; 193 | rclcpp::Publisher::SharedPtr voxel_map_pub_; 194 | std::unordered_map voxel_map_; 195 | 196 | PointCloudXYZI::Ptr feats_undistort_; 197 | PointCloudXYZI::Ptr feats_down_body_; 198 | PointCloudXYZI::Ptr feats_down_world_; 199 | 200 | M3D extR_; 201 | V3D extT_; 202 | float build_residual_time, ekf_time; 203 | float ave_build_residual_time = 0.0; 204 | float ave_ekf_time = 0.0; 205 | int scan_count = 0; 206 | StatesGroup state_; 207 | V3D position_last_; 208 | 209 | V3D last_slide_position = {0,0,0}; 210 | 211 | geometry_msgs::msg::Quaternion geoQuat_; 212 | 213 | int feats_down_size_; 214 | int effct_feat_num_; 215 | std::vector cross_mat_list_; 216 | std::vector body_cov_list_; 217 | std::vector pv_list_; 218 | std::vector ptpl_list_; 219 | 220 | VoxelMapManager(VoxelMapConfig &config_setting, std::unordered_map &voxel_map) 221 | : config_setting_(config_setting), voxel_map_(voxel_map) 222 | { 223 | current_frame_id_ = 0; 224 | feats_undistort_.reset(new PointCloudXYZI()); 225 | feats_down_body_.reset(new PointCloudXYZI()); 226 | feats_down_world_.reset(new PointCloudXYZI()); 227 | }; 228 | 229 | void StateEstimation(StatesGroup &state_propagat); 230 | void TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, 231 | pcl::PointCloud::Ptr &trans_cloud); 232 | 233 | void BuildVoxelMap(); 234 | V3F RGBFromVoxel(const V3D &input_point); 235 | 236 | void UpdateVoxelMap(const std::vector &input_points); 237 | 238 | void BuildResidualListOMP(std::vector &pv_list, std::vector &ptpl_list); 239 | 240 | void build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess, double &prob, 241 | PointToPlane &single_ptpl); 242 | 243 | void pubVoxelMap(); 244 | 245 | void mapSliding(); 246 | void clearMemOutOfMap(const int& x_max,const int& x_min,const int& y_max,const int& y_min,const int& z_max,const int& z_min ); 247 | 248 | private: 249 | void GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector &plane_list); 250 | 251 | void pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha, 252 | const Eigen::Vector3d rgb); 253 | void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::msg::Quaternion &q); 254 | 255 | void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b); 256 | }; 257 | typedef std::shared_ptr VoxelMapManagerPtr; 258 | 259 | #endif // VOXEL_MAP_H_ -------------------------------------------------------------------------------- /launch/mapping_avia_marslvig.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -- coding: utf-8 --** 3 | 4 | import os 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 7 | from launch.conditions import IfCondition 8 | from launch.substitutions import LaunchConfiguration 9 | from ament_index_python.packages import get_package_share_directory 10 | from launch_ros.actions import Node 11 | 12 | def generate_launch_description(): 13 | 14 | # Find path 15 | config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") 16 | rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "M300.rviz") 17 | 18 | #Load parameters 19 | avia_config_cmd = os.path.join(config_file_dir, "MARS_LVIG.yaml") 20 | camera_config_cmd = os.path.join(config_file_dir, "camera_MARS_LVIG.yaml") 21 | 22 | # Param use_rviz 23 | use_rviz_arg = DeclareLaunchArgument( 24 | "use_rviz", 25 | default_value="False", 26 | description="Whether to launch Rviz2", 27 | ) 28 | 29 | avia_config_arg = DeclareLaunchArgument( 30 | 'avia_params_file', 31 | default_value=avia_config_cmd, 32 | description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', 33 | ) 34 | 35 | camera_config_arg = DeclareLaunchArgument( 36 | 'camera_params_file', 37 | default_value=camera_config_cmd, 38 | description='Full path to the ROS2 parameters file to use for vikit_ros nodes', 39 | ) 40 | 41 | # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 42 | use_respawn_arg = DeclareLaunchArgument( 43 | 'use_respawn', 44 | default_value='True', 45 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 46 | 47 | avia_params_file = LaunchConfiguration('avia_params_file') 48 | camera_params_file = LaunchConfiguration('camera_params_file') 49 | use_respawn = LaunchConfiguration('use_respawn') 50 | 51 | return LaunchDescription([ 52 | use_rviz_arg, 53 | avia_config_arg, 54 | camera_config_arg, 55 | use_respawn_arg, 56 | 57 | # use parameter_blackboard as global parameters server and load camera params 58 | Node( 59 | package='demo_nodes_cpp', 60 | executable='parameter_blackboard', 61 | name='parameter_blackboard', 62 | # namespace='laserMapping', 63 | parameters=[ 64 | camera_params_file, 65 | ], 66 | output='screen' 67 | ), 68 | 69 | # republish compressed image to raw image 70 | # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 71 | # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image 72 | Node( 73 | package="image_transport", 74 | executable="republish", 75 | name="republish", 76 | arguments=[ # Array of strings/parametric arguments that will end up in process's argv 77 | 'compressed', 78 | 'raw', 79 | ], 80 | remappings=[ 81 | ("in", "/left_camera/image"), 82 | ("out", "/left_camera/image") 83 | ], 84 | output="screen", 85 | respawn=use_respawn, 86 | ), 87 | 88 | Node( 89 | package="fast_livo", 90 | executable="fastlivo_mapping", 91 | name="laserMapping", 92 | parameters=[ 93 | avia_params_file, 94 | ], 95 | # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html 96 | prefix=[ 97 | # ("gdb -ex run --args"), 98 | # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") 99 | ], 100 | output="screen" 101 | ), 102 | 103 | Node( 104 | condition=IfCondition(LaunchConfiguration("use_rviz")), 105 | package="rviz2", 106 | executable="rviz2", 107 | name="rviz2", 108 | arguments=["-d", rviz_config_file], 109 | output="screen" 110 | ), 111 | ]) 112 | -------------------------------------------------------------------------------- /launch/mapping_aviz.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -- coding: utf-8 --** 3 | 4 | import os 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 7 | from launch.conditions import IfCondition 8 | from launch.substitutions import LaunchConfiguration 9 | from ament_index_python.packages import get_package_share_directory 10 | from launch_ros.actions import Node 11 | 12 | def generate_launch_description(): 13 | 14 | # Find path 15 | config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") 16 | rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "fast_livo2.rviz") 17 | 18 | #Load parameters 19 | avia_config_cmd = os.path.join(config_file_dir, "avia.yaml") 20 | camera_config_cmd = os.path.join(config_file_dir, "camera_pinhole.yaml") 21 | 22 | # Param use_rviz 23 | use_rviz_arg = DeclareLaunchArgument( 24 | "use_rviz", 25 | default_value="False", 26 | description="Whether to launch Rviz2", 27 | ) 28 | 29 | avia_config_arg = DeclareLaunchArgument( 30 | 'avia_params_file', 31 | default_value=avia_config_cmd, 32 | description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', 33 | ) 34 | 35 | camera_config_arg = DeclareLaunchArgument( 36 | 'camera_params_file', 37 | default_value=camera_config_cmd, 38 | description='Full path to the ROS2 parameters file to use for vikit_ros nodes', 39 | ) 40 | 41 | # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 42 | use_respawn_arg = DeclareLaunchArgument( 43 | 'use_respawn', 44 | default_value='True', 45 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 46 | 47 | avia_params_file = LaunchConfiguration('avia_params_file') 48 | camera_params_file = LaunchConfiguration('camera_params_file') 49 | use_respawn = LaunchConfiguration('use_respawn') 50 | 51 | return LaunchDescription([ 52 | use_rviz_arg, 53 | avia_config_arg, 54 | camera_config_arg, 55 | use_respawn_arg, 56 | 57 | # play ros2 bag 58 | # ExecuteProcess( 59 | # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], 60 | # shell=True 61 | # ), 62 | 63 | # use parameter_blackboard as global parameters server and load camera params 64 | Node( 65 | package='demo_nodes_cpp', 66 | executable='parameter_blackboard', 67 | name='parameter_blackboard', 68 | # namespace='laserMapping', 69 | parameters=[ 70 | camera_params_file, 71 | ], 72 | output='screen' 73 | ), 74 | 75 | # republish compressed image to raw image 76 | # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 77 | # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image 78 | Node( 79 | package="image_transport", 80 | executable="republish", 81 | name="republish", 82 | arguments=[ # Array of strings/parametric arguments that will end up in process's argv 83 | 'compressed', 84 | 'raw', 85 | ], 86 | remappings=[ 87 | ("in", "/left_camera/image"), 88 | ("out", "/left_camera/image") 89 | ], 90 | output="screen", 91 | respawn=use_respawn, 92 | ), 93 | 94 | Node( 95 | package="fast_livo", 96 | executable="fastlivo_mapping", 97 | name="laserMapping", 98 | parameters=[ 99 | avia_params_file, 100 | ], 101 | # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html 102 | prefix=[ 103 | # ("gdb -ex run --args"), 104 | # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") 105 | ], 106 | output="screen" 107 | ), 108 | 109 | Node( 110 | condition=IfCondition(LaunchConfiguration("use_rviz")), 111 | package="rviz2", 112 | executable="rviz2", 113 | name="rviz2", 114 | arguments=["-d", rviz_config_file], 115 | output="screen" 116 | ), 117 | ]) 118 | -------------------------------------------------------------------------------- /launch/mapping_ouster_ntu.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -- coding: utf-8 --** 3 | 4 | import os 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 7 | from launch.conditions import IfCondition 8 | from launch.substitutions import LaunchConfiguration 9 | from ament_index_python.packages import get_package_share_directory 10 | from launch_ros.actions import Node 11 | 12 | def generate_launch_description(): 13 | 14 | # Find path 15 | config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") 16 | rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "ntu_viral.rviz") 17 | 18 | #Load parameters 19 | avia_config_cmd = os.path.join(config_file_dir, "NTU_VIRAL.yaml") 20 | camera_config_cmd = os.path.join(config_file_dir, "camera_NTU_VIRAL.yaml") 21 | 22 | # Param use_rviz 23 | use_rviz_arg = DeclareLaunchArgument( 24 | "use_rviz", 25 | default_value="False", 26 | description="Whether to launch Rviz2", 27 | ) 28 | 29 | avia_config_arg = DeclareLaunchArgument( 30 | 'avia_params_file', 31 | default_value=avia_config_cmd, 32 | description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', 33 | ) 34 | 35 | camera_config_arg = DeclareLaunchArgument( 36 | 'camera_params_file', 37 | default_value=camera_config_cmd, 38 | description='Full path to the ROS2 parameters file to use for vikit_ros nodes', 39 | ) 40 | 41 | # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 42 | use_respawn_arg = DeclareLaunchArgument( 43 | 'use_respawn', 44 | default_value='True', 45 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 46 | 47 | avia_params_file = LaunchConfiguration('avia_params_file') 48 | camera_params_file = LaunchConfiguration('camera_params_file') 49 | use_respawn = LaunchConfiguration('use_respawn') 50 | 51 | return LaunchDescription([ 52 | use_rviz_arg, 53 | avia_config_arg, 54 | camera_config_arg, 55 | use_respawn_arg, 56 | 57 | # play ros2 bag 58 | # ExecuteProcess( 59 | # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], 60 | # shell=True 61 | # ), 62 | 63 | # use parameter_blackboard as global parameters server and load camera params 64 | Node( 65 | package='demo_nodes_cpp', 66 | executable='parameter_blackboard', 67 | name='parameter_blackboard', 68 | # namespace='laserMapping', 69 | parameters=[ 70 | camera_params_file, 71 | ], 72 | output='screen' 73 | ), 74 | 75 | # republish compressed image to raw image 76 | # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 77 | # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image 78 | Node( 79 | package="image_transport", 80 | executable="republish", 81 | name="republish", 82 | arguments=[ # Array of strings/parametric arguments that will end up in process's argv 83 | 'compressed', 84 | 'raw', 85 | ], 86 | remappings=[ 87 | ("in", "/left_camera/image"), 88 | ("out", "/left_camera/image") 89 | ], 90 | output="screen", 91 | respawn=use_respawn, 92 | ), 93 | 94 | Node( 95 | package="fast_livo", 96 | executable="fastlivo_mapping", 97 | name="laserMapping", 98 | parameters=[ 99 | avia_params_file, 100 | ], 101 | # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html 102 | prefix=[ 103 | # ("gdb -ex run --args"), 104 | # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") 105 | ], 106 | output="screen" 107 | ), 108 | 109 | Node( 110 | condition=IfCondition(LaunchConfiguration("use_rviz")), 111 | package="rviz2", 112 | executable="rviz2", 113 | name="rviz2", 114 | arguments=["-d", rviz_config_file], 115 | output="screen" 116 | ), 117 | ]) 118 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_livo 4 | 0.0.0 5 | 6 | 7 | This is a modified version of LOAM which is original algorithm 8 | is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | claydergc 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | ament_cmake 20 | 21 | rclcpp 22 | rclpy 23 | sensor_msgs 24 | geometry_msgs 25 | visualization_msgs 26 | nav_msgs 27 | std_msgs 28 | tf2_ros 29 | pcl_ros 30 | pcl_conversions 31 | livox_ros_driver2 32 | vikit_common 33 | vikit_ros 34 | cv_bridge 35 | image_transport 36 | libopencv-dev 37 | 38 | cv_bridge 39 | image_transport 40 | libopencv-dev 41 | sensor_msgs 42 | std_msgs 43 | 44 | rosidl_interface_packages 45 | 46 | ament_lint_auto 47 | ament_lint_common 48 | 49 | 50 | ament_cmake 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /pics/Framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/pics/Framework.png -------------------------------------------------------------------------------- /pics/debug_error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/pics/debug_error.png -------------------------------------------------------------------------------- /pics/rosgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotic-Developer-Road/FAST-LIVO2/18f02d8665c58e21d78e66517e311e64f7c2bf8a/pics/rosgraph.png -------------------------------------------------------------------------------- /rviz_cfg/M300.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /Axes1 9 | - /mapping1 10 | - /mapping1/currPoints1 11 | - /mapping1/surround1 12 | - /mapping1/surround1/Autocompute Value Bounds1 13 | - /mapping1/PointCloud21 14 | - /Odometry1 15 | - /Odometry1/Odometry1 16 | - /Odometry1/Odometry1/Shape1 17 | - /Path1 18 | - /currPoints1/Autocompute Value Bounds1 19 | - /MarkerArray1/Namespaces1 20 | - /currPoints2/Autocompute Value Bounds1 21 | - /Odometry2/Shape1 22 | - /MarkerArray3 23 | - /MarkerArray4 24 | - /MarkerArray5 25 | - /Image1 26 | Splitter Ratio: 0.34272301197052 27 | Tree Height: 538 28 | - Class: rviz_common/Selection 29 | Name: Selection 30 | - Class: rviz_common/Tool Properties 31 | Expanded: 32 | - /2D Pose Estimate1 33 | - /2D Nav Goal1 34 | - /Publish Point1 35 | Name: Tool Properties 36 | Splitter Ratio: 0.5886790156364441 37 | - Class: rviz_common/Views 38 | Expanded: 39 | - /Current View1 40 | Name: Views 41 | Splitter Ratio: 0.5 42 | - Class: rviz_common/Time 43 | Name: Time 44 | SyncMode: 0 45 | SyncSource: surround 46 | Preferences: 47 | PromptSaveOnExit: true 48 | Toolbars: 49 | toolButtonStyle: 2 50 | Visualization Manager: 51 | Class: "" 52 | Displays: 53 | - Alpha: 1 54 | Cell Size: 1 55 | Class: rviz_default_plugins/Grid 56 | Color: 160; 160; 164 57 | Enabled: false 58 | Line Style: 59 | Line Width: 0.029999999329447746 60 | Value: Lines 61 | Name: Grid 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Plane: XY 68 | Plane Cell Count: 160 69 | Reference Frame: 70 | Value: false 71 | - Alpha: 1 72 | Class: rviz_default_plugins/Axes 73 | Enabled: true 74 | Length: 4 75 | Name: Axes 76 | Radius: 1.2000000476837158 77 | Reference Frame: 78 | Show Trail: false 79 | Value: true 80 | - Class: rviz_common/Group 81 | Displays: 82 | - Alpha: 1 83 | Autocompute Intensity Bounds: true 84 | Autocompute Value Bounds: 85 | Max Value: 20 86 | Min Value: -3 87 | Value: false 88 | Axis: Z 89 | Channel Name: intensity 90 | Class: rviz_default_plugins/PointCloud2 91 | Color: 199; 228; 247 92 | Color Transformer: FlatColor 93 | Decay Time: 0 94 | Enabled: true 95 | Invert Rainbow: false 96 | Max Color: 255; 255; 255 97 | Min Color: 0; 0; 0 98 | Name: currPoints 99 | Position Transformer: XYZ 100 | Queue Size: 100000 101 | Selectable: true 102 | Size (Pixels): 2 103 | Size (m): 0.009999999776482582 104 | Style: Points 105 | Topic: /cloud_registered 106 | Unreliable: false 107 | Use Fixed Frame: true 108 | Use rainbow: false 109 | Value: true 110 | - Alpha: 0.5 111 | Autocompute Intensity Bounds: false 112 | Autocompute Value Bounds: 113 | Max Value: 15 114 | Min Value: -5 115 | Value: false 116 | Axis: Z 117 | Channel Name: intensity 118 | Class: rviz_default_plugins/PointCloud2 119 | Color: 238; 238; 236 120 | Color Transformer: RGB8 121 | Decay Time: 10000 122 | Enabled: true 123 | Invert Rainbow: false 124 | Max Color: 255; 255; 255 125 | Max Intensity: 159 126 | Min Color: 0; 0; 0 127 | Min Intensity: 5 128 | Name: surround 129 | Position Transformer: XYZ 130 | Queue Size: 1 131 | Selectable: false 132 | Size (Pixels): 1 133 | Size (m): 0.004999999888241291 134 | Style: Points 135 | Topic: /cloud_registered 136 | Unreliable: true 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | - Alpha: 1 141 | Autocompute Intensity Bounds: true 142 | Autocompute Value Bounds: 143 | Max Value: 10 144 | Min Value: -10 145 | Value: true 146 | Axis: Z 147 | Channel Name: intensity 148 | Class: rviz_default_plugins/PointCloud2 149 | Color: 239; 41; 41 150 | Color Transformer: "" 151 | Decay Time: 0 152 | Enabled: false 153 | Invert Rainbow: false 154 | Max Color: 255; 255; 255 155 | Min Color: 0; 0; 0 156 | Name: PointCloud2 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 5 161 | Size (m): 0.019999999552965164 162 | Style: Squares 163 | Topic: /cloud_effected 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: false 168 | Enabled: true 169 | Name: mapping 170 | - Class: rviz_common/Group 171 | Displays: 172 | - Angle Tolerance: 0.009999999776482582 173 | Class: rviz_default_plugins/Odometry 174 | Covariance: 175 | Orientation: 176 | Alpha: 0.5 177 | Color: 255; 255; 127 178 | Color Style: Unique 179 | Frame: Local 180 | Offset: 1 181 | Scale: 1 182 | Value: true 183 | Position: 184 | Alpha: 0.30000001192092896 185 | Color: 204; 51; 204 186 | Scale: 1 187 | Value: true 188 | Value: true 189 | Enabled: true 190 | Keep: 1 191 | Name: Odometry 192 | Position Tolerance: 0.0010000000474974513 193 | Queue Size: 10 194 | Shape: 195 | Alpha: 1 196 | Axes Length: 5 197 | Axes Radius: 1 198 | Color: 255; 85; 0 199 | Head Length: 0 200 | Head Radius: 0 201 | Shaft Length: 0.800000011920929 202 | Shaft Radius: 0.5 203 | Value: Axes 204 | Topic: /aft_mapped_to_init 205 | Unreliable: false 206 | Value: true 207 | Enabled: true 208 | Name: Odometry 209 | - Alpha: 0 210 | Buffer Length: 2 211 | Class: rviz_default_plugins/Path 212 | Color: 25; 255; 255 213 | Enabled: true 214 | Head Diameter: 0 215 | Head Length: 0 216 | Length: 0.30000001192092896 217 | Line Style: Billboards 218 | Line Width: 0.699999988079071 219 | Name: Path 220 | Offset: 221 | X: 0 222 | Y: 0 223 | Z: 0 224 | Pose Color: 25; 255; 255 225 | Pose Style: None 226 | Queue Size: 10 227 | Radius: 0.029999999329447746 228 | Shaft Diameter: 0.4000000059604645 229 | Shaft Length: 0.4000000059604645 230 | Topic: /path 231 | Unreliable: false 232 | Value: true 233 | - Alpha: 0.10000000149011612 234 | Autocompute Intensity Bounds: true 235 | Autocompute Value Bounds: 236 | Max Value: 20 237 | Min Value: -3 238 | Value: false 239 | Axis: Z 240 | Channel Name: intensity 241 | Class: rviz_default_plugins/PointCloud2 242 | Color: 239; 41; 41 243 | Color Transformer: Intensity 244 | Decay Time: 1000 245 | Enabled: false 246 | Invert Rainbow: true 247 | Max Color: 255; 255; 255 248 | Min Color: 0; 0; 0 249 | Name: currPoints 250 | Position Transformer: XYZ 251 | Queue Size: 1 252 | Selectable: true 253 | Size (Pixels): 2 254 | Size (m): 0.009999999776482582 255 | Style: Points 256 | Topic: /cloud_voxel 257 | Unreliable: false 258 | Use Fixed Frame: true 259 | Use rainbow: true 260 | Value: false 261 | - Class: rviz_default_plugins/Marker 262 | Enabled: true 263 | Marker Topic: /planner_normal 264 | Name: Marker 265 | Namespaces: 266 | {} 267 | Queue Size: 100 268 | Value: true 269 | - Class: rviz_default_plugins/MarkerArray 270 | Enabled: false 271 | Marker Topic: /voxels 272 | Name: MarkerArray 273 | Namespaces: 274 | {} 275 | Queue Size: 100 276 | Value: false 277 | - Alpha: 1 278 | Autocompute Intensity Bounds: true 279 | Autocompute Value Bounds: 280 | Max Value: 10 281 | Min Value: -10 282 | Value: true 283 | Axis: Z 284 | Channel Name: intensity 285 | Class: rviz_default_plugins/PointCloud2 286 | Color: 245; 121; 0 287 | Color Transformer: FlatColor 288 | Decay Time: 0 289 | Enabled: false 290 | Invert Rainbow: false 291 | Max Color: 255; 255; 255 292 | Min Color: 0; 0; 0 293 | Name: PointCloud2 294 | Position Transformer: XYZ 295 | Queue Size: 1 296 | Selectable: true 297 | Size (Pixels): 15 298 | Size (m): 0.009999999776482582 299 | Style: Points 300 | Topic: /cloud_ray_sub_map 301 | Unreliable: false 302 | Use Fixed Frame: true 303 | Use rainbow: true 304 | Value: false 305 | - Class: rviz_default_plugins/MarkerArray 306 | Enabled: false 307 | Marker Topic: /visualization_marker 308 | Name: MarkerArray 309 | Namespaces: 310 | {} 311 | Queue Size: 100 312 | Value: false 313 | - Alpha: 1 314 | Autocompute Intensity Bounds: true 315 | Autocompute Value Bounds: 316 | Max Value: 10 317 | Min Value: -10 318 | Value: true 319 | Axis: Z 320 | Channel Name: intensity 321 | Class: rviz_default_plugins/PointCloud2 322 | Color: 92; 53; 102 323 | Color Transformer: FlatColor 324 | Decay Time: 99999 325 | Enabled: false 326 | Invert Rainbow: false 327 | Max Color: 255; 255; 255 328 | Min Color: 0; 0; 0 329 | Name: PointCloud2 330 | Position Transformer: XYZ 331 | Queue Size: 10 332 | Selectable: true 333 | Size (Pixels): 10 334 | Size (m): 0.009999999776482582 335 | Style: Points 336 | Topic: /cloud_visual_map 337 | Unreliable: false 338 | Use Fixed Frame: true 339 | Use rainbow: true 340 | Value: false 341 | - Alpha: 1 342 | Autocompute Intensity Bounds: true 343 | Autocompute Value Bounds: 344 | Max Value: 10 345 | Min Value: -10 346 | Value: true 347 | Axis: Z 348 | Channel Name: intensity 349 | Class: rviz_default_plugins/PointCloud2 350 | Color: 115; 210; 22 351 | Color Transformer: FlatColor 352 | Decay Time: 0 353 | Enabled: false 354 | Invert Rainbow: false 355 | Max Color: 255; 255; 255 356 | Min Color: 0; 0; 0 357 | Name: surround 358 | Position Transformer: XYZ 359 | Queue Size: 1 360 | Selectable: false 361 | Size (Pixels): 12 362 | Size (m): 0.05000000074505806 363 | Style: Points 364 | Topic: /cloud_visual_sub_map 365 | Unreliable: true 366 | Use Fixed Frame: true 367 | Use rainbow: true 368 | Value: false 369 | - Alpha: 1 370 | Autocompute Intensity Bounds: true 371 | Autocompute Value Bounds: 372 | Max Value: 20 373 | Min Value: -3 374 | Value: false 375 | Axis: Z 376 | Channel Name: intensity 377 | Class: rviz_default_plugins/PointCloud2 378 | Color: 237; 212; 0 379 | Color Transformer: FlatColor 380 | Decay Time: 0 381 | Enabled: false 382 | Invert Rainbow: false 383 | Max Color: 255; 255; 255 384 | Min Color: 0; 0; 0 385 | Name: currPoints 386 | Position Transformer: XYZ 387 | Queue Size: 100000 388 | Selectable: true 389 | Size (Pixels): 5 390 | Size (m): 0.009999999776482582 391 | Style: Points 392 | Topic: /cloud_sample_points 393 | Unreliable: false 394 | Use Fixed Frame: true 395 | Use rainbow: false 396 | Value: false 397 | - Alpha: 1 398 | Autocompute Intensity Bounds: true 399 | Autocompute Value Bounds: 400 | Max Value: 10 401 | Min Value: -10 402 | Value: true 403 | Axis: Z 404 | Channel Name: intensity 405 | Class: rviz_default_plugins/PointCloud2 406 | Color: 239; 41; 41 407 | Color Transformer: FlatColor 408 | Decay Time: 99999 409 | Enabled: false 410 | Invert Rainbow: false 411 | Max Color: 255; 255; 255 412 | Min Color: 239; 41; 41 413 | Name: PointCloud2 414 | Position Transformer: XYZ 415 | Queue Size: 10 416 | Selectable: true 417 | Size (Pixels): 4 418 | Size (m): 0.009999999776482582 419 | Style: Points 420 | Topic: /cloud_visual_map 421 | Unreliable: false 422 | Use Fixed Frame: true 423 | Use rainbow: true 424 | Value: false 425 | - Alpha: 1 426 | Autocompute Intensity Bounds: true 427 | Autocompute Value Bounds: 428 | Max Value: 10 429 | Min Value: -10 430 | Value: true 431 | Axis: Z 432 | Channel Name: intensity 433 | Class: rviz_default_plugins/PointCloud2 434 | Color: 92; 53; 102 435 | Color Transformer: FlatColor 436 | Decay Time: 0 437 | Enabled: false 438 | Invert Rainbow: false 439 | Max Color: 255; 255; 255 440 | Min Color: 0; 0; 0 441 | Name: PointCloud2 442 | Position Transformer: XYZ 443 | Queue Size: 10 444 | Selectable: true 445 | Size (Pixels): 20 446 | Size (m): 0.009999999776482582 447 | Style: Points 448 | Topic: /cloud_ray_sub_map_fov 449 | Unreliable: false 450 | Use Fixed Frame: true 451 | Use rainbow: true 452 | Value: false 453 | - Angle Tolerance: 0 454 | Class: rviz_default_plugins/Odometry 455 | Covariance: 456 | Orientation: 457 | Alpha: 0.5 458 | Color: 255; 255; 127 459 | Color Style: Unique 460 | Frame: Local 461 | Offset: 1 462 | Scale: 1 463 | Value: true 464 | Position: 465 | Alpha: 0.30000001192092896 466 | Color: 204; 51; 204 467 | Scale: 1 468 | Value: true 469 | Value: true 470 | Enabled: false 471 | Keep: 1 472 | Name: Odometry 473 | Position Tolerance: 0 474 | Queue Size: 10 475 | Shape: 476 | Alpha: 1 477 | Axes Length: 0.699999988079071 478 | Axes Radius: 0.20000000298023224 479 | Color: 255; 25; 0 480 | Head Length: 0.30000001192092896 481 | Head Radius: 0.10000000149011612 482 | Shaft Length: 1 483 | Shaft Radius: 0.05000000074505806 484 | Value: Axes 485 | Topic: /aft_mapped_to_init 486 | Unreliable: false 487 | Value: false 488 | - Class: rviz_default_plugins/MarkerArray 489 | Enabled: false 490 | Marker Topic: /waypoint_planner/visualize 491 | Name: MarkerArray 492 | Namespaces: 493 | {} 494 | Queue Size: 100 495 | Value: false 496 | - Class: rviz_default_plugins/MarkerArray 497 | Enabled: true 498 | Marker Topic: /fsm_node/visualization/exp_traj 499 | Name: MarkerArray 500 | Namespaces: 501 | {} 502 | Queue Size: 100 503 | Value: true 504 | - Class: rviz_default_plugins/MarkerArray 505 | Enabled: false 506 | Marker Topic: /fsm_node/visualization/exp_sfcs 507 | Name: MarkerArray 508 | Namespaces: 509 | {} 510 | Queue Size: 100 511 | Value: false 512 | - Class: rviz_default_plugins/Image 513 | Enabled: true 514 | Image Topic: /rgb_img 515 | Max Value: 1 516 | Median window: 5 517 | Min Value: 0 518 | Name: Image 519 | Normalize Range: true 520 | Queue Size: 2 521 | Transport Hint: raw 522 | Unreliable: false 523 | Value: true 524 | Enabled: true 525 | Global Options: 526 | Background Color: 0; 0; 0 527 | Default Light: true 528 | Fixed Frame: camera_init 529 | Frame Rate: 30 530 | Name: root 531 | Tools: 532 | - Class: rviz_default_plugins/Interact 533 | Hide Inactive Objects: true 534 | - Class: rviz_default_plugins/MoveCamera 535 | - Class: rviz_default_plugins/Select 536 | - Class: rviz_default_plugins/FocusCamera 537 | - Class: rviz_default_plugins/Measure 538 | - Class: rviz_default_plugins/SetInitialPose 539 | Theta std deviation: 0.2617993950843811 540 | Topic: /initialpose 541 | X std deviation: 0.5 542 | Y std deviation: 0.5 543 | - Class: rviz_default_plugins/SetGoal 544 | Topic: /move_base_simple/goal 545 | - Class: rviz_default_plugins/PublishPoint 546 | Single click: true 547 | Topic: /clicked_point 548 | Value: true 549 | Views: 550 | Current: 551 | Class: rviz_default_plugins/ThirdPersonFollower 552 | Distance: 582.7694702148438 553 | Enable Stereo Rendering: 554 | Stereo Eye Separation: 0.05999999865889549 555 | Stereo Focal Distance: 1 556 | Swap Stereo Eyes: false 557 | Value: false 558 | Field of View: 0.7853981852531433 559 | Focal Point: 560 | X: 463.3948974609375 561 | Y: -4.546019554138184 562 | Z: -4.951948722009547e-05 563 | Focal Shape Fixed Size: false 564 | Focal Shape Size: 0.05000000074505806 565 | Invert Z Axis: false 566 | Name: Current View 567 | Near Clip Distance: 0.009999999776482582 568 | Pitch: -0.26979732513427734 569 | Target Frame: drone 570 | Yaw: 3.1317780017852783 571 | Saved: 572 | - Class: rviz_default_plugins/Orbit 573 | Distance: 117.53474426269531 574 | Enable Stereo Rendering: 575 | Stereo Eye Separation: 0.05999999865889549 576 | Stereo Focal Distance: 1 577 | Swap Stereo Eyes: false 578 | Value: false 579 | Field of View: 0.7853981852531433 580 | Focal Point: 581 | X: -35.713138580322266 582 | Y: 36.932613372802734 583 | Z: 4.459701061248779 584 | Focal Shape Fixed Size: false 585 | Focal Shape Size: 0.05000000074505806 586 | Invert Z Axis: false 587 | Name: far1 588 | Near Clip Distance: 0.009999999776482582 589 | Pitch: 0.19539840519428253 590 | Target Frame: 591 | Yaw: 0.17540442943572998 592 | - Class: rviz_default_plugins/Orbit 593 | Distance: 109.3125 594 | Enable Stereo Rendering: 595 | Stereo Eye Separation: 0.05999999865889549 596 | Stereo Focal Distance: 1 597 | Swap Stereo Eyes: false 598 | Value: false 599 | Field of View: 0.7853981852531433 600 | Focal Point: 601 | X: -22.092714309692383 602 | Y: 63.322662353515625 603 | Z: 14.125411987304688 604 | Focal Shape Fixed Size: false 605 | Focal Shape Size: 0.05000000074505806 606 | Invert Z Axis: false 607 | Name: far2 608 | Near Clip Distance: 0.009999999776482582 609 | Pitch: 0.035398442298173904 610 | Target Frame: 611 | Yaw: 5.793589115142822 612 | - Class: rviz_default_plugins/Orbit 613 | Distance: 85.65605163574219 614 | Enable Stereo Rendering: 615 | Stereo Eye Separation: 0.05999999865889549 616 | Stereo Focal Distance: 1 617 | Swap Stereo Eyes: false 618 | Value: false 619 | Field of View: 0.7853981852531433 620 | Focal Point: 621 | X: 28.252656936645508 622 | Y: -35.49672317504883 623 | Z: -36.31112289428711 624 | Focal Shape Fixed Size: false 625 | Focal Shape Size: 0.05000000074505806 626 | Invert Z Axis: false 627 | Name: near1 628 | Near Clip Distance: 0.009999999776482582 629 | Pitch: 0.5653983950614929 630 | Target Frame: 631 | Yaw: 0.9104044437408447 632 | - Class: rviz_default_plugins/Orbit 633 | Distance: 60.1053581237793 634 | Enable Stereo Rendering: 635 | Stereo Eye Separation: 0.05999999865889549 636 | Stereo Focal Distance: 1 637 | Swap Stereo Eyes: false 638 | Value: false 639 | Field of View: 0.7853981852531433 640 | Focal Point: 641 | X: 30.61589241027832 642 | Y: 29.98663330078125 643 | Z: -12.290168762207031 644 | Focal Shape Fixed Size: false 645 | Focal Shape Size: 0.05000000074505806 646 | Invert Z Axis: false 647 | Name: near2 648 | Near Clip Distance: 0.009999999776482582 649 | Pitch: 0.315398633480072 650 | Target Frame: 651 | Yaw: 5.788588047027588 652 | Window Geometry: 653 | Displays: 654 | collapsed: false 655 | Height: 1376 656 | Hide Left Dock: false 657 | Hide Right Dock: false 658 | Image: 659 | collapsed: false 660 | QMainWindow State: 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 661 | Selection: 662 | collapsed: false 663 | Time: 664 | collapsed: false 665 | Tool Properties: 666 | collapsed: false 667 | Views: 668 | collapsed: false 669 | Width: 2488 670 | X: 72 671 | Y: 27 672 | -------------------------------------------------------------------------------- /rviz_cfg/ntu_viral.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /Axes1 9 | - /mapping1 10 | - /mapping1/currPoints1 11 | - /mapping1/surround1 12 | - /mapping1/surround1/Autocompute Value Bounds1 13 | - /mapping1/PointCloud21 14 | - /Odometry1 15 | - /Odometry1/Odometry1 16 | - /Odometry1/Odometry1/Shape1 17 | - /Path1 18 | - /currPoints1/Autocompute Value Bounds1 19 | - /MarkerArray1/Namespaces1 20 | - /currPoints2/Autocompute Value Bounds1 21 | - /Odometry2/Shape1 22 | - /MarkerArray3 23 | - /MarkerArray4 24 | - /MarkerArray5 25 | - /Image1 26 | Splitter Ratio: 0.34272301197052 27 | Tree Height: 538 28 | - Class: rviz_common/Selection 29 | Name: Selection 30 | - Class: rviz_common/Tool Properties 31 | Expanded: 32 | - /2D Pose Estimate1 33 | - /2D Nav Goal1 34 | - /Publish Point1 35 | Name: Tool Properties 36 | Splitter Ratio: 0.5886790156364441 37 | - Class: rviz_common/Views 38 | Expanded: 39 | - /Current View1 40 | Name: Views 41 | Splitter Ratio: 0.5 42 | - Class: rviz_common/Time 43 | Name: Time 44 | SyncMode: 0 45 | SyncSource: surround 46 | Preferences: 47 | PromptSaveOnExit: true 48 | Toolbars: 49 | toolButtonStyle: 2 50 | Visualization Manager: 51 | Class: "" 52 | Displays: 53 | - Alpha: 1 54 | Cell Size: 1 55 | Class: rviz_default_plugins/Grid 56 | Color: 160; 160; 164 57 | Enabled: false 58 | Line Style: 59 | Line Width: 0.029999999329447746 60 | Value: Lines 61 | Name: Grid 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Plane: XY 68 | Plane Cell Count: 160 69 | Reference Frame: 70 | Value: false 71 | - Alpha: 1 72 | Class: rviz_default_plugins/Axes 73 | Enabled: true 74 | Length: 0.699999988079071 75 | Name: Axes 76 | Radius: 0.10000000149011612 77 | Reference Frame: 78 | Show Trail: false 79 | Value: true 80 | - Class: rviz_common/Group 81 | Displays: 82 | - Alpha: 1 83 | Autocompute Intensity Bounds: true 84 | Autocompute Value Bounds: 85 | Max Value: 20 86 | Min Value: -3 87 | Value: false 88 | Axis: Z 89 | Channel Name: intensity 90 | Class: rviz_default_plugins/PointCloud2 91 | Color: 239; 41; 41 92 | Color Transformer: FlatColor 93 | Decay Time: 0 94 | Enabled: true 95 | Invert Rainbow: false 96 | Max Color: 255; 255; 255 97 | Min Color: 0; 0; 0 98 | Name: currPoints 99 | Position Transformer: XYZ 100 | Queue Size: 100000 101 | Selectable: true 102 | Size (Pixels): 4 103 | Size (m): 0.009999999776482582 104 | Style: Points 105 | Topic: /cloud_registered 106 | Unreliable: false 107 | Use Fixed Frame: true 108 | Use rainbow: false 109 | Value: true 110 | - Alpha: 0.5 111 | Autocompute Intensity Bounds: false 112 | Autocompute Value Bounds: 113 | Max Value: 15 114 | Min Value: -5 115 | Value: false 116 | Axis: Z 117 | Channel Name: intensity 118 | Class: rviz_default_plugins/PointCloud2 119 | Color: 238; 238; 236 120 | Color Transformer: RGB8 121 | Decay Time: 10000 122 | Enabled: true 123 | Invert Rainbow: false 124 | Max Color: 255; 255; 255 125 | Max Intensity: 159 126 | Min Color: 0; 0; 0 127 | Min Intensity: 5 128 | Name: surround 129 | Position Transformer: XYZ 130 | Queue Size: 1 131 | Selectable: false 132 | Size (Pixels): 1 133 | Size (m): 0.004999999888241291 134 | Style: Points 135 | Topic: /cloud_registered 136 | Unreliable: true 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | - Alpha: 1 141 | Autocompute Intensity Bounds: true 142 | Autocompute Value Bounds: 143 | Max Value: 10 144 | Min Value: -10 145 | Value: true 146 | Axis: Z 147 | Channel Name: intensity 148 | Class: rviz_default_plugins/PointCloud2 149 | Color: 239; 41; 41 150 | Color Transformer: "" 151 | Decay Time: 0 152 | Enabled: false 153 | Invert Rainbow: false 154 | Max Color: 255; 255; 255 155 | Min Color: 0; 0; 0 156 | Name: PointCloud2 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 5 161 | Size (m): 0.019999999552965164 162 | Style: Squares 163 | Topic: /cloud_effected 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: false 168 | Enabled: true 169 | Name: mapping 170 | - Class: rviz_common/Group 171 | Displays: 172 | - Angle Tolerance: 0.009999999776482582 173 | Class: rviz_default_plugins/Odometry 174 | Covariance: 175 | Orientation: 176 | Alpha: 0.5 177 | Color: 255; 255; 127 178 | Color Style: Unique 179 | Frame: Local 180 | Offset: 1 181 | Scale: 1 182 | Value: true 183 | Position: 184 | Alpha: 0.30000001192092896 185 | Color: 204; 51; 204 186 | Scale: 1 187 | Value: true 188 | Value: true 189 | Enabled: true 190 | Keep: 1 191 | Name: Odometry 192 | Position Tolerance: 0.0010000000474974513 193 | Queue Size: 10 194 | Shape: 195 | Alpha: 1 196 | Axes Length: 0.5 197 | Axes Radius: 0.15000000596046448 198 | Color: 255; 85; 0 199 | Head Length: 0 200 | Head Radius: 0 201 | Shaft Length: 0.800000011920929 202 | Shaft Radius: 0.5 203 | Value: Axes 204 | Topic: /aft_mapped_to_init 205 | Unreliable: false 206 | Value: true 207 | Enabled: true 208 | Name: Odometry 209 | - Alpha: 0 210 | Buffer Length: 2 211 | Class: rviz_default_plugins/Path 212 | Color: 25; 255; 255 213 | Enabled: true 214 | Head Diameter: 0 215 | Head Length: 0 216 | Length: 0.30000001192092896 217 | Line Style: Billboards 218 | Line Width: 0.03999999910593033 219 | Name: Path 220 | Offset: 221 | X: 0 222 | Y: 0 223 | Z: 0 224 | Pose Color: 25; 255; 255 225 | Pose Style: None 226 | Queue Size: 10 227 | Radius: 0.029999999329447746 228 | Shaft Diameter: 0.4000000059604645 229 | Shaft Length: 0.4000000059604645 230 | Topic: /path 231 | Unreliable: false 232 | Value: true 233 | - Alpha: 0.10000000149011612 234 | Autocompute Intensity Bounds: true 235 | Autocompute Value Bounds: 236 | Max Value: 20 237 | Min Value: -3 238 | Value: false 239 | Axis: Z 240 | Channel Name: intensity 241 | Class: rviz_default_plugins/PointCloud2 242 | Color: 239; 41; 41 243 | Color Transformer: Intensity 244 | Decay Time: 1000 245 | Enabled: false 246 | Invert Rainbow: true 247 | Max Color: 255; 255; 255 248 | Min Color: 0; 0; 0 249 | Name: currPoints 250 | Position Transformer: XYZ 251 | Queue Size: 1 252 | Selectable: true 253 | Size (Pixels): 2 254 | Size (m): 0.009999999776482582 255 | Style: Points 256 | Topic: /cloud_voxel 257 | Unreliable: false 258 | Use Fixed Frame: true 259 | Use rainbow: true 260 | Value: false 261 | - Class: rviz_default_plugins/Marker 262 | Enabled: true 263 | Marker Topic: /planner_normal 264 | Name: Marker 265 | Namespaces: 266 | {} 267 | Queue Size: 100 268 | Value: true 269 | - Class: rviz_default_plugins/MarkerArray 270 | Enabled: false 271 | Marker Topic: /voxels 272 | Name: MarkerArray 273 | Namespaces: 274 | {} 275 | Queue Size: 100 276 | Value: false 277 | - Alpha: 1 278 | Autocompute Intensity Bounds: true 279 | Autocompute Value Bounds: 280 | Max Value: 10 281 | Min Value: -10 282 | Value: true 283 | Axis: Z 284 | Channel Name: intensity 285 | Class: rviz_default_plugins/PointCloud2 286 | Color: 245; 121; 0 287 | Color Transformer: FlatColor 288 | Decay Time: 0 289 | Enabled: false 290 | Invert Rainbow: false 291 | Max Color: 255; 255; 255 292 | Min Color: 0; 0; 0 293 | Name: PointCloud2 294 | Position Transformer: XYZ 295 | Queue Size: 1 296 | Selectable: true 297 | Size (Pixels): 15 298 | Size (m): 0.009999999776482582 299 | Style: Points 300 | Topic: /cloud_ray_sub_map 301 | Unreliable: false 302 | Use Fixed Frame: true 303 | Use rainbow: true 304 | Value: false 305 | - Class: rviz_default_plugins/MarkerArray 306 | Enabled: false 307 | Marker Topic: /visualization_marker 308 | Name: MarkerArray 309 | Namespaces: 310 | {} 311 | Queue Size: 100 312 | Value: false 313 | - Alpha: 1 314 | Autocompute Intensity Bounds: true 315 | Autocompute Value Bounds: 316 | Max Value: 10 317 | Min Value: -10 318 | Value: true 319 | Axis: Z 320 | Channel Name: intensity 321 | Class: rviz_default_plugins/PointCloud2 322 | Color: 92; 53; 102 323 | Color Transformer: FlatColor 324 | Decay Time: 99999 325 | Enabled: false 326 | Invert Rainbow: false 327 | Max Color: 255; 255; 255 328 | Min Color: 0; 0; 0 329 | Name: PointCloud2 330 | Position Transformer: XYZ 331 | Queue Size: 10 332 | Selectable: true 333 | Size (Pixels): 10 334 | Size (m): 0.009999999776482582 335 | Style: Points 336 | Topic: /cloud_visual_map 337 | Unreliable: false 338 | Use Fixed Frame: true 339 | Use rainbow: true 340 | Value: false 341 | - Alpha: 1 342 | Autocompute Intensity Bounds: true 343 | Autocompute Value Bounds: 344 | Max Value: 10 345 | Min Value: -10 346 | Value: true 347 | Axis: Z 348 | Channel Name: intensity 349 | Class: rviz_default_plugins/PointCloud2 350 | Color: 115; 210; 22 351 | Color Transformer: FlatColor 352 | Decay Time: 0 353 | Enabled: false 354 | Invert Rainbow: false 355 | Max Color: 255; 255; 255 356 | Min Color: 0; 0; 0 357 | Name: surround 358 | Position Transformer: XYZ 359 | Queue Size: 1 360 | Selectable: false 361 | Size (Pixels): 12 362 | Size (m): 0.05000000074505806 363 | Style: Points 364 | Topic: /cloud_visual_sub_map 365 | Unreliable: true 366 | Use Fixed Frame: true 367 | Use rainbow: true 368 | Value: false 369 | - Alpha: 1 370 | Autocompute Intensity Bounds: true 371 | Autocompute Value Bounds: 372 | Max Value: 20 373 | Min Value: -3 374 | Value: false 375 | Axis: Z 376 | Channel Name: intensity 377 | Class: rviz_default_plugins/PointCloud2 378 | Color: 237; 212; 0 379 | Color Transformer: FlatColor 380 | Decay Time: 0 381 | Enabled: false 382 | Invert Rainbow: false 383 | Max Color: 255; 255; 255 384 | Min Color: 0; 0; 0 385 | Name: currPoints 386 | Position Transformer: XYZ 387 | Queue Size: 100000 388 | Selectable: true 389 | Size (Pixels): 5 390 | Size (m): 0.009999999776482582 391 | Style: Points 392 | Topic: /cloud_sample_points 393 | Unreliable: false 394 | Use Fixed Frame: true 395 | Use rainbow: false 396 | Value: false 397 | - Alpha: 1 398 | Autocompute Intensity Bounds: true 399 | Autocompute Value Bounds: 400 | Max Value: 10 401 | Min Value: -10 402 | Value: true 403 | Axis: Z 404 | Channel Name: intensity 405 | Class: rviz_default_plugins/PointCloud2 406 | Color: 239; 41; 41 407 | Color Transformer: FlatColor 408 | Decay Time: 99999 409 | Enabled: false 410 | Invert Rainbow: false 411 | Max Color: 255; 255; 255 412 | Min Color: 239; 41; 41 413 | Name: PointCloud2 414 | Position Transformer: XYZ 415 | Queue Size: 10 416 | Selectable: true 417 | Size (Pixels): 4 418 | Size (m): 0.009999999776482582 419 | Style: Points 420 | Topic: /cloud_visual_map 421 | Unreliable: false 422 | Use Fixed Frame: true 423 | Use rainbow: true 424 | Value: false 425 | - Alpha: 1 426 | Autocompute Intensity Bounds: true 427 | Autocompute Value Bounds: 428 | Max Value: 10 429 | Min Value: -10 430 | Value: true 431 | Axis: Z 432 | Channel Name: intensity 433 | Class: rviz_default_plugins/PointCloud2 434 | Color: 92; 53; 102 435 | Color Transformer: FlatColor 436 | Decay Time: 0 437 | Enabled: false 438 | Invert Rainbow: false 439 | Max Color: 255; 255; 255 440 | Min Color: 0; 0; 0 441 | Name: PointCloud2 442 | Position Transformer: XYZ 443 | Queue Size: 10 444 | Selectable: true 445 | Size (Pixels): 20 446 | Size (m): 0.009999999776482582 447 | Style: Points 448 | Topic: /cloud_ray_sub_map_fov 449 | Unreliable: false 450 | Use Fixed Frame: true 451 | Use rainbow: true 452 | Value: false 453 | - Angle Tolerance: 0 454 | Class: rviz_default_plugins/Odometry 455 | Covariance: 456 | Orientation: 457 | Alpha: 0.5 458 | Color: 255; 255; 127 459 | Color Style: Unique 460 | Frame: Local 461 | Offset: 1 462 | Scale: 1 463 | Value: true 464 | Position: 465 | Alpha: 0.30000001192092896 466 | Color: 204; 51; 204 467 | Scale: 1 468 | Value: true 469 | Value: true 470 | Enabled: false 471 | Keep: 1 472 | Name: Odometry 473 | Position Tolerance: 0 474 | Queue Size: 10 475 | Shape: 476 | Alpha: 1 477 | Axes Length: 0.699999988079071 478 | Axes Radius: 0.20000000298023224 479 | Color: 255; 25; 0 480 | Head Length: 0.30000001192092896 481 | Head Radius: 0.10000000149011612 482 | Shaft Length: 1 483 | Shaft Radius: 0.05000000074505806 484 | Value: Axes 485 | Topic: /aft_mapped_to_init 486 | Unreliable: false 487 | Value: false 488 | - Class: rviz_default_plugins/MarkerArray 489 | Enabled: false 490 | Marker Topic: /waypoint_planner/visualize 491 | Name: MarkerArray 492 | Namespaces: 493 | {} 494 | Queue Size: 100 495 | Value: false 496 | - Class: rviz_default_plugins/MarkerArray 497 | Enabled: true 498 | Marker Topic: /fsm_node/visualization/exp_traj 499 | Name: MarkerArray 500 | Namespaces: 501 | {} 502 | Queue Size: 100 503 | Value: true 504 | - Class: rviz_default_plugins/MarkerArray 505 | Enabled: false 506 | Marker Topic: /fsm_node/visualization/exp_sfcs 507 | Name: MarkerArray 508 | Namespaces: 509 | {} 510 | Queue Size: 100 511 | Value: false 512 | - Class: rviz_default_plugins/Image 513 | Enabled: true 514 | Image Topic: /rgb_img 515 | Max Value: 1 516 | Median window: 5 517 | Min Value: 0 518 | Name: Image 519 | Normalize Range: true 520 | Queue Size: 2 521 | Transport Hint: raw 522 | Unreliable: false 523 | Value: true 524 | Enabled: true 525 | Global Options: 526 | Background Color: 0; 0; 0 527 | Default Light: true 528 | Fixed Frame: camera_init 529 | Frame Rate: 30 530 | Name: root 531 | Tools: 532 | - Class: rviz_default_plugins/Interact 533 | Hide Inactive Objects: true 534 | - Class: rviz_default_plugins/MoveCamera 535 | - Class: rviz_default_plugins/Select 536 | - Class: rviz_default_plugins/FocusCamera 537 | - Class: rviz_default_plugins/Measure 538 | - Class: rviz_default_plugins/SetInitialPose 539 | Theta std deviation: 0.2617993950843811 540 | Topic: /initialpose 541 | X std deviation: 0.5 542 | Y std deviation: 0.5 543 | - Class: rviz_default_plugins/SetGoal 544 | Topic: /move_base_simple/goal 545 | - Class: rviz_default_plugins/PublishPoint 546 | Single click: true 547 | Topic: /clicked_point 548 | Value: true 549 | Views: 550 | Current: 551 | Class: rviz_default_plugins/ThirdPersonFollower 552 | Distance: 65.96137237548828 553 | Enable Stereo Rendering: 554 | Stereo Eye Separation: 0.05999999865889549 555 | Stereo Focal Distance: 1 556 | Swap Stereo Eyes: false 557 | Value: false 558 | Field of View: 0.7853981852531433 559 | Focal Point: 560 | X: -2.06162166595459 561 | Y: 2.7847142219543457 562 | Z: -2.219532325398177e-05 563 | Focal Shape Fixed Size: false 564 | Focal Shape Size: 0.05000000074505806 565 | Invert Z Axis: true 566 | Name: Current View 567 | Near Clip Distance: 0.009999999776482582 568 | Pitch: 1.4797966480255127 569 | Target Frame: drone 570 | Yaw: 3.251800537109375 571 | Saved: 572 | - Class: rviz_default_plugins/Orbit 573 | Distance: 117.53474426269531 574 | Enable Stereo Rendering: 575 | Stereo Eye Separation: 0.05999999865889549 576 | Stereo Focal Distance: 1 577 | Swap Stereo Eyes: false 578 | Value: false 579 | Field of View: 0.7853981852531433 580 | Focal Point: 581 | X: -35.713138580322266 582 | Y: 36.932613372802734 583 | Z: 4.459701061248779 584 | Focal Shape Fixed Size: false 585 | Focal Shape Size: 0.05000000074505806 586 | Invert Z Axis: false 587 | Name: far1 588 | Near Clip Distance: 0.009999999776482582 589 | Pitch: 0.19539840519428253 590 | Target Frame: 591 | Yaw: 0.17540442943572998 592 | - Class: rviz_default_plugins/Orbit 593 | Distance: 109.3125 594 | Enable Stereo Rendering: 595 | Stereo Eye Separation: 0.05999999865889549 596 | Stereo Focal Distance: 1 597 | Swap Stereo Eyes: false 598 | Value: false 599 | Field of View: 0.7853981852531433 600 | Focal Point: 601 | X: -22.092714309692383 602 | Y: 63.322662353515625 603 | Z: 14.125411987304688 604 | Focal Shape Fixed Size: false 605 | Focal Shape Size: 0.05000000074505806 606 | Invert Z Axis: false 607 | Name: far2 608 | Near Clip Distance: 0.009999999776482582 609 | Pitch: 0.035398442298173904 610 | Target Frame: 611 | Yaw: 5.793589115142822 612 | - Class: rviz_default_plugins/Orbit 613 | Distance: 85.65605163574219 614 | Enable Stereo Rendering: 615 | Stereo Eye Separation: 0.05999999865889549 616 | Stereo Focal Distance: 1 617 | Swap Stereo Eyes: false 618 | Value: false 619 | Field of View: 0.7853981852531433 620 | Focal Point: 621 | X: 28.252656936645508 622 | Y: -35.49672317504883 623 | Z: -36.31112289428711 624 | Focal Shape Fixed Size: false 625 | Focal Shape Size: 0.05000000074505806 626 | Invert Z Axis: false 627 | Name: near1 628 | Near Clip Distance: 0.009999999776482582 629 | Pitch: 0.5653983950614929 630 | Target Frame: 631 | Yaw: 0.9104044437408447 632 | - Class: rviz_default_plugins/Orbit 633 | Distance: 60.1053581237793 634 | Enable Stereo Rendering: 635 | Stereo Eye Separation: 0.05999999865889549 636 | Stereo Focal Distance: 1 637 | Swap Stereo Eyes: false 638 | Value: false 639 | Field of View: 0.7853981852531433 640 | Focal Point: 641 | X: 30.61589241027832 642 | Y: 29.98663330078125 643 | Z: -12.290168762207031 644 | Focal Shape Fixed Size: false 645 | Focal Shape Size: 0.05000000074505806 646 | Invert Z Axis: false 647 | Name: near2 648 | Near Clip Distance: 0.009999999776482582 649 | Pitch: 0.315398633480072 650 | Target Frame: 651 | Yaw: 5.788588047027588 652 | Window Geometry: 653 | Displays: 654 | collapsed: false 655 | Height: 1376 656 | Hide Left Dock: false 657 | Hide Right Dock: false 658 | Image: 659 | collapsed: false 660 | QMainWindow State: 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 661 | Selection: 662 | collapsed: false 663 | Time: 664 | collapsed: false 665 | Tool Properties: 666 | collapsed: false 667 | Views: 668 | collapsed: false 669 | Width: 2488 670 | X: 72 671 | Y: 27 672 | -------------------------------------------------------------------------------- /scripts/colmap_output.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | TARGET_DIRS=( 4 | "$(rospack find fast_livo)/Log/Colmap/images" 5 | "$(rospack find fast_livo)/Log/Colmap/sparse/0" 6 | ) 7 | 8 | for dir in "${TARGET_DIRS[@]}"; do 9 | if [ -d "$dir" ]; then 10 | rm -rf "$dir" 11 | echo "Removed: $dir" 12 | else 13 | echo "Not found: $dir" 14 | fi 15 | done 16 | 17 | for dir in "${TARGET_DIRS[@]}"; do 18 | if [ ! -d "$dir" ]; then 19 | mkdir -p "$dir" 20 | echo "Created: $dir" 21 | else 22 | echo "Exists: $dir" 23 | fi 24 | done 25 | 26 | -------------------------------------------------------------------------------- /scripts/mesh.py: -------------------------------------------------------------------------------- 1 | import os 2 | import glob 3 | import vdbfusion 4 | import numpy as np 5 | import open3d as o3d 6 | from scipy.spatial import KDTree 7 | 8 | # ------------------------- Key Parameters ------------------------- 9 | # Dataset path 10 | SOURCE_DIR = "/home/chunran/Downloads/" # Replace with your dataset path 11 | 12 | # VDBVolume parameters 13 | VOXEL_SIZE = 0.02 # Voxel size (smaller values increase precision but require more memory) 14 | SDF_TRUNC = 0.1 # Truncation distance for SDF (affects surface thickness, typically a multiple of voxel size) 15 | MIN_WEIGHT = 0.1 # Minimum weight for mesh extraction (filters out noisy voxels) 16 | 17 | # ------------------------- Dataset Class ------------------------- 18 | class Dataset: 19 | def __init__(self, folder: str): 20 | super().__init__() 21 | # Get all .pcd files in the folder 22 | self.scan_files = glob.glob(os.path.join(folder, "*.pcd")) 23 | # Initialize poses as identity matrices 24 | self.poses = np.array([np.eye(4) for _ in range(len(self.scan_files))]) 25 | 26 | def __getitem__(self, idx): 27 | if idx >= len(self.scan_files): 28 | raise IndexError("Index out of range") 29 | 30 | # Compute relative pose 31 | pose = np.linalg.inv(self.poses[0]) @ self.poses[idx] 32 | # Read point cloud 33 | points, colors = self.read_pcd(self.scan_files[idx]) 34 | points = np.array(points, dtype=np.float64) 35 | return points, colors, pose 36 | 37 | def __len__(self): 38 | return len(self.scan_files) 39 | 40 | def read_pcd(self, pcd_file): 41 | # Read .pcd file using Open3D 42 | pcd = o3d.io.read_point_cloud(pcd_file) 43 | # Extract point cloud coordinates 44 | points = np.asarray(pcd.points) 45 | # Extract colors (if available) 46 | if pcd.has_colors(): 47 | colors = np.asarray(pcd.colors) # Open3D colors are in range [0, 1] 48 | else: 49 | colors = np.zeros_like(points) # If no colors, fill with zeros 50 | return points, colors 51 | 52 | # ------------------------- Main Program ------------------------- 53 | if __name__ == '__main__': 54 | # Initialize VDBVolume 55 | print("Initializing VDBVolume...") 56 | vdb_volume = vdbfusion.VDBVolume(voxel_size=VOXEL_SIZE, sdf_trunc=SDF_TRUNC) 57 | 58 | # Load dataset 59 | print("Loading dataset from", SOURCE_DIR) 60 | dataset = Dataset(SOURCE_DIR) 61 | 62 | # Integrate all point clouds into the VDBVolume 63 | print("Integrating point clouds into VDBVolume...") 64 | for i in range(len(dataset)): 65 | scan, colors, origin = dataset[i] 66 | vdb_volume.integrate(scan, origin) 67 | 68 | print("Point cloud integration complete!") 69 | 70 | # Extract triangle mesh 71 | print("Extracting triangle mesh...") 72 | vert, tri = vdb_volume.extract_triangle_mesh(min_weight=MIN_WEIGHT) 73 | 74 | # Create Open3D mesh object 75 | print("Creating Open3D mesh object...") 76 | mesh = o3d.geometry.TriangleMesh( 77 | o3d.utility.Vector3dVector(vert), 78 | o3d.utility.Vector3iVector(tri), 79 | ) 80 | 81 | # Save the mesh 82 | print("Saving the mesh to output_mesh.ply...") 83 | o3d.io.write_triangle_mesh("mesh.ply", mesh) 84 | print("Mesh saved successfully.") 85 | 86 | # ------------------------- Colorize Mesh Vertices ------------------------- 87 | print("Starting mesh colorization...") 88 | if dataset[0][1] is not None: # Check if color information exists 89 | # Combine all point cloud points and colors 90 | pcd_points = np.vstack([dataset[i][0] for i in range(len(dataset))]) 91 | pcd_colors = np.vstack([dataset[i][1] for i in range(len(dataset))]) 92 | 93 | # Use KDTree to find the nearest point for each vertex 94 | kdtree = KDTree(pcd_points) 95 | _, indices = kdtree.query(vert) # Find the nearest point cloud point for each vertex 96 | vertex_colors = pcd_colors[indices] # Assign colors 97 | 98 | # Set mesh vertex colors 99 | mesh.vertex_colors = o3d.utility.Vector3dVector(vertex_colors) 100 | 101 | print("Mesh colorization complete!") 102 | 103 | # Compute vertex normals 104 | print("Computing vertex normals...") 105 | mesh.compute_vertex_normals() 106 | 107 | # Save the textured mesh 108 | print("Saving the textured mesh to textured_mesh.ply...") 109 | o3d.io.write_triangle_mesh("textured_mesh.ply", mesh) 110 | print("Textured mesh saved successfully.") 111 | 112 | # Visualize the final colorized mesh 113 | print("Visualizing the colorized mesh...") 114 | o3d.visualization.draw_geometries([mesh]) 115 | -------------------------------------------------------------------------------- /src/frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #include 14 | #include "feature.h" 15 | #include "frame.h" 16 | #include "visual_point.h" 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | int Frame::frame_counter_ = 0; 23 | 24 | Frame::Frame(vk::AbstractCamera *cam, const cv::Mat &img) 25 | : id_(frame_counter_++), 26 | cam_(cam) 27 | { 28 | initFrame(img); 29 | } 30 | 31 | Frame::~Frame() 32 | { 33 | std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; }); 34 | } 35 | 36 | void Frame::initFrame(const cv::Mat &img) 37 | { 38 | if (img.empty()) { throw std::runtime_error("Frame: provided image is empty"); } 39 | 40 | if (img.cols != cam_->width() || img.rows != cam_->height()) 41 | { 42 | throw std::runtime_error("Frame: provided image has not the same size as the camera model"); 43 | } 44 | 45 | if (img.type() != CV_8UC1) { throw std::runtime_error("Frame: provided image is not grayscale"); } 46 | 47 | img_ = img; 48 | } 49 | 50 | /// Utility functions for the Frame class 51 | namespace frame_utils 52 | { 53 | 54 | void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr) 55 | { 56 | pyr.resize(n_levels); 57 | pyr[0] = img_level_0; 58 | for (int i = 1; i < n_levels; ++i) 59 | { 60 | pyr[i] = cv::Mat(pyr[i - 1].rows / 2, pyr[i - 1].cols / 2, CV_8U); 61 | vk::halfSample(pyr[i - 1], pyr[i]); 62 | } 63 | } 64 | 65 | } // namespace frame_utils 66 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "LIVMapper.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | rclcpp::init(argc, argv); 6 | rclcpp::NodeOptions options; 7 | rclcpp::Node::SharedPtr nh; 8 | image_transport::ImageTransport it_(nh); 9 | LIVMapper mapper(nh, "laserMapping"); 10 | mapper.initializeSubscribersAndPublishers(nh, it_); 11 | mapper.run(nh); 12 | rclcpp::shutdown(); 13 | return 0; 14 | } -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | // utils.cpp 2 | #include 3 | #include // for int64_t 4 | #include // for std::numeric_limits 5 | #include // for std::out_of_range 6 | 7 | std::vector convertToIntVectorSafe(const std::vector& int64_vector) { 8 | std::vector int_vector; 9 | int_vector.reserve(int64_vector.size()); // 预留空间以提高效率 10 | 11 | for (int64_t value : int64_vector) { 12 | if (value < std::numeric_limits::min() || value > std::numeric_limits::max()) { 13 | throw std::out_of_range("Value is out of range for int"); 14 | } 15 | int_vector.push_back(static_cast(value)); 16 | } 17 | 18 | return int_vector; 19 | } 20 | -------------------------------------------------------------------------------- /src/visual_point.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. 3 | 4 | Developer: Chunran Zheng 5 | 6 | For commercial use, please contact me at or 7 | Prof. Fu Zhang at . 8 | 9 | This file is subject to the terms and conditions outlined in the 'LICENSE' file, 10 | which is included as part of this source code package. 11 | */ 12 | 13 | #include "visual_point.h" 14 | #include "feature.h" 15 | #include 16 | #include 17 | 18 | VisualPoint::VisualPoint(const Vector3d &pos) 19 | : pos_(pos), previous_normal_(Vector3d::Zero()), normal_(Vector3d::Zero()), 20 | is_converged_(false), is_normal_initialized_(false), has_ref_patch_(false) 21 | { 22 | } 23 | 24 | VisualPoint::~VisualPoint() 25 | { 26 | for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) 27 | { 28 | delete(*it); 29 | } 30 | obs_.clear(); 31 | ref_patch = nullptr; 32 | } 33 | 34 | void VisualPoint::addFrameRef(Feature *ftr) 35 | { 36 | obs_.push_front(ftr); 37 | } 38 | 39 | void VisualPoint::deleteFeatureRef(Feature *ftr) 40 | { 41 | if (ref_patch == ftr) 42 | { 43 | ref_patch = nullptr; 44 | has_ref_patch_ = false; 45 | } 46 | for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) 47 | { 48 | if ((*it) == ftr) 49 | { 50 | delete((*it)); 51 | obs_.erase(it); 52 | return; 53 | } 54 | } 55 | } 56 | 57 | bool VisualPoint::getCloseViewObs(const Vector3d &framepos, Feature *&ftr, const Vector2d &cur_px) const 58 | { 59 | // TODO: get frame with same point of view AND same pyramid level! 60 | if (obs_.size() <= 0) return false; 61 | 62 | Vector3d obs_dir(framepos - pos_); 63 | obs_dir.normalize(); 64 | auto min_it = obs_.begin(); 65 | double min_cos_angle = 0; 66 | for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) 67 | { 68 | Vector3d dir((*it)->T_f_w_.inverse().translation() - pos_); 69 | dir.normalize(); 70 | double cos_angle = obs_dir.dot(dir); 71 | if (cos_angle > min_cos_angle) 72 | { 73 | min_cos_angle = cos_angle; 74 | min_it = it; 75 | } 76 | } 77 | ftr = *min_it; 78 | 79 | // Vector2d ftr_px = ftr->px_; 80 | // double pixel_dist = (cur_px-ftr_px).norm(); 81 | 82 | // if(pixel_dist > 200) 83 | // { 84 | // ROS_ERROR("The pixel dist exceeds 200."); 85 | // return false; 86 | // } 87 | 88 | if (min_cos_angle < 0.5) // assume that observations larger than 60° are useless 0.5 89 | { 90 | // ROS_ERROR("The obseved angle is larger than 60°."); 91 | return false; 92 | } 93 | 94 | return true; 95 | } 96 | 97 | void VisualPoint::findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const 98 | { 99 | auto min_it = obs_.begin(); 100 | float min_score = std::numeric_limits::max(); 101 | 102 | for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) 103 | { 104 | if ((*it)->score_ < min_score) 105 | { 106 | min_score = (*it)->score_; 107 | min_it = it; 108 | } 109 | } 110 | ftr = *min_it; 111 | } 112 | 113 | void VisualPoint::deleteNonRefPatchFeatures() 114 | { 115 | for (auto it = obs_.begin(); it != obs_.end();) 116 | { 117 | if (*it != ref_patch) 118 | { 119 | delete *it; 120 | it = obs_.erase(it); 121 | } 122 | else 123 | { 124 | ++it; 125 | } 126 | } 127 | } --------------------------------------------------------------------------------