├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── Log ├── fast_lio_time_log_analysis.m ├── guide.md └── plot.py ├── PCD └── 1 ├── README.md ├── config ├── avia.yaml ├── horizon.yaml ├── ouster64.yaml └── velodyne.yaml ├── doc ├── demo.GIF ├── demo_accu.GIF ├── demo_init.GIF └── demo_init_2.GIF ├── include ├── Exp_mat.h ├── IKFoM_toolkit │ ├── esekfom │ │ ├── esekfom.hpp │ │ └── util.hpp │ └── mtk │ │ ├── build_manifold.hpp │ │ ├── src │ │ ├── SubManifold.hpp │ │ ├── mtkmath.hpp │ │ └── vectview.hpp │ │ ├── startIdx.hpp │ │ └── types │ │ ├── S2.hpp │ │ ├── SOn.hpp │ │ ├── vect.hpp │ │ └── wrapped_cv_mat.hpp ├── common_lib.h ├── matplotlibcpp.h ├── so3_math.h └── use-ikfom.hpp ├── launch ├── gdb_debug_example.launch ├── localization_avia.launch ├── localization_horizon.launch ├── localization_ouster64.launch └── localization_velodyne.launch ├── msg └── Pose6D.msg ├── package.xml ├── rviz_cfg ├── .gitignore ├── loam_livox.rviz └── localization.rviz ├── scripts ├── global_localization.py ├── publish_initial_pose.py └── transform_fusion.py └── src ├── IMU_Processing.hpp ├── laserMapping.cpp ├── preprocess.cpp └── preprocess.h /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | Log/*.png 3 | Log/*.txt 4 | Log/*.csv 5 | Log/*.pdf 6 | .vscode/c_cpp_properties.json 7 | .vscode/settings.json 8 | PCD/*.pcd 9 | .idea 10 | cmake-build-debug -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "include/ikd-Tree"] 2 | path = include/ikd-Tree 3 | url = https://github.com/hku-mars/ikd-Tree.git 4 | branch = fast_lio 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fast_lio_localization) 3 | 4 | SET(CMAKE_BUILD_TYPE "Debug") 5 | 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 9 | 10 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 13 | set(CMAKE_CXX_STANDARD 14) 14 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 15 | set(CMAKE_CXX_EXTENSIONS OFF) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 17 | 18 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 19 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 20 | include(ProcessorCount) 21 | ProcessorCount(N) 22 | message("Processer number: ${N}") 23 | if(N GREATER 4) 24 | add_definitions(-DMP_EN) 25 | add_definitions(-DMP_PROC_NUM=3) 26 | message("core for MP: 3") 27 | elseif(N GREATER 3) 28 | add_definitions(-DMP_EN) 29 | add_definitions(-DMP_PROC_NUM=2) 30 | message("core for MP: 2") 31 | else() 32 | add_definitions(-DMP_PROC_NUM=1) 33 | endif() 34 | else() 35 | add_definitions(-DMP_PROC_NUM=1) 36 | endif() 37 | 38 | find_package(OpenMP QUIET) 39 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 40 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 41 | 42 | find_package(PythonLibs REQUIRED) 43 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") 44 | 45 | find_package(catkin REQUIRED COMPONENTS 46 | geometry_msgs 47 | nav_msgs 48 | sensor_msgs 49 | roscpp 50 | rospy 51 | std_msgs 52 | pcl_ros 53 | tf 54 | livox_ros_driver 55 | message_generation 56 | eigen_conversions 57 | ) 58 | 59 | find_package(Eigen3 REQUIRED) 60 | find_package(PCL 1.8 REQUIRED) 61 | 62 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 63 | 64 | include_directories( 65 | ${catkin_INCLUDE_DIRS} 66 | ${EIGEN3_INCLUDE_DIR} 67 | ${PCL_INCLUDE_DIRS} 68 | ${PYTHON_INCLUDE_DIRS} 69 | include) 70 | 71 | add_message_files( 72 | FILES 73 | Pose6D.msg 74 | ) 75 | 76 | generate_messages( 77 | DEPENDENCIES 78 | geometry_msgs 79 | ) 80 | 81 | catkin_package( 82 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 83 | DEPENDS EIGEN3 PCL 84 | INCLUDE_DIRS 85 | ) 86 | 87 | add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) 88 | target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) 89 | target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /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/fast_lio_time_log_analysis.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | Color_red = [0.6350 0.0780 0.1840]; 5 | Color_blue = [0 0.4470 0.7410]; 6 | Color_orange = [0.8500 0.3250 0.0980]; 7 | Color_green = [0.4660 0.6740 0.1880]; 8 | Color_lightblue = [0.3010 0.7450 0.9330]; 9 | Color_purple = [0.4940 0.1840 0.5560]; 10 | Color_yellow = [0.9290 0.6940 0.1250]; 11 | 12 | fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0); 13 | timestamp_ikd = fast_lio_ikdtree(:,1); 14 | timestamp_ikd = timestamp_ikd - min(timestamp_ikd); 15 | total_time_ikd = fast_lio_ikdtree(:,2)*1e3; 16 | scan_num = fast_lio_ikdtree(:,3); 17 | incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3; 18 | search_time_ikd = fast_lio_ikdtree(:,5)*1e3; 19 | delete_size_ikd = fast_lio_ikdtree(:,6); 20 | delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3; 21 | tree_size_ikd_st = fast_lio_ikdtree(:,8); 22 | tree_size_ikd = fast_lio_ikdtree(:,9); 23 | add_points = fast_lio_ikdtree(:,10); 24 | 25 | fast_lio_forest = csvread("fast_lio_time_log.csv",1,0); 26 | fov_check_time_forest = fast_lio_forest(:,5)*1e3; 27 | average_time_forest = fast_lio_forest(:,2)*1e3; 28 | total_time_forest = fast_lio_forest(:,6)*1e3; 29 | incremental_time_forest = fast_lio_forest(:,3)*1e3; 30 | search_time_forest = fast_lio_forest(:,4)*1e3; 31 | timestamp_forest = fast_lio_forest(:,1); 32 | 33 | % Use slide window to calculate average 34 | L = 1; % Length of slide window 35 | for i = 1:length(timestamp_ikd) 36 | if (i 0); 78 | search_time_ikd = search_time_ikd(index_ikd); 79 | index_forest = find(search_time_forest > 0); 80 | search_time_forest = search_time_forest(index_forest); 81 | 82 | t = nexttile; 83 | hold on; 84 | boxplot_data_ikd = [incremental_time_ikd,total_time_ikd]; 85 | boxplot_data_forest = [incremental_time_forest,total_time_forest]; 86 | Colors_ikd = [Color_blue;Color_blue;Color_blue]; 87 | Colors_forest = [Color_orange;Color_orange;Color_orange]; 88 | % xticks([3,8,13]) 89 | h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3); 90 | h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3); 91 | h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3); 92 | h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3); 93 | ax2 = gca; 94 | ax2.YAxis.Scale = 'log'; 95 | xlim([0.5,6.0]) 96 | ylim([0.0008,100]) 97 | xticks([1.25 3.25 5.25]) 98 | xticklabels({'Nearest Search',' Incremental Updates','Total Time'}); 99 | yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2]) 100 | ax2.YAxis.FontSize = 12; 101 | ax2.XAxis.FontSize = 14.5; 102 | % ax.XAxis.FontWeight = 'bold'; 103 | ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman') 104 | box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')]; 105 | for j=1:length(box_vars) 106 | if (j<=3) 107 | Color = Color_blue; 108 | else 109 | Color = Color_orange; 110 | end 111 | patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color); 112 | end 113 | Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman'); 114 | grid on 115 | set(gca,'YMinorGrid','off') 116 | nexttile; 117 | hold on; 118 | grid on; 119 | box on; 120 | set(gca,'FontSize',12,'FontName','Times New Roman') 121 | plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2); 122 | plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2); 123 | plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2); 124 | lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman') 125 | title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman') 126 | xlabel("time/s",'FontSize',16,'FontName','Times New Roman') 127 | yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1]) 128 | xlim([32,390]); 129 | ylim([0,0.85]); 130 | ax3 = gca; 131 | ax3.YAxis.FontSize = 12; 132 | ax3.XAxis.FontSize = 12; 133 | % print('./Figures/fastlio_exp_combine','-depsc','-r1200') 134 | % exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector') 135 | 136 | -------------------------------------------------------------------------------- /Log/guide.md: -------------------------------------------------------------------------------- 1 | Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. 2 | -------------------------------------------------------------------------------- /Log/plot.py: -------------------------------------------------------------------------------- 1 | # import matplotlib 2 | # matplotlib.use('Agg') 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | #######for ikfom 8 | fig, axs = plt.subplots(4,2) 9 | lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] 10 | lab_out = ['', 'out-x', 'out-y', 'out-z'] 11 | plot_ind = range(7,10) 12 | a_pre=np.loadtxt('mat_pre.txt') 13 | a_out=np.loadtxt('mat_out.txt') 14 | time=a_pre[:,0] 15 | axs[0,0].set_title('Attitude') 16 | axs[1,0].set_title('Translation') 17 | axs[2,0].set_title('Extrins-R') 18 | axs[3,0].set_title('Extrins-T') 19 | axs[0,1].set_title('Velocity') 20 | axs[1,1].set_title('bg') 21 | axs[2,1].set_title('ba') 22 | axs[3,1].set_title('Gravity') 23 | for i in range(1,4): 24 | for j in range(8): 25 | axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) 26 | axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) 27 | for j in range(8): 28 | # axs[j].set_xlim(386,389) 29 | axs[j%4, j/4].grid() 30 | axs[j%4, j/4].legend() 31 | plt.grid() 32 | #######for ikfom####### 33 | 34 | 35 | #### Draw IMU data 36 | fig, axs = plt.subplots(2) 37 | imu=np.loadtxt('imu.txt') 38 | time=imu[:,0] 39 | axs[0].set_title('Gyroscope') 40 | axs[1].set_title('Accelerameter') 41 | lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] 42 | lab_2 = ['acc-x', 'acc-y', 'acc-z'] 43 | for i in range(3): 44 | # if i==1: 45 | axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) 46 | axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) 47 | for i in range(2): 48 | # axs[i].set_xlim(386,389) 49 | axs[i].grid() 50 | axs[i].legend() 51 | plt.grid() 52 | 53 | # #### Draw time calculation 54 | # plt.figure(3) 55 | # fig = plt.figure() 56 | # font1 = {'family' : 'Times New Roman', 57 | # 'weight' : 'normal', 58 | # 'size' : 12, 59 | # } 60 | # c="red" 61 | # a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') 62 | # a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') 63 | # a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') 64 | # # n = a_out[:,1].size 65 | # # time_mean = a_out[:,1].mean() 66 | # # time_se = a_out[:,1].std() / np.sqrt(n) 67 | # # time_err = a_out[:,1] - time_mean 68 | # # feat_mean = a_out[:,2].mean() 69 | # # feat_err = a_out[:,2] - feat_mean 70 | # # feat_se = a_out[:,2].std() / np.sqrt(n) 71 | # ax1 = fig.add_subplot(111) 72 | # ax1.set_ylabel('Effective Feature Numbers',font1) 73 | # ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) 74 | # ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) 75 | # ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) 76 | # ax1.set_ylim([0, 3000]) 77 | 78 | # ax2 = ax1.twinx() 79 | # ax2.spines['right'].set_color('red') 80 | # ax2.set_ylabel('Compute Time (ms)',font1) 81 | # ax2.yaxis.label.set_color('red') 82 | # ax2.tick_params(axis='y', colors='red') 83 | # ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 84 | # ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 85 | # ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 86 | # ax2.set_xlim([0.5, 3.5]) 87 | # ax2.set_ylim([0, 100]) 88 | 89 | # plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) 90 | # # # print(time_se) 91 | # # # print(a_out3[:,2]) 92 | # plt.grid() 93 | # plt.savefig("time.pdf", dpi=1200) 94 | plt.show() 95 | -------------------------------------------------------------------------------- /PCD/1: -------------------------------------------------------------------------------- 1 | 1 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FAST-LIO-LOCALIZATION 2 | 3 | A simple localization framework that can re-localize in built maps based on [FAST-LIO](https://github.com/hku-mars/FAST_LIO). 4 | 5 | ## News 6 | 7 | - **2021-08-11:** Add **Open3D 0.7** support. 8 | 9 | - **2021-08-09:** Migrate to **Open3D** for better performance. 10 | 11 | ## 1. Features 12 | - Realtime 3D global localization in a pre-built point cloud map. 13 | By fusing low-frequency global localization (about 0.5~0.2Hz), and high-frequency odometry from FAST-LIO, the entire system is computationally efficient. 14 | 15 |
16 | 17 | - Eliminate the accumulative error of the odometry. 18 | 19 |
20 | 21 | - The initial localization can be provided either by rough manual estimation from RVIZ, or pose from another sensor/algorithm. 22 | 23 | 24 | 25 |
26 | 27 | 28 |
29 | 30 | 31 | ## 2. Prerequisites 32 | ### 2.1 Dependencies for FAST-LIO 33 | 34 | Technically, if you have built and run FAST-LIO before, you may skip section 2.1. 35 | 36 | This part of dependency is consistent with FAST-LIO, please refer to the documentation https://github.com/hku-mars/FAST_LIO#1-prerequisites 37 | 38 | ### 2.2 Dependencies for localization module 39 | 40 | - python 2.7 41 | 42 | - [ros_numpy](https://github.com/eric-wieser/ros_numpy) 43 | 44 | ```shell 45 | sudo apt install ros-$ROS_DISTRO-ros-numpy 46 | ``` 47 | 48 | - [Open3D](http://www.open3d.org/docs/0.9.0/getting_started.html) 49 | 50 | ```shell 51 | pip install open3d==0.9 52 | ``` 53 | 54 | Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**: 55 | ```shell 56 | ERROR: Package 'pyrsistent' requires a different Python: 2.7.18 not in '>=3.5' 57 | ``` 58 | you may firstly install **pyrsistent**: 59 | ```shell 60 | pip install pyrsistent==0.15 61 | ``` 62 | Then 63 | ```shell 64 | pip install open3d==0.9 65 | ``` 66 | 67 | 68 | ## 3. Build 69 | Clone the repository and catkin_make: 70 | 71 | ``` 72 | cd ~/$A_ROS_DIR$/src 73 | git clone https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git 74 | cd FAST_LIO_LOCALIZATION 75 | git submodule update --init 76 | cd ../.. 77 | catkin_make 78 | source devel/setup.bash 79 | ``` 80 | - Remember to source the livox_ros_driver before build (follow [livox_ros_driver](https://github.com/hku-mars/FAST_LIO#13-livox_ros_driver)) 81 | - If you want to use a custom build of PCL, add the following line to ~/.bashrc 82 | ```export PCL_ROOT={CUSTOM_PCL_PATH}``` 83 | 84 | 85 | ## 4. Run Localization 86 | ### 4.1 Sample Dataset 87 | 88 | Demo rosbag in a large underground garage: 89 | [Google Drive](https://drive.google.com/file/d/15ZZAcz84mDxaWviwFPuALpkoeK-KAh-4/view?usp=sharing) | [Baidu Pan (Code: ne8d)](https://pan.baidu.com/s/1ceBiIAUqHa1vY3QjWpxwNA); 90 | 91 | Corresponding map: [Google Drive](https://drive.google.com/file/d/1X_mhPlSCNj-1erp_DStCQZfkY7l4w7j8/view?usp=sharing) | [Baidu Pan (Code: kw6f)](https://pan.baidu.com/s/1Yw4vY3kEK8x2g-AsBi6VCw) 92 | 93 | The map can be built using LIO-SAM or FAST-LIO-SLAM. 94 | 95 | ### 4.2 Run 96 | 97 | 1. First, please make sure you're using the **Python 2.7** environment; 98 | 99 | 100 | 2. Run localization, here we take Livox AVIA as an example: 101 | 102 | ```shell 103 | roslaunch fast_lio_localization localization_avia.launch map:=/path/to/your/map.pcd 104 | ``` 105 | 106 | Please modify `/path/to/your/map.pcd` to your own map point cloud file path. 107 | 108 | Wait for 3~5 seconds until the map cloud shows up in RVIZ; 109 | 110 | 3. If you are testing with the sample rosbag data: 111 | ```shell 112 | rosbag play localization_test_scene_1.bag 113 | ``` 114 | 115 | Or if you are running realtime 116 | 117 | ```shell 118 | roslaunch livox_ros_driver livox_lidar_msg.launch 119 | ``` 120 | Please set the **publish_freq** in **livox_lidar_rviz.launch** to **10Hz**, to ensure there are enough points for global localization in a single scan. 121 | Support for higher frequency is coming soon. 122 | 123 | 4. Provide initial pose 124 | ```shell 125 | rosrun fast_lio_localization publish_initial_pose.py 14.5 -7.5 0 -0.25 0 0 126 | ``` 127 | The numerical value **14.5 -7.5 0 -0.25 0 0** denotes 6D pose **x y z yaw pitch roll** in map frame, 128 | which is a rough initial guess for **localization_test_scene_1.bag**. 129 | 130 | The initial guess can also be provided by the '2D Pose Estimate' Tool in RVIZ. 131 | 132 | Note that, during the initialization stage, it's better to keep the robot still. Or if you play bags, fistly play the bag for about 0.5s, and then pause the bag until the initialization succeed. 133 | 134 | 135 | ## Related Works 136 | 1. [FAST-LIO](https://github.com/hku-mars/FAST_LIO): A computationally efficient and robust LiDAR-inertial odometry (LIO) package 137 | 2. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search. 138 | 3. [FAST-LIO-SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM): The integration of FAST-LIO with [Scan-Context](https://github.com/irapkaist/scancontext) **loop closure** module. 139 | 4. [LIO-SAM_based_relocalization](https://github.com/Gaochao-hit/LIO-SAM_based_relocalization): A simple system that can relocalize a robot on a built map based on LIO-SAM. 140 | 141 | 142 | ## Acknowledgments 143 | Thanks for the authors of [FAST-LIO](https://github.com/hku-mars/FAST_LIO) and [LIO-SAM_based_relocalization](https://github.com/Gaochao-hit/LIO-SAM_based_relocalization). 144 | 145 | ## TODO 146 | 1. Go over the timestamp issue of the published odometry and tf; 147 | 2. Using integrated points for global localization; 148 | 3. Fuse global localization with the state estimation of FAST-LIO, and smooth the localization trajectory; 149 | 4. Updating... -------------------------------------------------------------------------------- /config/avia.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/livox/lidar" 3 | imu_topic: "/livox/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 6 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 90 17 | det_range: 450.0 18 | extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] 19 | extrinsic_R: [ 1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: 1 # 'false' will close all the point cloud output 25 | dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /config/horizon.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/livox/lidar" 3 | imu_topic: "/livox/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 6 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 100 17 | det_range: 260.0 18 | extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] 19 | extrinsic_R: [ 1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: 1 # 'false' will close all the point cloud output 25 | dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /config/ouster64.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os_cloud_node/points" 3 | imu_topic: "/os_cloud_node/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 64 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 180 17 | det_range: 150.0 18 | extrinsic_T: [ 0.0, 0.0, 0.0 ] 19 | extrinsic_R: [1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: 1 # 'false' will close all the point cloud output 25 | dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /config/velodyne.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/velodyne_points" 3 | imu_topic: "/imu/data" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 32 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 180 17 | det_range: 100.0 18 | extrinsic_T: [ 0, 0, 0.28] 19 | extrinsic_R: [ 1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: 1 # 'false' will close all the point cloud output 25 | dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /doc/demo.GIF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HViktorTsoi/FAST_LIO_LOCALIZATION/2bc274ed0b36d14a5c34ada5e1473d52aa1db0d2/doc/demo.GIF -------------------------------------------------------------------------------- /doc/demo_accu.GIF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HViktorTsoi/FAST_LIO_LOCALIZATION/2bc274ed0b36d14a5c34ada5e1473d52aa1db0d2/doc/demo_accu.GIF -------------------------------------------------------------------------------- /doc/demo_init.GIF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HViktorTsoi/FAST_LIO_LOCALIZATION/2bc274ed0b36d14a5c34ada5e1473d52aa1db0d2/doc/demo_init.GIF -------------------------------------------------------------------------------- /doc/demo_init_2.GIF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HViktorTsoi/FAST_LIO_LOCALIZATION/2bc274ed0b36d14a5c34ada5e1473d52aa1db0d2/doc/demo_init_2.GIF -------------------------------------------------------------------------------- /include/Exp_mat.h: -------------------------------------------------------------------------------- 1 | #ifndef EXP_MAT_H 2 | #define EXP_MAT_H 3 | 4 | #include 5 | #include 6 | #include 7 | // #include 8 | 9 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 10 | 11 | template 12 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 13 | { 14 | T ang_norm = ang.norm(); 15 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 16 | if (ang_norm > 0.0000001) 17 | { 18 | Eigen::Matrix r_axis = ang / ang_norm; 19 | Eigen::Matrix K; 20 | K << SKEW_SYM_MATRX(r_axis); 21 | /// Roderigous Tranformation 22 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 23 | } 24 | else 25 | { 26 | return Eye3; 27 | } 28 | } 29 | 30 | template 31 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 32 | { 33 | T ang_vel_norm = ang_vel.norm(); 34 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 35 | 36 | if (ang_vel_norm > 0.0000001) 37 | { 38 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 39 | Eigen::Matrix K; 40 | 41 | K << SKEW_SYM_MATRX(r_axis); 42 | 43 | T r_ang = ang_vel_norm * dt; 44 | 45 | /// Roderigous Tranformation 46 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 47 | } 48 | else 49 | { 50 | return Eye3; 51 | } 52 | } 53 | 54 | template 55 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 56 | { 57 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 58 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 59 | if (norm > 0.00001) 60 | { 61 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 62 | Eigen::Matrix K; 63 | K << SKEW_SYM_MATRX(r_ang); 64 | 65 | /// Roderigous Tranformation 66 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 67 | } 68 | else 69 | { 70 | return Eye3; 71 | } 72 | } 73 | 74 | /* Logrithm of a Rotation Matrix */ 75 | template 76 | Eigen::Matrix Log(const Eigen::Matrix &R) 77 | { 78 | T &&theta = std::acos(0.5 * (R.trace() - 1)); 79 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 80 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 81 | } 82 | 83 | // template 84 | // cv::Mat Exp(const T &v1, const T &v2, const T &v3) 85 | // { 86 | 87 | // T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 88 | // cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); 89 | // if (norm > 0.0000001) 90 | // { 91 | // T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 92 | // cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); 93 | 94 | // /// Roderigous Tranformation 95 | // return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 96 | // } 97 | // else 98 | // { 99 | // return Eye3; 100 | // } 101 | // } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/esekfom/util.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019--2023, The University of Hong Kong 3 | * All rights reserved. 4 | * 5 | * Author: Dongjiao HE 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Universitaet Bremen nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef __MEKFOM_UTIL_HPP__ 36 | #define __MEKFOM_UTIL_HPP__ 37 | 38 | #include 39 | #include "../mtk/src/mtkmath.hpp" 40 | namespace esekfom { 41 | 42 | template 43 | class is_same { 44 | public: 45 | operator bool() { 46 | return false; 47 | } 48 | }; 49 | template 50 | class is_same { 51 | public: 52 | operator bool() { 53 | return true; 54 | } 55 | }; 56 | 57 | template 58 | class is_double { 59 | public: 60 | operator bool() { 61 | return false; 62 | } 63 | }; 64 | 65 | template<> 66 | class is_double { 67 | public: 68 | operator bool() { 69 | return true; 70 | } 71 | }; 72 | 73 | template 74 | static T 75 | id(const T &x) 76 | { 77 | return x; 78 | } 79 | 80 | } // namespace esekfom 81 | 82 | #endif // __MEKFOM_UTIL_HPP__ 83 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/build_manifold.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/build_manifold.hpp 75 | * @brief Macro to automatically construct compound manifolds. 76 | * 77 | */ 78 | #ifndef MTK_AUTOCONSTRUCT_HPP_ 79 | #define MTK_AUTOCONSTRUCT_HPP_ 80 | 81 | #include 82 | 83 | #include 84 | #include 85 | #include 86 | 87 | #include "src/SubManifold.hpp" 88 | #include "startIdx.hpp" 89 | 90 | #ifndef PARSED_BY_DOXYGEN 91 | //////// internals ////// 92 | 93 | #define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple 94 | 95 | #define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) 96 | 97 | #define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) 98 | 99 | #define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() 100 | #define MTK_CONSTRUCTOR_COPY( type, id) id(id) 101 | #define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); 102 | #define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); 103 | #define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); 104 | #define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} 105 | #define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} 106 | #define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} 107 | #define MTK_OSTREAM( type, id) << __var.id << " " 108 | #define MTK_ISTREAM( type, id) >> __var.id 109 | #define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} 110 | #define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} 111 | #define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} 112 | 113 | #define MTK_SUBVARLIST(seq, S2state, SO3state) \ 114 | BOOST_PP_FOR_1( \ 115 | ( \ 116 | BOOST_PP_SEQ_SIZE(seq), \ 117 | BOOST_PP_SEQ_HEAD(seq), \ 118 | BOOST_PP_SEQ_TAIL(seq) (~), \ 119 | 0,\ 120 | 0,\ 121 | S2state,\ 122 | SO3state ),\ 123 | MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) 124 | 125 | #define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ 126 | MTK::SubManifold id; 127 | #define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \ 128 | MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ 129 | enum {DOF = type::DOF + dof}; \ 130 | enum {DIM = type::DIM+dim}; \ 131 | typedef type::scalar scalar; 132 | 133 | #define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state 134 | #define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \ 135 | MTK_APPLY_MACRO_ON_TUPLE(~, \ 136 | BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ 137 | ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) 138 | 139 | #define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state 140 | 141 | //! this used to be BOOST_PP_TUPLE_ELEM_4_0: 142 | #define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a 143 | 144 | #define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state 145 | #define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \ 146 | BOOST_PP_DEC(len), \ 147 | BOOST_PP_SEQ_HEAD(seq), \ 148 | BOOST_PP_SEQ_TAIL(seq), \ 149 | dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ 150 | dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ 151 | S2state,\ 152 | SO3state) 153 | 154 | #endif /* not PARSED_BY_DOXYGEN */ 155 | 156 | 157 | /** 158 | * Construct a manifold. 159 | * @param name is the class-name of the manifold, 160 | * @param entries is the list of sub manifolds 161 | * 162 | * Entries must be given in a list like this: 163 | * @code 164 | * typedef MTK::trafo > Pose; 165 | * typedef MTK::vect Vec3; 166 | * MTK_BUILD_MANIFOLD(imu_state, 167 | * ((Pose, pose)) 168 | * ((Vec3, vel)) 169 | * ((Vec3, acc_bias)) 170 | * ) 171 | * @endcode 172 | * Whitespace is optional, but the double parentheses are necessary. 173 | * Construction is done entirely in preprocessor. 174 | * After construction @a name is also a manifold. Its members can be 175 | * accessed by names given in @a entries. 176 | * 177 | * @note Variable types are not allowed to have commas, thus types like 178 | * @c vect need to be typedef'ed ahead. 179 | */ 180 | #define MTK_BUILD_MANIFOLD(name, entries) \ 181 | struct name { \ 182 | typedef name self; \ 183 | std::vector > S2_state;\ 184 | std::vector > SO3_state;\ 185 | std::vector, int> > vect_state;\ 186 | MTK_SUBVARLIST(entries, S2_state, SO3_state) \ 187 | name ( \ 188 | MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ 189 | ) : \ 190 | MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ 191 | int getDOF() const { return DOF; } \ 192 | void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ 193 | MTK_TRANSFORM(MTK_BOXPLUS, entries)\ 194 | } \ 195 | void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ 196 | MTK_TRANSFORM(MTK_OPLUS, entries)\ 197 | } \ 198 | void boxminus(MTK::vectview __res, const name& __oth) const { \ 199 | MTK_TRANSFORM(MTK_BOXMINUS, entries)\ 200 | } \ 201 | friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ 202 | return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ 203 | } \ 204 | void build_S2_state(){\ 205 | MTK_TRANSFORM(MTK_S2_state, entries)\ 206 | }\ 207 | void build_vect_state(){\ 208 | MTK_TRANSFORM(MTK_vect_state, entries)\ 209 | }\ 210 | void build_SO3_state(){\ 211 | MTK_TRANSFORM(MTK_SO3_state, entries)\ 212 | }\ 213 | void S2_hat(Eigen::Matrix &res, int idx) {\ 214 | MTK_TRANSFORM(MTK_S2_hat, entries)\ 215 | }\ 216 | void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ 217 | MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ 218 | }\ 219 | void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ 220 | MTK_TRANSFORM(MTK_S2_Mx, entries)\ 221 | }\ 222 | friend std::istream& operator>>(std::istream& __is, name& __var){ \ 223 | return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ 224 | } \ 225 | }; 226 | 227 | 228 | 229 | #endif /*MTK_AUTOCONSTRUCT_HPP_*/ 230 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/src/SubManifold.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/src/SubManifold.hpp 75 | * @brief Defines the SubManifold class 76 | */ 77 | 78 | 79 | #ifndef SUBMANIFOLD_HPP_ 80 | #define SUBMANIFOLD_HPP_ 81 | 82 | 83 | #include "vectview.hpp" 84 | 85 | 86 | namespace MTK { 87 | 88 | /** 89 | * @ingroup SubManifolds 90 | * Helper class for compound manifolds. 91 | * This class wraps a manifold T and provides an enum IDX refering to the 92 | * index of the SubManifold within the compound manifold. 93 | * 94 | * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". 95 | * 96 | * @tparam T The manifold type of the sub-type 97 | * @tparam idx The index of the sub-type within the compound manifold 98 | */ 99 | template 100 | struct SubManifold : public T 101 | { 102 | enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; 103 | //! manifold type 104 | typedef T type; 105 | 106 | //! Construct from derived type 107 | template 108 | explicit 109 | SubManifold(const X& t) : T(t) {}; 110 | 111 | //! Construct from internal type 112 | //explicit 113 | SubManifold(const T& t) : T(t) {}; 114 | 115 | //! inherit assignment operator 116 | using T::operator=; 117 | 118 | }; 119 | 120 | } // namespace MTK 121 | 122 | 123 | #endif /* SUBMANIFOLD_HPP_ */ 124 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/src/mtkmath.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/src/mtkmath.hpp 75 | * @brief several math utility functions. 76 | */ 77 | 78 | #ifndef MTKMATH_H_ 79 | #define MTKMATH_H_ 80 | 81 | #include 82 | 83 | #include 84 | 85 | #include "../types/vect.hpp" 86 | 87 | #ifndef M_PI 88 | #define M_PI 3.1415926535897932384626433832795 89 | #endif 90 | 91 | 92 | namespace MTK { 93 | 94 | namespace internal { 95 | 96 | template 97 | struct traits { 98 | typedef typename Manifold::scalar scalar; 99 | enum {DOF = Manifold::DOF}; 100 | typedef vect vectorized_type; 101 | typedef Eigen::Matrix matrix_type; 102 | }; 103 | 104 | template<> 105 | struct traits : traits > {}; 106 | template<> 107 | struct traits : traits > {}; 108 | 109 | } // namespace internal 110 | 111 | /** 112 | * \defgroup MTKMath Mathematical helper functions 113 | */ 114 | //@{ 115 | 116 | //! constant @f$ \pi @f$ 117 | const double pi = M_PI; 118 | 119 | template inline scalar tolerance(); 120 | 121 | template<> inline float tolerance() { return 1e-5f; } 122 | template<> inline double tolerance() { return 1e-11; } 123 | 124 | 125 | /** 126 | * normalize @a x to @f$[-bound, bound] @f$. 127 | * 128 | * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. 129 | */ 130 | template 131 | inline scalar normalize(scalar x, scalar bound){ //not used 132 | if(std::fabs(x) <= bound) return x; 133 | int r = (int)(x *(scalar(1.0)/ bound)); 134 | return x - ((r + (r>>31) + 1) & ~1)*bound; 135 | } 136 | 137 | /** 138 | * Calculate cosine and sinc of sqrt(x2). 139 | * @param x2 the squared angle must be non-negative 140 | * @return a pair containing cos and sinc of sqrt(x2) 141 | */ 142 | template 143 | std::pair cos_sinc_sqrt(const scalar &x2){ 144 | using std::sqrt; 145 | using std::cos; 146 | using std::sin; 147 | static scalar const taylor_0_bound = boost::math::tools::epsilon(); 148 | static scalar const taylor_2_bound = sqrt(taylor_0_bound); 149 | static scalar const taylor_n_bound = sqrt(taylor_2_bound); 150 | 151 | assert(x2>=0 && "argument must be non-negative"); 152 | 153 | // FIXME check if bigger bounds are possible 154 | if(x2>=taylor_n_bound) { 155 | // slow fall-back solution 156 | scalar x = sqrt(x2); 157 | return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. 158 | } 159 | 160 | // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) 161 | // TODO Find optimal coefficients using Remez algorithm 162 | static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; 163 | scalar cosi = 1., sinc=1; 164 | scalar term = -1/2. * x2; 165 | for(int i=0; i<3; ++i) { 166 | cosi += term; 167 | term *= inv[2*i]; 168 | sinc += term; 169 | term *= -inv[2*i+1] * x2; 170 | } 171 | 172 | return std::make_pair(cosi, sinc); 173 | 174 | } 175 | 176 | template 177 | Eigen::Matrix hat(const Base& v) { 178 | Eigen::Matrix res; 179 | res << 0, -v[2], v[1], 180 | v[2], 0, -v[0], 181 | -v[1], v[0], 0; 182 | return res; 183 | } 184 | 185 | template 186 | Eigen::Matrix A_inv_trans(const Base& v){ 187 | Eigen::Matrix res; 188 | if(v.norm() > MTK::tolerance()) 189 | { 190 | res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); 191 | 192 | } 193 | else 194 | { 195 | res = Eigen::Matrix::Identity(); 196 | } 197 | 198 | return res; 199 | } 200 | 201 | template 202 | Eigen::Matrix A_inv(const Base& v){ 203 | Eigen::Matrix res; 204 | if(v.norm() > MTK::tolerance()) 205 | { 206 | res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); 207 | 208 | } 209 | else 210 | { 211 | res = Eigen::Matrix::Identity(); 212 | } 213 | 214 | return res; 215 | } 216 | 217 | template 218 | Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) 219 | { 220 | Eigen::Matrix res; 221 | scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); 222 | if(norm < MTK::tolerance()){ 223 | res = Eigen::Matrix::Zero(); 224 | res(0, 1) = 1; 225 | res(1, 2) = 1; 226 | res /= length; 227 | } 228 | else{ 229 | res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, 230 | -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); 231 | res /= length; 232 | } 233 | } 234 | 235 | template 236 | Eigen::Matrix A_matrix(const Base & v){ 237 | Eigen::Matrix res; 238 | double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; 239 | double norm = std::sqrt(squaredNorm); 240 | if(norm < MTK::tolerance()){ 241 | res = Eigen::Matrix::Identity(); 242 | } 243 | else{ 244 | res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); 245 | } 246 | return res; 247 | } 248 | 249 | template 250 | scalar exp(vectview result, vectview vec, const scalar& scale = 1) { 251 | scalar norm2 = vec.squaredNorm(); 252 | std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); 253 | scalar mult = cos_sinc.second * scale; 254 | result = mult * vec; 255 | return cos_sinc.first; 256 | } 257 | 258 | 259 | /** 260 | * Inverse function to @c exp. 261 | * 262 | * @param result @c vectview to the result 263 | * @param w scalar part of input 264 | * @param vec vector part of input 265 | * @param scale scale result by this value 266 | * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result 267 | */ 268 | template 269 | void log(vectview result, 270 | const scalar &w, const vectview vec, 271 | const scalar &scale, bool plus_minus_periodicity) 272 | { 273 | // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division 274 | scalar nv = vec.norm(); 275 | if(nv < tolerance()) { 276 | if(!plus_minus_periodicity && w < 0) { 277 | // find the maximal entry: 278 | int i; 279 | nv = vec.cwiseAbs().maxCoeff(&i); 280 | result = scale * std::atan2(nv, w) * vect::Unit(i); 281 | return; 282 | } 283 | nv = tolerance(); 284 | } 285 | scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); 286 | 287 | result = s * vec; 288 | } 289 | 290 | 291 | } // namespace MTK 292 | 293 | 294 | #endif /* MTKMATH_H_ */ 295 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/src/vectview.hpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * Copyright (c) 2008--2011, Universitaet Bremen 4 | * All rights reserved. 5 | * 6 | * Author: Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | /** 36 | * @file mtk/src/vectview.hpp 37 | * @brief Wrapper class around a pointer used as interface for plain vectors. 38 | */ 39 | 40 | #ifndef VECTVIEW_HPP_ 41 | #define VECTVIEW_HPP_ 42 | 43 | #include 44 | 45 | namespace MTK { 46 | 47 | /** 48 | * A view to a vector. 49 | * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. 50 | * The dimension of the vector is given as template parameter and type-checked when used in expressions. 51 | * Data has to be modifiable. 52 | * 53 | * @tparam scalar Scalar type of the vector. 54 | * @tparam dim Dimension of the vector. 55 | * 56 | * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct 57 | */ 58 | namespace internal { 59 | template 60 | struct CovBlock { 61 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; 62 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; 63 | }; 64 | 65 | template 66 | struct CovBlock_ { 67 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; 68 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; 69 | }; 70 | 71 | template 72 | struct CrossCovBlock { 73 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; 74 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; 75 | }; 76 | 77 | template 78 | struct CrossCovBlock_ { 79 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; 80 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; 81 | }; 82 | 83 | template 84 | struct VectviewBase { 85 | typedef Eigen::Matrix matrix_type; 86 | typedef typename matrix_type::MapType Type; 87 | typedef typename matrix_type::ConstMapType ConstType; 88 | }; 89 | 90 | template 91 | struct UnalignedType { 92 | typedef T type; 93 | }; 94 | } 95 | 96 | template 97 | class vectview : public internal::VectviewBase::Type { 98 | typedef internal::VectviewBase VectviewBase; 99 | public: 100 | //! plain matrix type 101 | typedef typename VectviewBase::matrix_type matrix_type; 102 | //! base type 103 | typedef typename VectviewBase::Type base; 104 | //! construct from pointer 105 | explicit 106 | vectview(scalar* data, int dim_=dim) : base(data, dim_) {} 107 | //! construct from plain matrix 108 | vectview(matrix_type& m) : base(m.data(), m.size()) {} 109 | //! construct from another @c vectview 110 | vectview(const vectview &v) : base(v) {} 111 | //! construct from Eigen::Block: 112 | template 113 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} 114 | template 115 | vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} 116 | 117 | //! inherit assignment operator 118 | using base::operator=; 119 | //! data pointer 120 | scalar* data() {return const_cast(base::data());} 121 | }; 122 | 123 | /** 124 | * @c const version of @c vectview. 125 | * Compared to @c Eigen::Map this implementation is const correct, i.e., 126 | * data will not be modifiable using this view. 127 | * 128 | * @tparam scalar Scalar type of the vector. 129 | * @tparam dim Dimension of the vector. 130 | * 131 | * @sa vectview 132 | */ 133 | template 134 | class vectview : public internal::VectviewBase::ConstType { 135 | typedef internal::VectviewBase VectviewBase; 136 | public: 137 | //! plain matrix type 138 | typedef typename VectviewBase::matrix_type matrix_type; 139 | //! base type 140 | typedef typename VectviewBase::ConstType base; 141 | //! construct from const pointer 142 | explicit 143 | vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} 144 | //! construct from column vector 145 | template 146 | vectview(const Eigen::Matrix& m) : base(m.data()) {} 147 | //! construct from row vector 148 | template 149 | vectview(const Eigen::Matrix& m) : base(m.data()) {} 150 | //! construct from another @c vectview 151 | vectview(vectview x) : base(x.data()) {} 152 | //! construct from base 153 | vectview(const base &x) : base(x) {} 154 | /** 155 | * Construct from Block 156 | * @todo adapt this, when Block gets const-correct 157 | */ 158 | template 159 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} 160 | template 161 | vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} 162 | 163 | }; 164 | 165 | 166 | } // namespace MTK 167 | 168 | #endif /* VECTVIEW_HPP_ */ 169 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/startIdx.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/startIdx.hpp 75 | * @brief Tools to access sub-elements of compound manifolds. 76 | */ 77 | #ifndef GET_START_INDEX_H_ 78 | #define GET_START_INDEX_H_ 79 | 80 | #include 81 | 82 | #include "src/SubManifold.hpp" 83 | #include "src/vectview.hpp" 84 | 85 | namespace MTK { 86 | 87 | 88 | /** 89 | * \defgroup SubManifolds Accessing Submanifolds 90 | * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers 91 | * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. 92 | * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers 93 | * @c &pose::orient and @c &pose::trans give all required information and are still 94 | * valid if the base type gets extended or the actual types of @a orient and @a trans 95 | * change (e.g. from 2D to 3D). 96 | * 97 | * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. 98 | */ 99 | //@{ 100 | 101 | /** 102 | * Determine the index of a sub-variable within a compound variable. 103 | */ 104 | template 105 | int getStartIdx( MTK::SubManifold Base::*) 106 | { 107 | return idx; 108 | } 109 | 110 | template 111 | int getStartIdx_( MTK::SubManifold Base::*) 112 | { 113 | return dim; 114 | } 115 | 116 | /** 117 | * Determine the degrees of freedom of a sub-variable within a compound variable. 118 | */ 119 | template 120 | int getDof( MTK::SubManifold Base::*) 121 | { 122 | return T::DOF; 123 | } 124 | template 125 | int getDim( MTK::SubManifold Base::*) 126 | { 127 | return T::DIM; 128 | } 129 | 130 | /** 131 | * set the diagonal elements of a covariance matrix corresponding to a sub-variable 132 | */ 133 | template 134 | void setDiagonal(Eigen::Matrix &cov, 135 | MTK::SubManifold Base::*, const typename Base::scalar &val) 136 | { 137 | cov.diagonal().template segment(idx).setConstant(val); 138 | } 139 | 140 | template 141 | void setDiagonal_(Eigen::Matrix &cov, 142 | MTK::SubManifold Base::*, const typename Base::scalar &val) 143 | { 144 | cov.diagonal().template segment(dim).setConstant(val); 145 | } 146 | 147 | /** 148 | * Get the subblock of corresponding to two members, i.e. 149 | * \code 150 | * Eigen::Matrix m; 151 | * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; 152 | * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); 153 | * \endcode 154 | * lets you modify mixed covariance entries in a bigger covariance matrix. 155 | */ 156 | template 157 | typename MTK::internal::CovBlock::Type 158 | subblock(Eigen::Matrix &cov, 159 | MTK::SubManifold Base::*, MTK::SubManifold Base::*) 160 | { 161 | return cov.template block(idx1, idx2); 162 | } 163 | 164 | template 165 | typename MTK::internal::CovBlock_::Type 166 | subblock_(Eigen::Matrix &cov, 167 | MTK::SubManifold Base::*, MTK::SubManifold Base::*) 168 | { 169 | return cov.template block(dim1, dim2); 170 | } 171 | 172 | template 173 | typename MTK::internal::CrossCovBlock::Type 174 | subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) 175 | { 176 | return cov.template block(idx1, idx2); 177 | } 178 | 179 | template 180 | typename MTK::internal::CrossCovBlock_::Type 181 | subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) 182 | { 183 | return cov.template block(dim1, dim2); 184 | } 185 | /** 186 | * Get the subblock of corresponding to a member, i.e. 187 | * \code 188 | * Eigen::Matrix m; 189 | * MTK::subblock(m, &Pose::orient) = some_expression; 190 | * \endcode 191 | * lets you modify covariance entries in a bigger covariance matrix. 192 | */ 193 | template 194 | typename MTK::internal::CovBlock_::Type 195 | subblock_(Eigen::Matrix &cov, 196 | MTK::SubManifold Base::*) 197 | { 198 | return cov.template block(dim, dim); 199 | } 200 | 201 | template 202 | typename MTK::internal::CovBlock::Type 203 | subblock(Eigen::Matrix &cov, 204 | MTK::SubManifold Base::*) 205 | { 206 | return cov.template block(idx, idx); 207 | } 208 | 209 | template 210 | class get_cov { 211 | public: 212 | typedef Eigen::Matrix type; 213 | typedef const Eigen::Matrix const_type; 214 | }; 215 | 216 | template 217 | class get_cov_ { 218 | public: 219 | typedef Eigen::Matrix type; 220 | typedef const Eigen::Matrix const_type; 221 | }; 222 | 223 | template 224 | class get_cross_cov { 225 | public: 226 | typedef Eigen::Matrix type; 227 | typedef const type const_type; 228 | }; 229 | 230 | template 231 | class get_cross_cov_ { 232 | public: 233 | typedef Eigen::Matrix type; 234 | typedef const type const_type; 235 | }; 236 | 237 | 238 | template 239 | vectview 240 | subvector_impl_(vectview vec, SubManifold Base::*) 241 | { 242 | return vec.template segment(dim); 243 | } 244 | 245 | template 246 | vectview 247 | subvector_impl(vectview vec, SubManifold Base::*) 248 | { 249 | return vec.template segment(idx); 250 | } 251 | 252 | /** 253 | * Get the subvector corresponding to a sub-manifold from a bigger vector. 254 | */ 255 | template 256 | vectview 257 | subvector_(vectview vec, SubManifold Base::* ptr) 258 | { 259 | return subvector_impl_(vec, ptr); 260 | } 261 | 262 | template 263 | vectview 264 | subvector(vectview vec, SubManifold Base::* ptr) 265 | { 266 | return subvector_impl(vec, ptr); 267 | } 268 | 269 | /** 270 | * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) 271 | */ 272 | template 273 | vectview 274 | subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) 275 | { 276 | return subvector_impl(vectview(vec), ptr); 277 | } 278 | 279 | template 280 | vectview 281 | subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) 282 | { 283 | return subvector_impl_(vectview(vec), ptr); 284 | } 285 | 286 | template 287 | vectview 288 | subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) 289 | { 290 | return subvector_impl_(vectview(vec), ptr); 291 | } 292 | 293 | template 294 | vectview 295 | subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) 296 | { 297 | return subvector_impl(vectview(vec), ptr); 298 | } 299 | 300 | 301 | /** 302 | * const version of subvector(vectview vec,SubManifold Base::*) 303 | */ 304 | template 305 | vectview 306 | subvector_impl(const vectview cvec, SubManifold Base::*) 307 | { 308 | return cvec.template segment(idx); 309 | } 310 | 311 | template 312 | vectview 313 | subvector_impl_(const vectview cvec, SubManifold Base::*) 314 | { 315 | return cvec.template segment(dim); 316 | } 317 | 318 | template 319 | vectview 320 | subvector(const vectview cvec, SubManifold Base::* ptr) 321 | { 322 | return subvector_impl(cvec, ptr); 323 | } 324 | 325 | 326 | } // namespace MTK 327 | 328 | #endif // GET_START_INDEX_H_ 329 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/S2.hpp: -------------------------------------------------------------------------------- 1 | // This is a NEW implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/S2.hpp 75 | * @brief Unit vectors on the sphere, or directions in 3D. 76 | */ 77 | #ifndef S2_H_ 78 | #define S2_H_ 79 | 80 | 81 | #include "vect.hpp" 82 | 83 | #include "SOn.hpp" 84 | #include "../src/mtkmath.hpp" 85 | 86 | 87 | 88 | 89 | namespace MTK { 90 | 91 | /** 92 | * Manifold representation of @f$ S^2 @f$. 93 | * Used for unit vectors on the sphere or directions in 3D. 94 | * 95 | * @todo add conversions from/to polar angles? 96 | */ 97 | template 98 | struct S2 { 99 | 100 | typedef _scalar scalar; 101 | typedef vect<3, scalar> vect_type; 102 | typedef SO3 SO3_type; 103 | typedef typename vect_type::base vec3; 104 | scalar length = scalar(den)/scalar(num); 105 | enum {DOF=2, TYP = 1, DIM = 3}; 106 | 107 | //private: 108 | /** 109 | * Unit vector on the sphere, or vector pointing in a direction 110 | */ 111 | vect_type vec; 112 | 113 | public: 114 | S2() { 115 | if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); 116 | if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); 117 | if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); 118 | } 119 | S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { 120 | vec.normalize(); 121 | vec = vec * length; 122 | } 123 | 124 | S2(const vect_type &_vec) : vec(_vec) { 125 | vec.normalize(); 126 | vec = vec * length; 127 | } 128 | 129 | void oplus(MTK::vectview delta, scalar scale = 1) 130 | { 131 | SO3_type res; 132 | res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); 133 | vec = res.toRotationMatrix() * vec; 134 | } 135 | 136 | void boxplus(MTK::vectview delta, scalar scale=1) { 137 | Eigen::Matrix Bx; 138 | S2_Bx(Bx); 139 | vect_type Bu = Bx*delta;SO3_type res; 140 | res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); 141 | vec = res.toRotationMatrix() * vec; 142 | } 143 | 144 | void boxminus(MTK::vectview res, const S2& other) const { 145 | scalar v_sin = (MTK::hat(vec)*other.vec).norm(); 146 | scalar v_cos = vec.transpose() * other.vec; 147 | scalar theta = std::atan2(v_sin, v_cos); 148 | if(v_sin < MTK::tolerance()) 149 | { 150 | if(std::fabs(theta) > MTK::tolerance() ) 151 | { 152 | res[0] = 3.1415926; 153 | res[1] = 0; 154 | } 155 | else{ 156 | res[0] = 0; 157 | res[1] = 0; 158 | } 159 | } 160 | else 161 | { 162 | S2 other_copy = other; 163 | Eigen::MatrixBx; 164 | other_copy.S2_Bx(Bx); 165 | res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; 166 | } 167 | } 168 | 169 | void S2_hat(Eigen::Matrix &res) 170 | { 171 | Eigen::Matrix skew_vec; 172 | skew_vec << scalar(0), -vec[2], vec[1], 173 | vec[2], scalar(0), -vec[0], 174 | -vec[1], vec[0], scalar(0); 175 | res = skew_vec; 176 | } 177 | 178 | 179 | void S2_Bx(Eigen::Matrix &res) 180 | { 181 | if(S2_typ == 3) 182 | { 183 | if(vec[2] + length > tolerance()) 184 | { 185 | 186 | res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), 187 | -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), 188 | -vec[0], -vec[1]; 189 | res /= length; 190 | } 191 | else 192 | { 193 | res = Eigen::Matrix::Zero(); 194 | res(1, 1) = -1; 195 | res(2, 0) = 1; 196 | } 197 | } 198 | else if(S2_typ == 2) 199 | { 200 | if(vec[1] + length > tolerance()) 201 | { 202 | 203 | res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), 204 | -vec[0], -vec[2], 205 | -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); 206 | res /= length; 207 | } 208 | else 209 | { 210 | res = Eigen::Matrix::Zero(); 211 | res(1, 1) = -1; 212 | res(2, 0) = 1; 213 | } 214 | } 215 | else 216 | { 217 | if(vec[0] + length > tolerance()) 218 | { 219 | 220 | res << -vec[1], -vec[2], 221 | length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), 222 | -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); 223 | res /= length; 224 | } 225 | else 226 | { 227 | res = Eigen::Matrix::Zero(); 228 | res(1, 1) = -1; 229 | res(2, 0) = 1; 230 | } 231 | } 232 | } 233 | 234 | void S2_Nx(Eigen::Matrix &res, S2& subtrahend) 235 | { 236 | if((vec+subtrahend.vec).norm() > tolerance()) 237 | { 238 | Eigen::Matrix Bx; 239 | S2_Bx(Bx); 240 | if((vec-subtrahend.vec).norm() > tolerance()) 241 | { 242 | scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); 243 | scalar v_cos = vec.transpose() * subtrahend.vec; 244 | 245 | res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); 246 | } 247 | else 248 | { 249 | res = 1/length/length*Bx.transpose()*MTK::hat(vec); 250 | } 251 | } 252 | else 253 | { 254 | std::cerr << "No N(x, y) for x=-y" << std::endl; 255 | std::exit(100); 256 | } 257 | } 258 | 259 | void S2_Nx_yy(Eigen::Matrix &res) 260 | { 261 | Eigen::Matrix Bx; 262 | S2_Bx(Bx); 263 | res = 1/length/length*Bx.transpose()*MTK::hat(vec); 264 | } 265 | 266 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 267 | { 268 | Eigen::Matrix Bx; 269 | S2_Bx(Bx); 270 | if(delta.norm() < tolerance()) 271 | { 272 | res = -MTK::hat(vec)*Bx; 273 | } 274 | else{ 275 | vect_type Bu = Bx*delta; 276 | SO3_type exp_delta; 277 | exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); 278 | res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; 279 | } 280 | } 281 | 282 | operator const vect_type&() const{ 283 | return vec; 284 | } 285 | 286 | const vect_type& get_vect() const { 287 | return vec; 288 | } 289 | 290 | friend S2 operator*(const SO3& rot, const S2& dir) 291 | { 292 | S2 ret; 293 | ret.vec = rot * dir.vec; 294 | return ret; 295 | } 296 | 297 | scalar operator[](int idx) const {return vec[idx]; } 298 | 299 | friend std::ostream& operator<<(std::ostream &os, const S2& vec){ 300 | return os << vec.vec.transpose() << " "; 301 | } 302 | friend std::istream& operator>>(std::istream &is, S2& vec){ 303 | for(int i=0; i<3; ++i) 304 | is >> vec.vec[i]; 305 | vec.vec.normalize(); 306 | vec.vec = vec.vec * vec.length; 307 | return is; 308 | 309 | } 310 | }; 311 | 312 | 313 | } // namespace MTK 314 | 315 | 316 | #endif /*S2_H_*/ 317 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/SOn.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/SOn.hpp 75 | * @brief Standard Orthogonal Groups i.e.\ rotatation groups. 76 | */ 77 | #ifndef SON_H_ 78 | #define SON_H_ 79 | 80 | #include 81 | 82 | #include "vect.hpp" 83 | #include "../src/mtkmath.hpp" 84 | 85 | 86 | namespace MTK { 87 | 88 | 89 | /** 90 | * Two-dimensional orientations represented as scalar. 91 | * There is no guarantee that the representing scalar is within any interval, 92 | * but the result of boxminus will always have magnitude @f$\le\pi @f$. 93 | */ 94 | template 95 | struct SO2 : public Eigen::Rotation2D<_scalar> { 96 | enum {DOF = 1, DIM = 2, TYP = 3}; 97 | 98 | typedef _scalar scalar; 99 | typedef Eigen::Rotation2D base; 100 | typedef vect vect_type; 101 | 102 | //! Construct from angle 103 | SO2(const scalar& angle = 0) : base(angle) { } 104 | 105 | //! Construct from Eigen::Rotation2D 106 | SO2(const base& src) : base(src) {} 107 | 108 | /** 109 | * Construct from 2D vector. 110 | * Resulting orientation will rotate the first unit vector to point to vec. 111 | */ 112 | SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; 113 | 114 | 115 | //! Calculate @c this->inverse() * @c r 116 | SO2 operator%(const base &r) const { 117 | return base::inverse() * r; 118 | } 119 | 120 | //! Calculate @c this->inverse() * @c r 121 | template 122 | vect_type operator%(const Eigen::MatrixBase &vec) const { 123 | return base::inverse() * vec; 124 | } 125 | 126 | //! Calculate @c *this * @c r.inverse() 127 | SO2 operator/(const SO2 &r) const { 128 | return *this * r.inverse(); 129 | } 130 | 131 | //! Gets the angle as scalar. 132 | operator scalar() const { 133 | return base::angle(); 134 | } 135 | void S2_hat(Eigen::Matrix &res) 136 | { 137 | res = Eigen::Matrix::Zero(); 138 | } 139 | //! @name Manifold requirements 140 | void S2_Nx_yy(Eigen::Matrix &res) 141 | { 142 | std::cerr << "wrong idx for S2" << std::endl; 143 | std::exit(100); 144 | res = Eigen::Matrix::Zero(); 145 | } 146 | 147 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 148 | { 149 | std::cerr << "wrong idx for S2" << std::endl; 150 | std::exit(100); 151 | res = Eigen::Matrix::Zero(); 152 | } 153 | 154 | void oplus(MTK::vectview vec, scalar scale = 1) { 155 | base::angle() += scale * vec[0]; 156 | } 157 | 158 | void boxplus(MTK::vectview vec, scalar scale = 1) { 159 | base::angle() += scale * vec[0]; 160 | } 161 | void boxminus(MTK::vectview res, const SO2& other) const { 162 | res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); 163 | } 164 | 165 | friend std::istream& operator>>(std::istream &is, SO2& ang){ 166 | return is >> ang.angle(); 167 | } 168 | 169 | }; 170 | 171 | 172 | /** 173 | * Three-dimensional orientations represented as Quaternion. 174 | * It is assumed that the internal Quaternion always stays normalized, 175 | * should this not be the case, call inherited member function @c normalize(). 176 | */ 177 | template 178 | struct SO3 : public Eigen::Quaternion<_scalar, Options> { 179 | enum {DOF = 3, DIM = 3, TYP = 2}; 180 | typedef _scalar scalar; 181 | typedef Eigen::Quaternion base; 182 | typedef Eigen::Quaternion Quaternion; 183 | typedef vect vect_type; 184 | 185 | //! Calculate @c this->inverse() * @c r 186 | template EIGEN_STRONG_INLINE 187 | Quaternion operator%(const Eigen::QuaternionBase &r) const { 188 | return base::conjugate() * r; 189 | } 190 | 191 | //! Calculate @c this->inverse() * @c r 192 | template 193 | vect_type operator%(const Eigen::MatrixBase &vec) const { 194 | return base::conjugate() * vec; 195 | } 196 | 197 | //! Calculate @c this * @c r.conjugate() 198 | template EIGEN_STRONG_INLINE 199 | Quaternion operator/(const Eigen::QuaternionBase &r) const { 200 | return *this * r.conjugate(); 201 | } 202 | 203 | /** 204 | * Construct from real part and three imaginary parts. 205 | * Quaternion is normalized after construction. 206 | */ 207 | SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { 208 | base::normalize(); 209 | } 210 | 211 | /** 212 | * Construct from Eigen::Quaternion. 213 | * @note Non-normalized input may result result in spurious behavior. 214 | */ 215 | SO3(const base& src = base::Identity()) : base(src) {} 216 | 217 | /** 218 | * Construct from rotation matrix. 219 | * @note Invalid rotation matrices may lead to spurious behavior. 220 | */ 221 | template 222 | SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} 223 | 224 | /** 225 | * Construct from arbitrary rotation type. 226 | * @note Invalid rotation matrices may lead to spurious behavior. 227 | */ 228 | template 229 | SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} 230 | 231 | //! @name Manifold requirements 232 | 233 | void boxplus(MTK::vectview vec, scalar scale=1) { 234 | SO3 delta = exp(vec, scale); 235 | *this = *this * delta; 236 | } 237 | void boxminus(MTK::vectview res, const SO3& other) const { 238 | res = SO3::log(other.conjugate() * *this); 239 | } 240 | //} 241 | 242 | void oplus(MTK::vectview vec, scalar scale=1) { 243 | SO3 delta = exp(vec, scale); 244 | *this = *this * delta; 245 | } 246 | 247 | void S2_hat(Eigen::Matrix &res) 248 | { 249 | res = Eigen::Matrix::Zero(); 250 | } 251 | void S2_Nx_yy(Eigen::Matrix &res) 252 | { 253 | std::cerr << "wrong idx for S2" << std::endl; 254 | std::exit(100); 255 | res = Eigen::Matrix::Zero(); 256 | } 257 | 258 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 259 | { 260 | std::cerr << "wrong idx for S2" << std::endl; 261 | std::exit(100); 262 | res = Eigen::Matrix::Zero(); 263 | } 264 | 265 | friend std::ostream& operator<<(std::ostream &os, const SO3& q){ 266 | return os << q.coeffs().transpose() << " "; 267 | } 268 | 269 | friend std::istream& operator>>(std::istream &is, SO3& q){ 270 | vect<4,scalar> coeffs; 271 | is >> coeffs; 272 | q.coeffs() = coeffs.normalized(); 273 | return is; 274 | } 275 | 276 | //! @name Helper functions 277 | //{ 278 | /** 279 | * Calculate the exponential map. In matrix terms this would correspond 280 | * to the Rodrigues formula. 281 | */ 282 | // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround 283 | // static SO3 exp(MTK::vectview dvec, scalar scale = 1){ 284 | static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ 285 | SO3 res; 286 | res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); 287 | return res; 288 | } 289 | /** 290 | * Calculate the inverse of @c exp. 291 | * Only guarantees that exp(log(x)) == x 292 | */ 293 | static typename base::Vector3 log(const SO3 &orient){ 294 | typename base::Vector3 res; 295 | MTK::log(res, orient.w(), orient.vec(), scalar(2), true); 296 | return res; 297 | } 298 | }; 299 | 300 | namespace internal { 301 | template 302 | struct UnalignedType >{ 303 | typedef SO2 type; 304 | }; 305 | 306 | template 307 | struct UnalignedType >{ 308 | typedef SO3 type; 309 | }; 310 | 311 | } // namespace internal 312 | 313 | 314 | } // namespace MTK 315 | 316 | #endif /*SON_H_*/ 317 | 318 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/vect.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/vect.hpp 75 | * @brief Basic vectors interpreted as manifolds. 76 | * 77 | * This file also implements a simple wrapper for matrices, for arbitrary scalars 78 | * and for positive scalars. 79 | */ 80 | #ifndef VECT_H_ 81 | #define VECT_H_ 82 | 83 | #include 84 | #include 85 | #include 86 | 87 | #include "../src/vectview.hpp" 88 | 89 | namespace MTK { 90 | 91 | static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); 92 | 93 | 94 | /** 95 | * A simple vector class. 96 | * Implementation is basically a wrapper around Eigen::Matrix with manifold 97 | * requirements added. 98 | */ 99 | template 100 | struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { 101 | typedef Eigen::Matrix<_scalar, D, 1, _Options> base; 102 | enum {DOF = D, DIM = D, TYP = 0}; 103 | typedef _scalar scalar; 104 | 105 | //using base::operator=; 106 | 107 | /** Standard constructor. Sets all values to zero. */ 108 | vect(const base &src = base::Zero()) : base(src) {} 109 | 110 | /** Constructor copying the value of the expression \a other */ 111 | template 112 | EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} 113 | 114 | /** Construct from memory. */ 115 | vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } 116 | 117 | void boxplus(MTK::vectview vec, scalar scale=1) { 118 | *this += scale * vec; 119 | } 120 | void boxminus(MTK::vectview res, const vect& other) const { 121 | res = *this - other; 122 | } 123 | 124 | void oplus(MTK::vectview vec, scalar scale=1) { 125 | *this += scale * vec; 126 | } 127 | 128 | void S2_hat(Eigen::Matrix &res) 129 | { 130 | res = Eigen::Matrix::Zero(); 131 | } 132 | 133 | void S2_Nx_yy(Eigen::Matrix &res) 134 | { 135 | std::cerr << "wrong idx for S2" << std::endl; 136 | std::exit(100); 137 | res = Eigen::Matrix::Zero(); 138 | } 139 | 140 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 141 | { 142 | std::cerr << "wrong idx for S2" << std::endl; 143 | std::exit(100); 144 | res = Eigen::Matrix::Zero(); 145 | } 146 | 147 | friend std::ostream& operator<<(std::ostream &os, const vect& v){ 148 | // Eigen sometimes messes with the streams flags, so output manually: 149 | for(int i=0; i>(std::istream &is, vect& v){ 154 | char term=0; 155 | is >> std::ws; // skip whitespace 156 | switch(is.peek()) { 157 | case '(': term=')'; is.ignore(1); break; 158 | case '[': term=']'; is.ignore(1); break; 159 | case '{': term='}'; is.ignore(1); break; 160 | default: break; 161 | } 162 | if(D==Eigen::Dynamic) { 163 | assert(term !=0 && "Dynamic vectors must be embraced"); 164 | std::vector temp; 165 | while(is.good() && is.peek() != term) { 166 | scalar x; 167 | is >> x; 168 | temp.push_back(x); 169 | if(is.peek()==',') is.ignore(1); 170 | } 171 | v = vect::Map(temp.data(), temp.size()); 172 | } else 173 | for(int i=0; i> v[i]; 175 | if(is.peek()==',') { // ignore commas between values 176 | is.ignore(1); 177 | } 178 | } 179 | if(term!=0) { 180 | char x; 181 | is >> x; 182 | if(x!=term) { 183 | is.setstate(is.badbit); 184 | // assert(x==term && "start and end bracket do not match!"); 185 | } 186 | } 187 | return is; 188 | } 189 | 190 | template 191 | vectview tail(){ 192 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 193 | return base::template tail(); 194 | } 195 | template 196 | vectview tail() const{ 197 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 198 | return base::template tail(); 199 | } 200 | template 201 | vectview head(){ 202 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 203 | return base::template head(); 204 | } 205 | template 206 | vectview head() const{ 207 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 208 | return base::template head(); 209 | } 210 | }; 211 | 212 | 213 | /** 214 | * A simple matrix class. 215 | * Implementation is basically a wrapper around Eigen::Matrix with manifold 216 | * requirements added, i.e., matrix is viewed as a plain vector for that. 217 | */ 218 | template::Options> 219 | struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { 220 | typedef Eigen::Matrix<_scalar, M, N, _Options> base; 221 | enum {DOF = M * N, TYP = 4, DIM=0}; 222 | typedef _scalar scalar; 223 | 224 | using base::operator=; 225 | 226 | /** Standard constructor. Sets all values to zero. */ 227 | matrix() { 228 | base::setZero(); 229 | } 230 | 231 | /** Constructor copying the value of the expression \a other */ 232 | template 233 | EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} 234 | 235 | /** Construct from memory. */ 236 | matrix(const scalar* src) : base(src) { } 237 | 238 | void boxplus(MTK::vectview vec, scalar scale = 1) { 239 | *this += scale * base::Map(vec.data()); 240 | } 241 | void boxminus(MTK::vectview res, const matrix& other) const { 242 | base::Map(res.data()) = *this - other; 243 | } 244 | 245 | void S2_hat(Eigen::Matrix &res) 246 | { 247 | res = Eigen::Matrix::Zero(); 248 | } 249 | 250 | void oplus(MTK::vectview vec, scalar scale = 1) { 251 | *this += scale * base::Map(vec.data()); 252 | } 253 | 254 | void S2_Nx_yy(Eigen::Matrix &res) 255 | { 256 | std::cerr << "wrong idx for S2" << std::endl; 257 | std::exit(100); 258 | res = Eigen::Matrix::Zero(); 259 | } 260 | 261 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 262 | { 263 | std::cerr << "wrong idx for S2" << std::endl; 264 | std::exit(100); 265 | res = Eigen::Matrix::Zero(); 266 | } 267 | 268 | friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ 269 | for(int i=0; i>(std::istream &is, matrix& mat){ 275 | for(int i=0; i> mat.data()[i]; 277 | } 278 | return is; 279 | } 280 | };// @todo What if M / N = Eigen::Dynamic? 281 | 282 | 283 | 284 | /** 285 | * A simple scalar type. 286 | */ 287 | template 288 | struct Scalar { 289 | enum {DOF = 1, TYP = 5, DIM=0}; 290 | typedef _scalar scalar; 291 | 292 | scalar value; 293 | 294 | Scalar(const scalar& value = scalar(0)) : value(value) {} 295 | operator const scalar&() const { return value; } 296 | operator scalar&() { return value; } 297 | Scalar& operator=(const scalar& val) { value = val; return *this; } 298 | 299 | void S2_hat(Eigen::Matrix &res) 300 | { 301 | res = Eigen::Matrix::Zero(); 302 | } 303 | 304 | void S2_Nx_yy(Eigen::Matrix &res) 305 | { 306 | std::cerr << "wrong idx for S2" << std::endl; 307 | std::exit(100); 308 | res = Eigen::Matrix::Zero(); 309 | } 310 | 311 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 312 | { 313 | std::cerr << "wrong idx for S2" << std::endl; 314 | std::exit(100); 315 | res = Eigen::Matrix::Zero(); 316 | } 317 | 318 | void oplus(MTK::vectview vec, scalar scale=1) { 319 | value += scale * vec[0]; 320 | } 321 | 322 | void boxplus(MTK::vectview vec, scalar scale=1) { 323 | value += scale * vec[0]; 324 | } 325 | void boxminus(MTK::vectview res, const Scalar& other) const { 326 | res[0] = *this - other; 327 | } 328 | }; 329 | 330 | /** 331 | * Positive scalars. 332 | * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. 333 | */ 334 | template 335 | struct PositiveScalar { 336 | enum {DOF = 1, TYP = 6, DIM=0}; 337 | typedef _scalar scalar; 338 | 339 | scalar value; 340 | 341 | PositiveScalar(const scalar& value = scalar(1)) : value(value) { 342 | assert(value > scalar(0)); 343 | } 344 | operator const scalar&() const { return value; } 345 | PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } 346 | 347 | void boxplus(MTK::vectview vec, scalar scale = 1) { 348 | value *= std::exp(scale * vec[0]); 349 | } 350 | void boxminus(MTK::vectview res, const PositiveScalar& other) const { 351 | res[0] = std::log(*this / other); 352 | } 353 | 354 | void oplus(MTK::vectview vec, scalar scale = 1) { 355 | value *= std::exp(scale * vec[0]); 356 | } 357 | 358 | void S2_hat(Eigen::Matrix &res) 359 | { 360 | res = Eigen::Matrix::Zero(); 361 | } 362 | 363 | void S2_Nx_yy(Eigen::Matrix &res) 364 | { 365 | std::cerr << "wrong idx for S2" << std::endl; 366 | std::exit(100); 367 | res = Eigen::Matrix::Zero(); 368 | } 369 | 370 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 371 | { 372 | std::cerr << "wrong idx for S2" << std::endl; 373 | std::exit(100); 374 | res = Eigen::Matrix::Zero(); 375 | } 376 | 377 | 378 | friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ 379 | is >> s.value; 380 | assert(s.value > 0); 381 | return is; 382 | } 383 | }; 384 | 385 | template 386 | struct Complex : public std::complex<_scalar>{ 387 | enum {DOF = 2, TYP = 7, DIM=0}; 388 | typedef _scalar scalar; 389 | 390 | typedef std::complex Base; 391 | 392 | Complex(const Base& value) : Base(value) {} 393 | Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} 394 | Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} 395 | template 396 | Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} 397 | 398 | void boxplus(MTK::vectview vec, scalar scale = 1) { 399 | Base::real() += scale * vec[0]; 400 | Base::imag() += scale * vec[1]; 401 | }; 402 | void boxminus(MTK::vectview res, const Complex& other) const { 403 | Complex diff = *this - other; 404 | res << diff.real(), diff.imag(); 405 | } 406 | 407 | void S2_hat(Eigen::Matrix &res) 408 | { 409 | res = Eigen::Matrix::Zero(); 410 | } 411 | 412 | void oplus(MTK::vectview vec, scalar scale = 1) { 413 | Base::real() += scale * vec[0]; 414 | Base::imag() += scale * vec[1]; 415 | }; 416 | 417 | void S2_Nx_yy(Eigen::Matrix &res) 418 | { 419 | std::cerr << "wrong idx for S2" << std::endl; 420 | std::exit(100); 421 | res = Eigen::Matrix::Zero(); 422 | } 423 | 424 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 425 | { 426 | std::cerr << "wrong idx for S2" << std::endl; 427 | std::exit(100); 428 | res = Eigen::Matrix::Zero(); 429 | } 430 | 431 | scalar squaredNorm() const { 432 | return std::pow(Base::real(),2) + std::pow(Base::imag(),2); 433 | } 434 | 435 | const scalar& operator()(int i) const { 436 | assert(0<=i && i<2 && "Index out of range"); 437 | return i==0 ? Base::real() : Base::imag(); 438 | } 439 | scalar& operator()(int i){ 440 | assert(0<=i && i<2 && "Index out of range"); 441 | return i==0 ? Base::real() : Base::imag(); 442 | } 443 | }; 444 | 445 | 446 | namespace internal { 447 | 448 | template 449 | struct UnalignedType >{ 450 | typedef vect type; 451 | }; 452 | 453 | } // namespace internal 454 | 455 | 456 | } // namespace MTK 457 | 458 | 459 | 460 | 461 | #endif /*VECT_H_*/ 462 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH 3 | * All rights reserved. 4 | * 5 | * Author: Rene Wagner 6 | * Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the DFKI GmbH 19 | * nor the names of its contributors may be used to endorse or 20 | * promote products derived from this software without specific 21 | * prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef WRAPPED_CV_MAT_HPP_ 38 | #define WRAPPED_CV_MAT_HPP_ 39 | 40 | #include 41 | #include 42 | 43 | namespace MTK { 44 | 45 | template 46 | struct cv_f_type; 47 | 48 | template<> 49 | struct cv_f_type 50 | { 51 | enum {value = CV_64F}; 52 | }; 53 | 54 | template<> 55 | struct cv_f_type 56 | { 57 | enum {value = CV_32F}; 58 | }; 59 | 60 | /** 61 | * cv_mat wraps a CvMat around an Eigen Matrix 62 | */ 63 | template 64 | class cv_mat : public matrix 65 | { 66 | typedef matrix base_type; 67 | enum {type_ = cv_f_type::value}; 68 | CvMat cv_mat_; 69 | 70 | public: 71 | cv_mat() 72 | { 73 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 74 | } 75 | 76 | cv_mat(const cv_mat& oth) : base_type(oth) 77 | { 78 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 79 | } 80 | 81 | template 82 | cv_mat(const Eigen::MatrixBase &value) : base_type(value) 83 | { 84 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 85 | } 86 | 87 | template 88 | cv_mat& operator=(const Eigen::MatrixBase &value) 89 | { 90 | base_type::operator=(value); 91 | return *this; 92 | } 93 | 94 | cv_mat& operator=(const cv_mat& value) 95 | { 96 | base_type::operator=(value); 97 | return *this; 98 | } 99 | 100 | // FIXME: Maybe overloading operator& is not a good idea ... 101 | CvMat* operator&() 102 | { 103 | return &cv_mat_; 104 | } 105 | const CvMat* operator&() const 106 | { 107 | return &cv_mat_; 108 | } 109 | }; 110 | 111 | } // namespace MTK 112 | 113 | #endif /* WRAPPED_CV_MAT_HPP_ */ 114 | -------------------------------------------------------------------------------- /include/common_lib.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_LIB_H 2 | #define COMMON_LIB_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | using namespace Eigen; 16 | 17 | #define USE_IKFOM 18 | 19 | #define PI_M (3.14159265358) 20 | #define G_m_s2 (9.81) // Gravaty const in GuangDong/China 21 | #define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) 22 | #define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) 23 | #define CUBE_LEN (6.0) 24 | #define LIDAR_SP_LEN (2) 25 | #define INIT_COV (1) 26 | #define NUM_MATCH_POINTS (5) 27 | #define MAX_MEAS_DIM (10000) 28 | 29 | #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] 30 | #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] 31 | #define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) 34 | #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) 35 | 36 | typedef fast_lio::Pose6D Pose6D; 37 | typedef pcl::PointXYZINormal PointType; 38 | typedef pcl::PointCloud PointCloudXYZI; 39 | typedef vector> PointVector; 40 | typedef Vector3d V3D; 41 | typedef Matrix3d M3D; 42 | typedef Vector3f V3F; 43 | typedef Matrix3f M3F; 44 | 45 | #define MD(a,b) Matrix 46 | #define VD(a) Matrix 47 | #define MF(a,b) Matrix 48 | #define VF(a) Matrix 49 | 50 | M3D Eye3d(M3D::Identity()); 51 | M3F Eye3f(M3F::Identity()); 52 | V3D Zero3d(0, 0, 0); 53 | V3F Zero3f(0, 0, 0); 54 | 55 | struct MeasureGroup // Lidar data and imu dates for the curent process 56 | { 57 | MeasureGroup() 58 | { 59 | lidar_beg_time = 0.0; 60 | this->lidar.reset(new PointCloudXYZI()); 61 | }; 62 | double lidar_beg_time; 63 | PointCloudXYZI::Ptr lidar; 64 | deque imu; 65 | }; 66 | 67 | struct StatesGroup 68 | { 69 | StatesGroup() { 70 | this->rot_end = M3D::Identity(); 71 | this->pos_end = Zero3d; 72 | this->vel_end = Zero3d; 73 | this->bias_g = Zero3d; 74 | this->bias_a = Zero3d; 75 | this->gravity = Zero3d; 76 | this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; 77 | this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001; 78 | }; 79 | 80 | StatesGroup(const StatesGroup& b) { 81 | this->rot_end = b.rot_end; 82 | this->pos_end = b.pos_end; 83 | this->vel_end = b.vel_end; 84 | this->bias_g = b.bias_g; 85 | this->bias_a = b.bias_a; 86 | this->gravity = b.gravity; 87 | this->cov = b.cov; 88 | }; 89 | 90 | StatesGroup& operator=(const StatesGroup& b) 91 | { 92 | this->rot_end = b.rot_end; 93 | this->pos_end = b.pos_end; 94 | this->vel_end = b.vel_end; 95 | this->bias_g = b.bias_g; 96 | this->bias_a = b.bias_a; 97 | this->gravity = b.gravity; 98 | this->cov = b.cov; 99 | return *this; 100 | }; 101 | 102 | StatesGroup operator+(const Matrix &state_add) 103 | { 104 | StatesGroup a; 105 | a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 106 | a.pos_end = this->pos_end + state_add.block<3,1>(3,0); 107 | a.vel_end = this->vel_end + state_add.block<3,1>(6,0); 108 | a.bias_g = this->bias_g + state_add.block<3,1>(9,0); 109 | a.bias_a = this->bias_a + state_add.block<3,1>(12,0); 110 | a.gravity = this->gravity + state_add.block<3,1>(15,0); 111 | a.cov = this->cov; 112 | return a; 113 | }; 114 | 115 | StatesGroup& operator+=(const Matrix &state_add) 116 | { 117 | this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 118 | this->pos_end += state_add.block<3,1>(3,0); 119 | this->vel_end += state_add.block<3,1>(6,0); 120 | this->bias_g += state_add.block<3,1>(9,0); 121 | this->bias_a += state_add.block<3,1>(12,0); 122 | this->gravity += state_add.block<3,1>(15,0); 123 | return *this; 124 | }; 125 | 126 | Matrix operator-(const StatesGroup& b) 127 | { 128 | Matrix a; 129 | M3D rotd(b.rot_end.transpose() * this->rot_end); 130 | a.block<3,1>(0,0) = Log(rotd); 131 | a.block<3,1>(3,0) = this->pos_end - b.pos_end; 132 | a.block<3,1>(6,0) = this->vel_end - b.vel_end; 133 | a.block<3,1>(9,0) = this->bias_g - b.bias_g; 134 | a.block<3,1>(12,0) = this->bias_a - b.bias_a; 135 | a.block<3,1>(15,0) = this->gravity - b.gravity; 136 | return a; 137 | }; 138 | 139 | void resetpose() 140 | { 141 | this->rot_end = M3D::Identity(); 142 | this->pos_end = Zero3d; 143 | this->vel_end = Zero3d; 144 | } 145 | 146 | M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point 147 | V3D pos_end; // the estimated position at the end lidar point (world frame) 148 | V3D vel_end; // the estimated velocity at the end lidar point (world frame) 149 | V3D bias_g; // gyroscope bias 150 | V3D bias_a; // accelerator bias 151 | V3D gravity; // the estimated gravity acceleration 152 | Matrix cov; // states covariance 153 | }; 154 | 155 | template 156 | T rad2deg(T radians) 157 | { 158 | return radians * 180.0 / PI_M; 159 | } 160 | 161 | template 162 | T deg2rad(T degrees) 163 | { 164 | return degrees * PI_M / 180.0; 165 | } 166 | 167 | template 168 | auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ 169 | const Matrix &v, const Matrix &p, const Matrix &R) 170 | { 171 | Pose6D rot_kp; 172 | rot_kp.offset_time = t; 173 | for (int i = 0; i < 3; i++) 174 | { 175 | rot_kp.acc[i] = a(i); 176 | rot_kp.gyr[i] = g(i); 177 | rot_kp.vel[i] = v(i); 178 | rot_kp.pos[i] = p(i); 179 | for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); 180 | } 181 | return move(rot_kp); 182 | } 183 | 184 | /* comment 185 | plane equation: Ax + By + Cz + D = 0 186 | convert to: A/D*x + B/D*y + C/D*z = -1 187 | solve: A0*x0 = b0 188 | where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T 189 | normvec: normalized x0 190 | */ 191 | template 192 | bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) 193 | { 194 | MatrixXf A(point_num, 3); 195 | MatrixXf b(point_num, 1); 196 | b.setOnes(); 197 | b *= -1.0f; 198 | 199 | for (int j = 0; j < point_num; j++) 200 | { 201 | A(j,0) = point[j].x; 202 | A(j,1) = point[j].y; 203 | A(j,2) = point[j].z; 204 | } 205 | normvec = A.colPivHouseholderQr().solve(b); 206 | 207 | for (int j = 0; j < point_num; j++) 208 | { 209 | if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) 210 | { 211 | return false; 212 | } 213 | } 214 | 215 | normvec.normalize(); 216 | return true; 217 | } 218 | 219 | float calc_dist(PointType p1, PointType p2){ 220 | float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); 221 | return d; 222 | } 223 | 224 | template 225 | bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) 226 | { 227 | Matrix A; 228 | Matrix b; 229 | A.setZero(); 230 | b.setOnes(); 231 | b *= -1.0f; 232 | 233 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 234 | { 235 | A(j,0) = point[j].x; 236 | A(j,1) = point[j].y; 237 | A(j,2) = point[j].z; 238 | } 239 | 240 | Matrix normvec = A.colPivHouseholderQr().solve(b); 241 | 242 | T n = normvec.norm(); 243 | pca_result(0) = normvec(0) / n; 244 | pca_result(1) = normvec(1) / n; 245 | pca_result(2) = normvec(2) / n; 246 | pca_result(3) = 1.0 / n; 247 | 248 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 249 | { 250 | if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) 251 | { 252 | return false; 253 | } 254 | } 255 | return true; 256 | } 257 | 258 | #endif -------------------------------------------------------------------------------- /include/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 10 | Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) 11 | { 12 | Eigen::Matrix skew_sym_mat; 13 | skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; 14 | return skew_sym_mat; 15 | } 16 | 17 | template 18 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 19 | { 20 | T ang_norm = ang.norm(); 21 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 22 | if (ang_norm > 0.0000001) 23 | { 24 | Eigen::Matrix r_axis = ang / ang_norm; 25 | Eigen::Matrix K; 26 | K << SKEW_SYM_MATRX(r_axis); 27 | /// Roderigous Tranformation 28 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 29 | } 30 | else 31 | { 32 | return Eye3; 33 | } 34 | } 35 | 36 | template 37 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 38 | { 39 | T ang_vel_norm = ang_vel.norm(); 40 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 41 | 42 | if (ang_vel_norm > 0.0000001) 43 | { 44 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 45 | Eigen::Matrix K; 46 | 47 | K << SKEW_SYM_MATRX(r_axis); 48 | 49 | T r_ang = ang_vel_norm * dt; 50 | 51 | /// Roderigous Tranformation 52 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 53 | } 54 | else 55 | { 56 | return Eye3; 57 | } 58 | } 59 | 60 | template 61 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 62 | { 63 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 64 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 65 | if (norm > 0.00001) 66 | { 67 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 68 | Eigen::Matrix K; 69 | K << SKEW_SYM_MATRX(r_ang); 70 | 71 | /// Roderigous Tranformation 72 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 73 | } 74 | else 75 | { 76 | return Eye3; 77 | } 78 | } 79 | 80 | /* Logrithm of a Rotation Matrix */ 81 | template 82 | Eigen::Matrix Log(const Eigen::Matrix &R) 83 | { 84 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); 85 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 86 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 87 | } 88 | 89 | template 90 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) 91 | { 92 | T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); 93 | bool singular = sy < 1e-6; 94 | T x, y, z; 95 | if(!singular) 96 | { 97 | x = atan2(rot(2, 1), rot(2, 2)); 98 | y = atan2(-rot(2, 0), sy); 99 | z = atan2(rot(1, 0), rot(0, 0)); 100 | } 101 | else 102 | { 103 | x = atan2(-rot(1, 2), rot(1, 1)); 104 | y = atan2(-rot(2, 0), sy); 105 | z = 0; 106 | } 107 | Eigen::Matrix ang(x, y, z); 108 | return ang; 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /include/use-ikfom.hpp: -------------------------------------------------------------------------------- 1 | #ifndef USE_IKFOM_H 2 | #define USE_IKFOM_H 3 | 4 | #include 5 | 6 | typedef MTK::vect<3, double> vect3; 7 | typedef MTK::SO3 SO3; 8 | typedef MTK::S2 S2; 9 | typedef MTK::vect<1, double> vect1; 10 | typedef MTK::vect<2, double> vect2; 11 | 12 | MTK_BUILD_MANIFOLD(state_ikfom, 13 | ((vect3, pos)) 14 | ((SO3, rot)) 15 | ((SO3, offset_R_L_I)) 16 | ((vect3, offset_T_L_I)) 17 | ((vect3, vel)) 18 | ((vect3, bg)) 19 | ((vect3, ba)) 20 | ((S2, grav)) 21 | ); 22 | 23 | MTK_BUILD_MANIFOLD(input_ikfom, 24 | ((vect3, acc)) 25 | ((vect3, gyro)) 26 | ); 27 | 28 | MTK_BUILD_MANIFOLD(process_noise_ikfom, 29 | ((vect3, ng)) 30 | ((vect3, na)) 31 | ((vect3, nbg)) 32 | ((vect3, nba)) 33 | ); 34 | 35 | MTK::get_cov::type process_noise_cov() 36 | { 37 | MTK::get_cov::type cov = MTK::get_cov::type::Zero(); 38 | MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03 39 | MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 40 | MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 41 | MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01 42 | return cov; 43 | } 44 | 45 | //double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia 46 | //vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); 47 | Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in) 48 | { 49 | Eigen::Matrix res = Eigen::Matrix::Zero(); 50 | vect3 omega; 51 | in.gyro.boxminus(omega, s.bg); 52 | vect3 a_inertial = s.rot * (in.acc-s.ba); 53 | for(int i = 0; i < 3; i++ ){ 54 | res(i) = s.vel[i]; 55 | res(i + 3) = omega[i]; 56 | res(i + 12) = a_inertial[i] + s.grav[i]; 57 | } 58 | return res; 59 | } 60 | 61 | Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in) 62 | { 63 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 64 | cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); 65 | vect3 acc_; 66 | in.acc.boxminus(acc_, s.ba); 67 | vect3 omega; 68 | in.gyro.boxminus(omega, s.bg); 69 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); 70 | cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); 71 | Eigen::Matrix vec = Eigen::Matrix::Zero(); 72 | Eigen::Matrix grav_matrix; 73 | s.S2_Mx(grav_matrix, vec, 21); 74 | cov.template block<3, 2>(12, 21) = grav_matrix; 75 | cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 76 | return cov; 77 | } 78 | 79 | 80 | Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) 81 | { 82 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 83 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); 84 | cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); 85 | cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); 86 | cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); 87 | return cov; 88 | } 89 | 90 | vect3 SO3ToEuler(const SO3 &orient) 91 | { 92 | Eigen::Matrix _ang; 93 | Eigen::Vector4d q_data = orient.coeffs().transpose(); 94 | //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; 95 | double sqw = q_data[3]*q_data[3]; 96 | double sqx = q_data[0]*q_data[0]; 97 | double sqy = q_data[1]*q_data[1]; 98 | double sqz = q_data[2]*q_data[2]; 99 | double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor 100 | double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; 101 | 102 | if (test > 0.49999*unit) { // singularity at north pole 103 | 104 | _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; 105 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 106 | vect3 euler_ang(temp, 3); 107 | return euler_ang; 108 | } 109 | if (test < -0.49999*unit) { // singularity at south pole 110 | _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; 111 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 112 | vect3 euler_ang(temp, 3); 113 | return euler_ang; 114 | } 115 | 116 | _ang << 117 | std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), 118 | std::asin (2*test/unit), 119 | std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); 120 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 121 | vect3 euler_ang(temp, 3); 122 | // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw 123 | return euler_ang; 124 | } 125 | 126 | #endif -------------------------------------------------------------------------------- /launch/gdb_debug_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/localization_avia.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /launch/localization_horizon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /launch/localization_ouster64.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /launch/localization_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /msg/Pose6D.msg: -------------------------------------------------------------------------------- 1 | # the preintegrated Lidar states at the time of IMU measurements in a frame 2 | float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point 3 | float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin 4 | float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin 5 | float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin 6 | float64[3] pos # the preintegrated position (global frame) at the Lidar origin 7 | float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_lio_localization 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 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | sensor_msgs 26 | tf 27 | pcl_ros 28 | livox_ros_driver 29 | message_generation 30 | 31 | geometry_msgs 32 | nav_msgs 33 | sensor_msgs 34 | roscpp 35 | rospy 36 | std_msgs 37 | tf 38 | pcl_ros 39 | livox_ros_driver 40 | message_runtime 41 | 42 | rostest 43 | rosbag 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /rviz_cfg/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HViktorTsoi/FAST_LIO_LOCALIZATION/2bc274ed0b36d14a5c34ada5e1473d52aa1db0d2/rviz_cfg/.gitignore -------------------------------------------------------------------------------- /rviz_cfg/loam_livox.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /mapping1 9 | - /mapping1/surround1 10 | - /mapping1/currPoints1 11 | - /mapping1/currPoints1/Autocompute Value Bounds1 12 | - /Odometry1/Odometry1 13 | - /Odometry1/Odometry1/Shape1 14 | - /Odometry1/Odometry1/Covariance1 15 | - /Odometry1/Odometry1/Covariance1/Position1 16 | - /Odometry1/Odometry1/Covariance1/Orientation1 17 | - /MarkerArray1/Namespaces1 18 | Splitter Ratio: 0.6432291865348816 19 | Tree Height: 1139 20 | - Class: rviz/Selection 21 | Name: Selection 22 | - Class: rviz/Tool Properties 23 | Expanded: 24 | - /2D Pose Estimate1 25 | - /2D Nav Goal1 26 | - /Publish Point1 27 | Name: Tool Properties 28 | Splitter Ratio: 0.5886790156364441 29 | - Class: rviz/Views 30 | Expanded: 31 | - /Current View1 32 | Name: Views 33 | Splitter Ratio: 0.5 34 | - Class: rviz/Time 35 | Experimental: false 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: surround 39 | Preferences: 40 | PromptSaveOnExit: true 41 | Toolbars: 42 | toolButtonStyle: 2 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 1 47 | Cell Size: 1000 48 | Class: rviz/Grid 49 | Color: 160; 160; 164 50 | Enabled: false 51 | Line Style: 52 | Line Width: 0.029999999329447746 53 | Value: Lines 54 | Name: Grid 55 | Normal Cell Count: 0 56 | Offset: 57 | X: 0 58 | Y: 0 59 | Z: 0 60 | Plane: XY 61 | Plane Cell Count: 40 62 | Reference Frame: 63 | Value: false 64 | - Class: rviz/Axes 65 | Enabled: false 66 | Length: 0.699999988079071 67 | Name: Axes 68 | Radius: 0.05999999865889549 69 | Reference Frame: 70 | Value: false 71 | - Class: rviz/Group 72 | Displays: 73 | - Alpha: 1 74 | Autocompute Intensity Bounds: true 75 | Autocompute Value Bounds: 76 | Max Value: 10 77 | Min Value: -10 78 | Value: true 79 | Axis: Z 80 | Channel Name: intensity 81 | Class: rviz/PointCloud2 82 | Color: 238; 238; 236 83 | Color Transformer: Intensity 84 | Decay Time: 0 85 | Enabled: true 86 | Invert Rainbow: false 87 | Max Color: 255; 255; 255 88 | Max Intensity: 255 89 | Min Color: 0; 0; 0 90 | Min Intensity: 0 91 | Name: surround 92 | Position Transformer: XYZ 93 | Queue Size: 1 94 | Selectable: false 95 | Size (Pixels): 1 96 | Size (m): 0.05000000074505806 97 | Style: Points 98 | Topic: /cloud_registered 99 | Unreliable: false 100 | Use Fixed Frame: true 101 | Use rainbow: true 102 | Value: true 103 | - Alpha: 0.6000000238418579 104 | Autocompute Intensity Bounds: false 105 | Autocompute Value Bounds: 106 | Max Value: 15 107 | Min Value: -5 108 | Value: false 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/PointCloud2 112 | Color: 255; 255; 255 113 | Color Transformer: Intensity 114 | Decay Time: 1000 115 | Enabled: false 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Max Intensity: 70 119 | Min Color: 0; 0; 0 120 | Min Intensity: 0 121 | Name: currPoints 122 | Position Transformer: XYZ 123 | Queue Size: 100000 124 | Selectable: true 125 | Size (Pixels): 1 126 | Size (m): 0.009999999776482582 127 | Style: Points 128 | Topic: /cloud_registered 129 | Unreliable: false 130 | Use Fixed Frame: true 131 | Use rainbow: true 132 | Value: false 133 | - Alpha: 1 134 | Autocompute Intensity Bounds: true 135 | Autocompute Value Bounds: 136 | Max Value: 10 137 | Min Value: -10 138 | Value: true 139 | Axis: Z 140 | Channel Name: intensity 141 | Class: rviz/PointCloud2 142 | Color: 255; 0; 0 143 | Color Transformer: FlatColor 144 | Decay Time: 0 145 | Enabled: false 146 | Invert Rainbow: false 147 | Max Color: 255; 255; 255 148 | Max Intensity: 151 149 | Min Color: 0; 0; 0 150 | Min Intensity: 0 151 | Name: PointCloud2 152 | Position Transformer: XYZ 153 | Queue Size: 10 154 | Selectable: true 155 | Size (Pixels): 3 156 | Size (m): 0.10000000149011612 157 | Style: Flat Squares 158 | Topic: /Laser_map 159 | Unreliable: false 160 | Use Fixed Frame: true 161 | Use rainbow: true 162 | Value: false 163 | Enabled: true 164 | Name: mapping 165 | - Class: rviz/Group 166 | Displays: 167 | - Angle Tolerance: 0.009999999776482582 168 | Class: rviz/Odometry 169 | Covariance: 170 | Orientation: 171 | Alpha: 0.5 172 | Color: 255; 255; 127 173 | Color Style: Unique 174 | Frame: Local 175 | Offset: 1 176 | Scale: 1 177 | Value: true 178 | Position: 179 | Alpha: 0.30000001192092896 180 | Color: 204; 51; 204 181 | Scale: 1 182 | Value: true 183 | Value: true 184 | Enabled: true 185 | Keep: 1 186 | Name: Odometry 187 | Position Tolerance: 0.0010000000474974513 188 | Shape: 189 | Alpha: 1 190 | Axes Length: 1 191 | Axes Radius: 0.20000000298023224 192 | Color: 255; 85; 0 193 | Head Length: 0 194 | Head Radius: 0 195 | Shaft Length: 0.05000000074505806 196 | Shaft Radius: 0.05000000074505806 197 | Value: Axes 198 | Topic: /Odometry 199 | Unreliable: false 200 | Value: true 201 | Enabled: true 202 | Name: Odometry 203 | - Class: rviz/Axes 204 | Enabled: true 205 | Length: 0.699999988079071 206 | Name: Axes 207 | Radius: 0.10000000149011612 208 | Reference Frame: 209 | Value: true 210 | - Alpha: 0 211 | Buffer Length: 2 212 | Class: rviz/Path 213 | Color: 25; 255; 255 214 | Enabled: true 215 | Head Diameter: 0 216 | Head Length: 0 217 | Length: 0.30000001192092896 218 | Line Style: Billboards 219 | Line Width: 0.20000000298023224 220 | Name: Path 221 | Offset: 222 | X: 0 223 | Y: 0 224 | Z: 0 225 | Pose Color: 25; 255; 255 226 | Pose Style: None 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: 1 234 | Autocompute Intensity Bounds: false 235 | Autocompute Value Bounds: 236 | Max Value: 10 237 | Min Value: -10 238 | Value: true 239 | Axis: Z 240 | Channel Name: intensity 241 | Class: rviz/PointCloud2 242 | Color: 255; 255; 255 243 | Color Transformer: Intensity 244 | Decay Time: 0 245 | Enabled: false 246 | Invert Rainbow: false 247 | Max Color: 239; 41; 41 248 | Max Intensity: 0 249 | Min Color: 239; 41; 41 250 | Min Intensity: 0 251 | Name: PointCloud2 252 | Position Transformer: XYZ 253 | Queue Size: 10 254 | Selectable: true 255 | Size (Pixels): 4 256 | Size (m): 0.30000001192092896 257 | Style: Spheres 258 | Topic: /cloud_effected 259 | Unreliable: false 260 | Use Fixed Frame: true 261 | Use rainbow: true 262 | Value: false 263 | - Alpha: 1 264 | Autocompute Intensity Bounds: true 265 | Autocompute Value Bounds: 266 | Max Value: 13.139549255371094 267 | Min Value: -32.08251953125 268 | Value: true 269 | Axis: Z 270 | Channel Name: intensity 271 | Class: rviz/PointCloud2 272 | Color: 138; 226; 52 273 | Color Transformer: FlatColor 274 | Decay Time: 0 275 | Enabled: false 276 | Invert Rainbow: false 277 | Max Color: 138; 226; 52 278 | Max Intensity: 248 279 | Min Color: 138; 226; 52 280 | Min Intensity: 0 281 | Name: PointCloud2 282 | Position Transformer: XYZ 283 | Queue Size: 10 284 | Selectable: true 285 | Size (Pixels): 3 286 | Size (m): 0.10000000149011612 287 | Style: Flat Squares 288 | Topic: /Laser_map 289 | Unreliable: false 290 | Use Fixed Frame: true 291 | Use rainbow: true 292 | Value: false 293 | - Class: rviz/MarkerArray 294 | Enabled: false 295 | Marker Topic: /MarkerArray 296 | Name: MarkerArray 297 | Namespaces: 298 | {} 299 | Queue Size: 100 300 | Value: false 301 | Enabled: true 302 | Global Options: 303 | Background Color: 0; 0; 0 304 | Default Light: true 305 | Fixed Frame: camera_init 306 | Frame Rate: 10 307 | Name: root 308 | Tools: 309 | - Class: rviz/Interact 310 | Hide Inactive Objects: true 311 | - Class: rviz/MoveCamera 312 | - Class: rviz/Select 313 | - Class: rviz/FocusCamera 314 | - Class: rviz/Measure 315 | - Class: rviz/SetInitialPose 316 | Theta std deviation: 0.2617993950843811 317 | Topic: /initialpose 318 | X std deviation: 0.5 319 | Y std deviation: 0.5 320 | - Class: rviz/SetGoal 321 | Topic: /move_base_simple/goal 322 | - Class: rviz/PublishPoint 323 | Single click: true 324 | Topic: /clicked_point 325 | Value: true 326 | Views: 327 | Current: 328 | Class: rviz/Orbit 329 | Distance: 19.453998565673828 330 | Enable Stereo Rendering: 331 | Stereo Eye Separation: 0.05999999865889549 332 | Stereo Focal Distance: 1 333 | Swap Stereo Eyes: false 334 | Value: false 335 | Focal Point: 336 | X: 0 337 | Y: 0 338 | Z: 0 339 | Focal Shape Fixed Size: false 340 | Focal Shape Size: 0.05000000074505806 341 | Invert Z Axis: false 342 | Name: Current View 343 | Near Clip Distance: 0.009999999776482582 344 | Pitch: 1.0753980875015259 345 | Target Frame: body 346 | Value: Orbit (rviz) 347 | Yaw: 2.4953973293304443 348 | Saved: ~ 349 | Window Geometry: 350 | Displays: 351 | collapsed: false 352 | Height: 1385 353 | Hide Left Dock: false 354 | Hide Right Dock: false 355 | QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004b3fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000004b3000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000004b3000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009c100000052fc0100000002fb0000000800540069006d00650100000000000009c10000031300fffffffb0000000800540069006d006501000000000000045000000000000000000000069b000004b300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 356 | Selection: 357 | collapsed: false 358 | Time: 359 | collapsed: false 360 | Tool Properties: 361 | collapsed: false 362 | Views: 363 | collapsed: false 364 | Width: 2497 365 | X: 63 366 | Y: 27 367 | -------------------------------------------------------------------------------- /rviz_cfg/localization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Grid1 9 | - /mapping1/surround1 10 | - /mapping1/currPoints1 11 | - /mapping1/currPoints1/Autocompute Value Bounds1 12 | - /Odometry1/Odometry1 13 | - /Odometry1/Odometry1/Shape1 14 | - /Odometry1/Odometry1/Covariance1 15 | - /Odometry1/Odometry1/Covariance1/Position1 16 | - /Odometry1/Odometry1/Covariance1/Orientation1 17 | - /Localization1 18 | - /Localization1/map1 19 | - /Localization1/localization1 20 | - /Localization1/localization1/Shape1 21 | - /MarkerArray1/Namespaces1 22 | Splitter Ratio: 0.6382352709770203 23 | Tree Height: 1139 24 | - Class: rviz/Selection 25 | Name: Selection 26 | - Class: rviz/Tool Properties 27 | Expanded: 28 | - /2D Pose Estimate1 29 | - /2D Nav Goal1 30 | - /Publish Point1 31 | Name: Tool Properties 32 | Splitter Ratio: 0.5886790156364441 33 | - Class: rviz/Views 34 | Expanded: 35 | - /Current View1 36 | Name: Views 37 | Splitter Ratio: 0.5 38 | - Class: rviz/Time 39 | Experimental: false 40 | Name: Time 41 | SyncMode: 0 42 | SyncSource: map 43 | Preferences: 44 | PromptSaveOnExit: true 45 | Toolbars: 46 | toolButtonStyle: 2 47 | Visualization Manager: 48 | Class: "" 49 | Displays: 50 | - Alpha: 1 51 | Cell Size: 0.20000000298023224 52 | Class: rviz/Grid 53 | Color: 160; 160; 164 54 | Enabled: true 55 | Line Style: 56 | Line Width: 0.029999999329447746 57 | Value: Lines 58 | Name: Grid 59 | Normal Cell Count: 0 60 | Offset: 61 | X: 0 62 | Y: 0 63 | Z: 0 64 | Plane: XY 65 | Plane Cell Count: 10 66 | Reference Frame: body 67 | Value: true 68 | - Class: rviz/Axes 69 | Enabled: false 70 | Length: 0.699999988079071 71 | Name: Axes 72 | Radius: 0.05999999865889549 73 | Reference Frame: 74 | Value: false 75 | - Class: rviz/Group 76 | Displays: 77 | - Alpha: 1 78 | Autocompute Intensity Bounds: true 79 | Autocompute Value Bounds: 80 | Max Value: 10 81 | Min Value: -10 82 | Value: true 83 | Axis: Z 84 | Channel Name: intensity 85 | Class: rviz/PointCloud2 86 | Color: 238; 238; 236 87 | Color Transformer: Intensity 88 | Decay Time: 0 89 | Enabled: true 90 | Invert Rainbow: false 91 | Max Color: 255; 255; 255 92 | Max Intensity: 255 93 | Min Color: 0; 0; 0 94 | Min Intensity: 0 95 | Name: surround 96 | Position Transformer: XYZ 97 | Queue Size: 1 98 | Selectable: false 99 | Size (Pixels): 1 100 | Size (m): 0.05000000074505806 101 | Style: Points 102 | Topic: /cloud_registered 103 | Unreliable: false 104 | Use Fixed Frame: true 105 | Use rainbow: true 106 | Value: true 107 | - Alpha: 0.6000000238418579 108 | Autocompute Intensity Bounds: false 109 | Autocompute Value Bounds: 110 | Max Value: 15 111 | Min Value: -5 112 | Value: false 113 | Axis: Z 114 | Channel Name: intensity 115 | Class: rviz/PointCloud2 116 | Color: 255; 255; 255 117 | Color Transformer: Intensity 118 | Decay Time: 1000 119 | Enabled: false 120 | Invert Rainbow: false 121 | Max Color: 255; 255; 255 122 | Max Intensity: 70 123 | Min Color: 0; 0; 0 124 | Min Intensity: 0 125 | Name: currPoints 126 | Position Transformer: XYZ 127 | Queue Size: 100000 128 | Selectable: true 129 | Size (Pixels): 1 130 | Size (m): 0.009999999776482582 131 | Style: Points 132 | Topic: /cloud_registered 133 | Unreliable: false 134 | Use Fixed Frame: true 135 | Use rainbow: true 136 | Value: false 137 | - Alpha: 1 138 | Autocompute Intensity Bounds: true 139 | Autocompute Value Bounds: 140 | Max Value: 10 141 | Min Value: -10 142 | Value: true 143 | Axis: Z 144 | Channel Name: intensity 145 | Class: rviz/PointCloud2 146 | Color: 255; 0; 0 147 | Color Transformer: FlatColor 148 | Decay Time: 0 149 | Enabled: false 150 | Invert Rainbow: false 151 | Max Color: 255; 255; 255 152 | Max Intensity: 151 153 | Min Color: 0; 0; 0 154 | Min Intensity: 0 155 | Name: PointCloud2 156 | Position Transformer: XYZ 157 | Queue Size: 10 158 | Selectable: true 159 | Size (Pixels): 3 160 | Size (m): 0.10000000149011612 161 | Style: Flat Squares 162 | Topic: /Laser_map 163 | Unreliable: false 164 | Use Fixed Frame: true 165 | Use rainbow: true 166 | Value: false 167 | Enabled: false 168 | Name: mapping 169 | - Class: rviz/Group 170 | Displays: 171 | - Angle Tolerance: 0.009999999776482582 172 | Class: rviz/Odometry 173 | Covariance: 174 | Orientation: 175 | Alpha: 0.5 176 | Color: 255; 255; 127 177 | Color Style: Unique 178 | Frame: Local 179 | Offset: 1 180 | Scale: 1 181 | Value: true 182 | Position: 183 | Alpha: 0.30000001192092896 184 | Color: 204; 51; 204 185 | Scale: 1 186 | Value: true 187 | Value: true 188 | Enabled: true 189 | Keep: 1 190 | Name: Odometry 191 | Position Tolerance: 0.0010000000474974513 192 | Shape: 193 | Alpha: 1 194 | Axes Length: 1 195 | Axes Radius: 0.20000000298023224 196 | Color: 255; 85; 0 197 | Head Length: 0 198 | Head Radius: 0 199 | Shaft Length: 0.05000000074505806 200 | Shaft Radius: 0.05000000074505806 201 | Value: Axes 202 | Topic: /Odometry 203 | Unreliable: false 204 | Value: true 205 | Enabled: false 206 | Name: Odometry 207 | - Class: rviz/Group 208 | Displays: 209 | - Alpha: 1 210 | Autocompute Intensity Bounds: true 211 | Autocompute Value Bounds: 212 | Max Value: 10 213 | Min Value: -10 214 | Value: true 215 | Axis: Z 216 | Channel Name: intensity 217 | Class: rviz/PointCloud2 218 | Color: 252; 233; 79 219 | Color Transformer: FlatColor 220 | Decay Time: 0 221 | Enabled: true 222 | Invert Rainbow: false 223 | Max Color: 255; 255; 255 224 | Max Intensity: 255 225 | Min Color: 0; 0; 0 226 | Min Intensity: 0 227 | Name: cur_scan_in_map 228 | Position Transformer: XYZ 229 | Queue Size: 10 230 | Selectable: true 231 | Size (Pixels): 3 232 | Size (m): 0.009999999776482582 233 | Style: Points 234 | Topic: /cur_scan_in_map 235 | Unreliable: false 236 | Use Fixed Frame: true 237 | Use rainbow: true 238 | Value: true 239 | - Alpha: 0.5 240 | Autocompute Intensity Bounds: true 241 | Autocompute Value Bounds: 242 | Max Value: 10 243 | Min Value: -10 244 | Value: true 245 | Axis: Z 246 | Channel Name: intensity 247 | Class: rviz/PointCloud2 248 | Color: 138; 226; 52 249 | Color Transformer: FlatColor 250 | Decay Time: 0 251 | Enabled: true 252 | Invert Rainbow: false 253 | Max Color: 255; 255; 255 254 | Max Intensity: 0 255 | Min Color: 0; 0; 0 256 | Min Intensity: 0 257 | Name: submap_in_FOV 258 | Position Transformer: XYZ 259 | Queue Size: 10 260 | Selectable: true 261 | Size (Pixels): 3 262 | Size (m): 0.009999999776482582 263 | Style: Points 264 | Topic: /submap 265 | Unreliable: false 266 | Use Fixed Frame: true 267 | Use rainbow: true 268 | Value: true 269 | - Alpha: 0.6000000238418579 270 | Autocompute Intensity Bounds: true 271 | Autocompute Value Bounds: 272 | Max Value: 10 273 | Min Value: -10 274 | Value: true 275 | Axis: Z 276 | Channel Name: intensity 277 | Class: rviz/PointCloud2 278 | Color: 255; 25; 0 279 | Color Transformer: FlatColor 280 | Decay Time: 0 281 | Enabled: true 282 | Invert Rainbow: false 283 | Max Color: 255; 255; 255 284 | Max Intensity: 255 285 | Min Color: 0; 0; 0 286 | Min Intensity: 0 287 | Name: map 288 | Position Transformer: XYZ 289 | Queue Size: 10 290 | Selectable: true 291 | Size (Pixels): 1 292 | Size (m): 0.009999999776482582 293 | Style: Points 294 | Topic: /map 295 | Unreliable: false 296 | Use Fixed Frame: true 297 | Use rainbow: true 298 | Value: true 299 | - Angle Tolerance: 0.10000000149011612 300 | Class: rviz/Odometry 301 | Covariance: 302 | Orientation: 303 | Alpha: 0.5 304 | Color: 255; 255; 127 305 | Color Style: Unique 306 | Frame: Local 307 | Offset: 1 308 | Scale: 1 309 | Value: true 310 | Position: 311 | Alpha: 0.30000001192092896 312 | Color: 204; 51; 204 313 | Scale: 1 314 | Value: true 315 | Value: true 316 | Enabled: true 317 | Keep: 500 318 | Name: localization 319 | Position Tolerance: 0.10000000149011612 320 | Shape: 321 | Alpha: 1 322 | Axes Length: 1 323 | Axes Radius: 0.10000000149011612 324 | Color: 138; 226; 52 325 | Head Length: 0.30000001192092896 326 | Head Radius: 0.20000000298023224 327 | Shaft Length: 1 328 | Shaft Radius: 0.10000000149011612 329 | Value: Arrow 330 | Topic: /localization 331 | Unreliable: false 332 | Value: true 333 | Enabled: true 334 | Name: Localization 335 | - Class: rviz/Axes 336 | Enabled: true 337 | Length: 0.699999988079071 338 | Name: Axes 339 | Radius: 0.10000000149011612 340 | Reference Frame: 341 | Value: true 342 | - Alpha: 0 343 | Buffer Length: 2 344 | Class: rviz/Path 345 | Color: 25; 255; 255 346 | Enabled: true 347 | Head Diameter: 0 348 | Head Length: 0 349 | Length: 0.30000001192092896 350 | Line Style: Billboards 351 | Line Width: 0.20000000298023224 352 | Name: Path 353 | Offset: 354 | X: 0 355 | Y: 0 356 | Z: 0 357 | Pose Color: 25; 255; 255 358 | Pose Style: None 359 | Radius: 0.029999999329447746 360 | Shaft Diameter: 0.4000000059604645 361 | Shaft Length: 0.4000000059604645 362 | Topic: /path 363 | Unreliable: false 364 | Value: true 365 | - Class: rviz/MarkerArray 366 | Enabled: false 367 | Marker Topic: /MarkerArray 368 | Name: MarkerArray 369 | Namespaces: 370 | {} 371 | Queue Size: 100 372 | Value: false 373 | Enabled: true 374 | Global Options: 375 | Background Color: 0; 0; 0 376 | Default Light: true 377 | Fixed Frame: map 378 | Frame Rate: 20 379 | Name: root 380 | Tools: 381 | - Class: rviz/Interact 382 | Hide Inactive Objects: true 383 | - Class: rviz/MoveCamera 384 | - Class: rviz/Select 385 | - Class: rviz/FocusCamera 386 | - Class: rviz/Measure 387 | - Class: rviz/SetInitialPose 388 | Theta std deviation: 0.2617993950843811 389 | Topic: /initialpose 390 | X std deviation: 0.5 391 | Y std deviation: 0.5 392 | - Class: rviz/SetGoal 393 | Topic: /move_base_simple/goal 394 | - Class: rviz/PublishPoint 395 | Single click: true 396 | Topic: /clicked_point 397 | Value: true 398 | Views: 399 | Current: 400 | Class: rviz/Orbit 401 | Distance: 52.648162841796875 402 | Enable Stereo Rendering: 403 | Stereo Eye Separation: 0.05999999865889549 404 | Stereo Focal Distance: 1 405 | Swap Stereo Eyes: false 406 | Value: false 407 | Focal Point: 408 | X: 17.5119571685791 409 | Y: -13.623329162597656 410 | Z: 4.375192642211914 411 | Focal Shape Fixed Size: false 412 | Focal Shape Size: 0.05000000074505806 413 | Invert Z Axis: false 414 | Name: Current View 415 | Near Clip Distance: 0.009999999776482582 416 | Pitch: 1.5697963237762451 417 | Target Frame: body 418 | Value: Orbit (rviz) 419 | Yaw: 1.278587818145752 420 | Saved: ~ 421 | Window Geometry: 422 | Displays: 423 | collapsed: false 424 | Height: 1385 425 | Hide Left Dock: false 426 | Hide Right Dock: true 427 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004b3fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000004b3000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000004b3000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009c100000052fc0100000002fb0000000800540069006d00650100000000000009c10000031300fffffffb0000000800540069006d0065010000000000000450000000000000000000000865000004b300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 428 | Selection: 429 | collapsed: false 430 | Time: 431 | collapsed: false 432 | Tool Properties: 433 | collapsed: false 434 | Views: 435 | collapsed: true 436 | Width: 2497 437 | X: 63 438 | Y: 27 439 | -------------------------------------------------------------------------------- /scripts/global_localization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # coding=utf8 3 | from __future__ import print_function, division, absolute_import 4 | 5 | import copy 6 | import thread 7 | import time 8 | 9 | import open3d as o3d 10 | import rospy 11 | import ros_numpy 12 | from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion 13 | from nav_msgs.msg import Odometry 14 | from sensor_msgs.msg import PointCloud2 15 | import numpy as np 16 | import tf 17 | import tf.transformations 18 | 19 | global_map = None 20 | initialized = False 21 | T_map_to_odom = np.eye(4) 22 | cur_odom = None 23 | cur_scan = None 24 | 25 | 26 | def pose_to_mat(pose_msg): 27 | return np.matmul( 28 | tf.listener.xyz_to_mat44(pose_msg.pose.pose.position), 29 | tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation), 30 | ) 31 | 32 | 33 | def msg_to_array(pc_msg): 34 | pc_array = ros_numpy.numpify(pc_msg) 35 | pc = np.zeros([len(pc_array), 3]) 36 | pc[:, 0] = pc_array['x'] 37 | pc[:, 1] = pc_array['y'] 38 | pc[:, 2] = pc_array['z'] 39 | return pc 40 | 41 | 42 | def registration_at_scale(pc_scan, pc_map, initial, scale): 43 | result_icp = o3d.registration.registration_icp( 44 | voxel_down_sample(pc_scan, SCAN_VOXEL_SIZE * scale), voxel_down_sample(pc_map, MAP_VOXEL_SIZE * scale), 45 | 1.0 * scale, initial, 46 | o3d.registration.TransformationEstimationPointToPoint(), 47 | o3d.registration.ICPConvergenceCriteria(max_iteration=20) 48 | ) 49 | 50 | return result_icp.transformation, result_icp.fitness 51 | 52 | 53 | def inverse_se3(trans): 54 | trans_inverse = np.eye(4) 55 | # R 56 | trans_inverse[:3, :3] = trans[:3, :3].T 57 | # t 58 | trans_inverse[:3, 3] = -np.matmul(trans[:3, :3].T, trans[:3, 3]) 59 | return trans_inverse 60 | 61 | 62 | def publish_point_cloud(publisher, header, pc): 63 | data = np.zeros(len(pc), dtype=[ 64 | ('x', np.float32), 65 | ('y', np.float32), 66 | ('z', np.float32), 67 | ('intensity', np.float32), 68 | ]) 69 | data['x'] = pc[:, 0] 70 | data['y'] = pc[:, 1] 71 | data['z'] = pc[:, 2] 72 | if pc.shape[1] == 4: 73 | data['intensity'] = pc[:, 3] 74 | msg = ros_numpy.msgify(PointCloud2, data) 75 | msg.header = header 76 | publisher.publish(msg) 77 | 78 | 79 | def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom): 80 | # 当前scan原点的位姿 81 | T_odom_to_base_link = pose_to_mat(cur_odom) 82 | T_map_to_base_link = np.matmul(pose_estimation, T_odom_to_base_link) 83 | T_base_link_to_map = inverse_se3(T_map_to_base_link) 84 | 85 | # 把地图转换到lidar系下 86 | global_map_in_map = np.array(global_map.points) 87 | global_map_in_map = np.column_stack([global_map_in_map, np.ones(len(global_map_in_map))]) 88 | global_map_in_base_link = np.matmul(T_base_link_to_map, global_map_in_map.T).T 89 | 90 | # 将视角内的地图点提取出来 91 | if FOV > 3.14: 92 | # 环状lidar 仅过滤距离 93 | indices = np.where( 94 | (global_map_in_base_link[:, 0] < FOV_FAR) & 95 | (np.abs(np.arctan2(global_map_in_base_link[:, 1], global_map_in_base_link[:, 0])) < FOV / 2.0) 96 | ) 97 | else: 98 | # 非环状lidar 保前视范围 99 | # FOV_FAR>x>0 且角度小于FOV 100 | indices = np.where( 101 | (global_map_in_base_link[:, 0] > 0) & 102 | (global_map_in_base_link[:, 0] < FOV_FAR) & 103 | (np.abs(np.arctan2(global_map_in_base_link[:, 1], global_map_in_base_link[:, 0])) < FOV / 2.0) 104 | ) 105 | global_map_in_FOV = o3d.geometry.PointCloud() 106 | global_map_in_FOV.points = o3d.utility.Vector3dVector(np.squeeze(global_map_in_map[indices, :3])) 107 | 108 | # 发布fov内点云 109 | header = cur_odom.header 110 | header.frame_id = 'map' 111 | publish_point_cloud(pub_submap, header, np.array(global_map_in_FOV.points)[::10]) 112 | 113 | return global_map_in_FOV 114 | 115 | 116 | def global_localization(pose_estimation): 117 | global global_map, cur_scan, cur_odom, T_map_to_odom 118 | # 用icp配准 119 | # print(global_map, cur_scan, T_map_to_odom) 120 | rospy.loginfo('Global localization by scan-to-map matching......') 121 | 122 | # TODO 这里注意线程安全 123 | scan_tobe_mapped = copy.copy(cur_scan) 124 | 125 | tic = time.time() 126 | 127 | global_map_in_FOV = crop_global_map_in_FOV(global_map, pose_estimation, cur_odom) 128 | 129 | # 粗配准 130 | transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=5) 131 | 132 | # 精配准 133 | transformation, fitness = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=transformation, 134 | scale=1) 135 | toc = time.time() 136 | rospy.loginfo('Time: {}'.format(toc - tic)) 137 | rospy.loginfo('') 138 | 139 | # 当全局定位成功时才更新map2odom 140 | if fitness > LOCALIZATION_TH: 141 | # T_map_to_odom = np.matmul(transformation, pose_estimation) 142 | T_map_to_odom = transformation 143 | 144 | # 发布map_to_odom 145 | map_to_odom = Odometry() 146 | xyz = tf.transformations.translation_from_matrix(T_map_to_odom) 147 | quat = tf.transformations.quaternion_from_matrix(T_map_to_odom) 148 | map_to_odom.pose.pose = Pose(Point(*xyz), Quaternion(*quat)) 149 | map_to_odom.header.stamp = cur_odom.header.stamp 150 | map_to_odom.header.frame_id = 'map' 151 | pub_map_to_odom.publish(map_to_odom) 152 | return True 153 | else: 154 | rospy.logwarn('Not match!!!!') 155 | rospy.logwarn('{}'.format(transformation)) 156 | rospy.logwarn('fitness score:{}'.format(fitness)) 157 | return False 158 | 159 | 160 | def voxel_down_sample(pcd, voxel_size): 161 | try: 162 | pcd_down = pcd.voxel_down_sample(voxel_size) 163 | except: 164 | # for opend3d 0.7 or lower 165 | pcd_down = o3d.geometry.voxel_down_sample(pcd, voxel_size) 166 | return pcd_down 167 | 168 | 169 | def initialize_global_map(pc_msg): 170 | global global_map 171 | 172 | global_map = o3d.geometry.PointCloud() 173 | global_map.points = o3d.utility.Vector3dVector(msg_to_array(pc_msg)[:, :3]) 174 | global_map = voxel_down_sample(global_map, MAP_VOXEL_SIZE) 175 | rospy.loginfo('Global map received.') 176 | 177 | 178 | def cb_save_cur_odom(odom_msg): 179 | global cur_odom 180 | cur_odom = odom_msg 181 | 182 | 183 | def cb_save_cur_scan(pc_msg): 184 | global cur_scan 185 | # 注意这里fastlio直接将scan转到odom系下了 不是lidar局部系 186 | pc_msg.header.frame_id = 'camera_init' 187 | pc_msg.header.stamp = rospy.Time().now() 188 | pub_pc_in_map.publish(pc_msg) 189 | 190 | # 转换为pcd 191 | # fastlio给的field有问题 处理一下 192 | pc_msg.fields = [pc_msg.fields[0], pc_msg.fields[1], pc_msg.fields[2], 193 | pc_msg.fields[4], pc_msg.fields[5], pc_msg.fields[6], 194 | pc_msg.fields[3], pc_msg.fields[7]] 195 | pc = msg_to_array(pc_msg) 196 | 197 | cur_scan = o3d.geometry.PointCloud() 198 | cur_scan.points = o3d.utility.Vector3dVector(pc[:, :3]) 199 | 200 | 201 | def thread_localization(): 202 | global T_map_to_odom 203 | while True: 204 | # 每隔一段时间进行全局定位 205 | rospy.sleep(1 / FREQ_LOCALIZATION) 206 | # TODO 由于这里Fast lio发布的scan是已经转换到odom系下了 所以每次全局定位的初始解就是上一次的map2odom 不需要再拿odom了 207 | global_localization(T_map_to_odom) 208 | 209 | 210 | if __name__ == '__main__': 211 | MAP_VOXEL_SIZE = 0.4 212 | SCAN_VOXEL_SIZE = 0.1 213 | 214 | # Global localization frequency (HZ) 215 | FREQ_LOCALIZATION = 0.5 216 | 217 | # The threshold of global localization, 218 | # only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken 219 | LOCALIZATION_TH = 0.95 220 | 221 | # FOV(rad), modify this according to your LiDAR type 222 | FOV = 1.6 223 | 224 | # The farthest distance(meters) within FOV 225 | FOV_FAR = 150 226 | 227 | rospy.init_node('fast_lio_localization') 228 | rospy.loginfo('Localization Node Inited...') 229 | 230 | # publisher 231 | pub_pc_in_map = rospy.Publisher('/cur_scan_in_map', PointCloud2, queue_size=1) 232 | pub_submap = rospy.Publisher('/submap', PointCloud2, queue_size=1) 233 | pub_map_to_odom = rospy.Publisher('/map_to_odom', Odometry, queue_size=1) 234 | 235 | rospy.Subscriber('/cloud_registered', PointCloud2, cb_save_cur_scan, queue_size=1) 236 | rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1) 237 | 238 | # 初始化全局地图 239 | rospy.logwarn('Waiting for global map......') 240 | initialize_global_map(rospy.wait_for_message('/map', PointCloud2)) 241 | 242 | # 初始化 243 | while not initialized: 244 | rospy.logwarn('Waiting for initial pose....') 245 | 246 | # 等待初始位姿 247 | pose_msg = rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped) 248 | initial_pose = pose_to_mat(pose_msg) 249 | if cur_scan: 250 | initialized = global_localization(initial_pose) 251 | else: 252 | rospy.logwarn('First scan not received!!!!!') 253 | 254 | rospy.loginfo('') 255 | rospy.loginfo('Initialize successfully!!!!!!') 256 | rospy.loginfo('') 257 | # 开始定期全局定位 258 | thread.start_new_thread(thread_localization, ()) 259 | 260 | rospy.spin() 261 | -------------------------------------------------------------------------------- /scripts/publish_initial_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # coding=utf8 3 | from __future__ import print_function, division, absolute_import 4 | 5 | import argparse 6 | 7 | import rospy 8 | import tf.transformations 9 | from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped 10 | 11 | if __name__ == '__main__': 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument('x', type=float) 14 | parser.add_argument('y', type=float) 15 | parser.add_argument('z', type=float) 16 | parser.add_argument('yaw', type=float) 17 | parser.add_argument('pitch', type=float) 18 | parser.add_argument('roll', type=float) 19 | args = parser.parse_args() 20 | 21 | rospy.init_node('publish_initial_pose') 22 | pub_pose = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1) 23 | 24 | # 转换为pose 25 | quat = tf.transformations.quaternion_from_euler(args.roll, args.pitch, args.yaw) 26 | xyz = [args.x, args.y, args.z] 27 | 28 | initial_pose = PoseWithCovarianceStamped() 29 | initial_pose.pose.pose = Pose(Point(*xyz), Quaternion(*quat)) 30 | initial_pose.header.stamp = rospy.Time().now() 31 | initial_pose.header.frame_id = 'map' 32 | rospy.sleep(1) 33 | rospy.loginfo('Initial Pose: {} {} {} {} {} {}'.format( 34 | args.x, args.y, args.z, args.yaw, args.pitch, args.roll, )) 35 | pub_pose.publish(initial_pose) 36 | -------------------------------------------------------------------------------- /scripts/transform_fusion.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # coding=utf8 3 | from __future__ import print_function, division, absolute_import 4 | 5 | import copy 6 | import thread 7 | import time 8 | 9 | import numpy as np 10 | import rospy 11 | import tf 12 | import tf.transformations 13 | from geometry_msgs.msg import Pose, Point, Quaternion 14 | from nav_msgs.msg import Odometry 15 | 16 | cur_odom_to_baselink = None 17 | cur_map_to_odom = None 18 | 19 | 20 | def pose_to_mat(pose_msg): 21 | return np.matmul( 22 | tf.listener.xyz_to_mat44(pose_msg.pose.pose.position), 23 | tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation), 24 | ) 25 | 26 | 27 | def transform_fusion(): 28 | global cur_odom_to_baselink, cur_map_to_odom 29 | 30 | br = tf.TransformBroadcaster() 31 | while True: 32 | time.sleep(1 / FREQ_PUB_LOCALIZATION) 33 | 34 | # TODO 这里注意线程安全 35 | cur_odom = copy.copy(cur_odom_to_baselink) 36 | if cur_map_to_odom is not None: 37 | T_map_to_odom = pose_to_mat(cur_map_to_odom) 38 | else: 39 | T_map_to_odom = np.eye(4) 40 | 41 | br.sendTransform(tf.transformations.translation_from_matrix(T_map_to_odom), 42 | tf.transformations.quaternion_from_matrix(T_map_to_odom), 43 | rospy.Time.now(), 44 | 'camera_init', 'map') 45 | if cur_odom is not None: 46 | # 发布全局定位的odometry 47 | localization = Odometry() 48 | T_odom_to_base_link = pose_to_mat(cur_odom) 49 | # 这里T_map_to_odom短时间内变化缓慢 暂时不考虑与T_odom_to_base_link时间同步 50 | T_map_to_base_link = np.matmul(T_map_to_odom, T_odom_to_base_link) 51 | xyz = tf.transformations.translation_from_matrix(T_map_to_base_link) 52 | quat = tf.transformations.quaternion_from_matrix(T_map_to_base_link) 53 | localization.pose.pose = Pose(Point(*xyz), Quaternion(*quat)) 54 | localization.twist = cur_odom.twist 55 | 56 | localization.header.stamp = cur_odom.header.stamp 57 | localization.header.frame_id = 'map' 58 | localization.child_frame_id = 'body' 59 | # rospy.loginfo_throttle(1, '{}'.format(np.matmul(T_map_to_odom, T_odom_to_base_link))) 60 | pub_localization.publish(localization) 61 | 62 | 63 | def cb_save_cur_odom(odom_msg): 64 | global cur_odom_to_baselink 65 | cur_odom_to_baselink = odom_msg 66 | 67 | 68 | def cb_save_map_to_odom(odom_msg): 69 | global cur_map_to_odom 70 | cur_map_to_odom = odom_msg 71 | 72 | 73 | if __name__ == '__main__': 74 | # tf and localization publishing frequency (HZ) 75 | FREQ_PUB_LOCALIZATION = 50 76 | 77 | rospy.init_node('transform_fusion') 78 | rospy.loginfo('Transform Fusion Node Inited...') 79 | 80 | rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1) 81 | rospy.Subscriber('/map_to_odom', Odometry, cb_save_map_to_odom, queue_size=1) 82 | 83 | pub_localization = rospy.Publisher('/localization', Odometry, queue_size=1) 84 | 85 | # 发布定位消息 86 | thread.start_new_thread(transform_fusion, ()) 87 | 88 | rospy.spin() 89 | -------------------------------------------------------------------------------- /src/IMU_Processing.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include "use-ikfom.hpp" 26 | 27 | /// *************Preconfiguration 28 | 29 | #define MAX_INI_COUNT (20) 30 | 31 | const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; 32 | 33 | /// *************IMU Process and undistortion 34 | class ImuProcess 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | 39 | ImuProcess(); 40 | ~ImuProcess(); 41 | 42 | void Reset(); 43 | void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); 44 | void set_extrinsic(const V3D &transl, const M3D &rot); 45 | void set_extrinsic(const V3D &transl); 46 | void set_extrinsic(const MD(4,4) &T); 47 | void set_gyr_cov(const V3D &scaler); 48 | void set_acc_cov(const V3D &scaler); 49 | void set_gyr_bias_cov(const V3D &b_g); 50 | void set_acc_bias_cov(const V3D &b_a); 51 | Eigen::Matrix Q; 52 | void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); 53 | 54 | ofstream fout_imu; 55 | V3D cov_acc; 56 | V3D cov_gyr; 57 | V3D cov_acc_scale; 58 | V3D cov_gyr_scale; 59 | V3D cov_bias_gyr; 60 | V3D cov_bias_acc; 61 | double first_lidar_time; 62 | 63 | private: 64 | void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); 65 | void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); 66 | 67 | PointCloudXYZI::Ptr cur_pcl_un_; 68 | sensor_msgs::ImuConstPtr last_imu_; 69 | deque v_imu_; 70 | vector IMUpose; 71 | vector v_rot_pcl_; 72 | M3D Lidar_R_wrt_IMU; 73 | V3D Lidar_T_wrt_IMU; 74 | V3D mean_acc; 75 | V3D mean_gyr; 76 | V3D angvel_last; 77 | V3D acc_s_last; 78 | double start_timestamp_; 79 | double last_lidar_end_time_; 80 | int init_iter_num = 1; 81 | bool b_first_frame_ = true; 82 | bool imu_need_init_ = true; 83 | }; 84 | 85 | ImuProcess::ImuProcess() 86 | : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) 87 | { 88 | init_iter_num = 1; 89 | Q = process_noise_cov(); 90 | cov_acc = V3D(0.1, 0.1, 0.1); 91 | cov_gyr = V3D(0.1, 0.1, 0.1); 92 | cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); 93 | cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); 94 | mean_acc = V3D(0, 0, -1.0); 95 | mean_gyr = V3D(0, 0, 0); 96 | angvel_last = Zero3d; 97 | Lidar_T_wrt_IMU = Zero3d; 98 | Lidar_R_wrt_IMU = Eye3d; 99 | last_imu_.reset(new sensor_msgs::Imu()); 100 | } 101 | 102 | ImuProcess::~ImuProcess() {} 103 | 104 | void ImuProcess::Reset() 105 | { 106 | // ROS_WARN("Reset ImuProcess"); 107 | mean_acc = V3D(0, 0, -1.0); 108 | mean_gyr = V3D(0, 0, 0); 109 | angvel_last = Zero3d; 110 | imu_need_init_ = true; 111 | start_timestamp_ = -1; 112 | init_iter_num = 1; 113 | v_imu_.clear(); 114 | IMUpose.clear(); 115 | last_imu_.reset(new sensor_msgs::Imu()); 116 | cur_pcl_un_.reset(new PointCloudXYZI()); 117 | } 118 | 119 | void ImuProcess::set_extrinsic(const MD(4,4) &T) 120 | { 121 | Lidar_T_wrt_IMU = T.block<3,1>(0,3); 122 | Lidar_R_wrt_IMU = T.block<3,3>(0,0); 123 | } 124 | 125 | void ImuProcess::set_extrinsic(const V3D &transl) 126 | { 127 | Lidar_T_wrt_IMU = transl; 128 | Lidar_R_wrt_IMU.setIdentity(); 129 | } 130 | 131 | void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) 132 | { 133 | Lidar_T_wrt_IMU = transl; 134 | Lidar_R_wrt_IMU = rot; 135 | } 136 | 137 | void ImuProcess::set_gyr_cov(const V3D &scaler) 138 | { 139 | cov_gyr_scale = scaler; 140 | } 141 | 142 | void ImuProcess::set_acc_cov(const V3D &scaler) 143 | { 144 | cov_acc_scale = scaler; 145 | } 146 | 147 | void ImuProcess::set_gyr_bias_cov(const V3D &b_g) 148 | { 149 | cov_bias_gyr = b_g; 150 | } 151 | 152 | void ImuProcess::set_acc_bias_cov(const V3D &b_a) 153 | { 154 | cov_bias_acc = b_a; 155 | } 156 | 157 | void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N) 158 | { 159 | /** 1. initializing the gravity, gyro bias, acc and gyro covariance 160 | ** 2. normalize the acceleration measurenments to unit gravity **/ 161 | 162 | V3D cur_acc, cur_gyr; 163 | 164 | if (b_first_frame_) 165 | { 166 | Reset(); 167 | N = 1; 168 | b_first_frame_ = false; 169 | const auto &imu_acc = meas.imu.front()->linear_acceleration; 170 | const auto &gyr_acc = meas.imu.front()->angular_velocity; 171 | mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; 172 | mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 173 | first_lidar_time = meas.lidar_beg_time; 174 | } 175 | 176 | for (const auto &imu : meas.imu) 177 | { 178 | const auto &imu_acc = imu->linear_acceleration; 179 | const auto &gyr_acc = imu->angular_velocity; 180 | cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; 181 | cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 182 | 183 | mean_acc += (cur_acc - mean_acc) / N; 184 | mean_gyr += (cur_gyr - mean_gyr) / N; 185 | 186 | cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); 187 | cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); 188 | 189 | // cout<<"acc norm: "<::cov init_P = kf_state.get_P(); 203 | init_P.setIdentity(); 204 | init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; 205 | init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; 206 | init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; 207 | init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; 208 | init_P(21,21) = init_P(22,22) = 0.00001; 209 | kf_state.change_P(init_P); 210 | last_imu_ = meas.imu.back(); 211 | 212 | } 213 | 214 | void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) 215 | { 216 | /*** add the imu of the last frame-tail to the of current frame-head ***/ 217 | auto v_imu = meas.imu; 218 | v_imu.push_front(last_imu_); 219 | const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); 220 | const double &imu_end_time = v_imu.back()->header.stamp.toSec(); 221 | const double &pcl_beg_time = meas.lidar_beg_time; 222 | 223 | /*** sort point clouds by offset time ***/ 224 | pcl_out = *(meas.lidar); 225 | sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); 226 | const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); 227 | // cout<<"[ IMU Process ]: Process lidar from "<header.stamp.toSec() < last_lidar_end_time_) continue; 248 | 249 | angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 250 | 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 251 | 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); 252 | acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 253 | 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 254 | 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); 255 | 256 | // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; 257 | 258 | acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; 259 | 260 | if(head->header.stamp.toSec() < last_lidar_end_time_) 261 | { 262 | dt = tail->header.stamp.toSec() - last_lidar_end_time_; 263 | // dt = tail->header.stamp.toSec() - pcl_beg_time; 264 | } 265 | else 266 | { 267 | dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); 268 | } 269 | 270 | in.acc = acc_avr; 271 | in.gyro = angvel_avr; 272 | Q.block<3, 3>(0, 0).diagonal() = cov_gyr; 273 | Q.block<3, 3>(3, 3).diagonal() = cov_acc; 274 | Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; 275 | Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; 276 | kf_state.predict(dt, Q, in); 277 | 278 | /* save the poses at each IMU measurements */ 279 | imu_state = kf_state.get_x(); 280 | angvel_last = angvel_avr - imu_state.bg; 281 | acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); 282 | for(int i=0; i<3; i++) 283 | { 284 | acc_s_last[i] += imu_state.grav[i]; 285 | } 286 | double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; 287 | IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); 288 | } 289 | 290 | /*** calculated the pos and attitude prediction at the frame-end ***/ 291 | double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; 292 | dt = note * (pcl_end_time - imu_end_time); 293 | kf_state.predict(dt, Q, in); 294 | 295 | imu_state = kf_state.get_x(); 296 | last_imu_ = meas.imu.back(); 297 | last_lidar_end_time_ = pcl_end_time; 298 | 299 | /*** undistort each lidar point (backward propagation) ***/ 300 | auto it_pcl = pcl_out.points.end() - 1; 301 | for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) 302 | { 303 | auto head = it_kp - 1; 304 | auto tail = it_kp; 305 | R_imu<rot); 306 | // cout<<"head imu acc: "<vel); 308 | pos_imu<pos); 309 | acc_imu<acc); 310 | angvel_avr<gyr); 311 | 312 | for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) 313 | { 314 | dt = it_pcl->curvature / double(1000) - head->offset_time; 315 | 316 | /* Transform to the 'end' frame, using only the rotation 317 | * Note: Compensation direction is INVERSE of Frame's moving direction 318 | * So if we want to compensate a point at timestamp-i to the frame-e 319 | * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ 320 | M3D R_i(R_imu * Exp(angvel_avr, dt)); 321 | 322 | V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); 323 | V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); 324 | V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate! 325 | 326 | // save Undistorted points and their rotation 327 | it_pcl->x = P_compensate(0); 328 | it_pcl->y = P_compensate(1); 329 | it_pcl->z = P_compensate(2); 330 | 331 | if (it_pcl == pcl_out.points.begin()) break; 332 | } 333 | } 334 | } 335 | 336 | void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) 337 | { 338 | double t1,t2,t3; 339 | t1 = omp_get_wtime(); 340 | 341 | if(meas.imu.empty()) {return;}; 342 | ROS_ASSERT(meas.lidar != nullptr); 343 | 344 | if (imu_need_init_) 345 | { 346 | /// The very first lidar frame 347 | IMU_init(meas, kf_state, init_iter_num); 348 | 349 | imu_need_init_ = true; 350 | 351 | last_imu_ = meas.imu.back(); 352 | 353 | state_ikfom imu_state = kf_state.get_x(); 354 | if (init_iter_num > MAX_INI_COUNT) 355 | { 356 | cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); 357 | imu_need_init_ = false; 358 | 359 | cov_acc = cov_acc_scale; 360 | cov_gyr = cov_gyr_scale; 361 | ROS_INFO("IMU Initial Done"); 362 | // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ 363 | // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); 364 | fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); 365 | } 366 | 367 | return; 368 | } 369 | 370 | UndistortPcl(meas, kf_state, *cur_pcl_un_); 371 | 372 | t2 = omp_get_wtime(); 373 | t3 = omp_get_wtime(); 374 | 375 | // cout<<"[ IMU Process ]: Time: "< 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | #define IS_VALID(a) ((abs(a)>1e8) ? true : false) 9 | 10 | typedef pcl::PointXYZINormal PointType; 11 | typedef pcl::PointCloud PointCloudXYZI; 12 | 13 | enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3} 14 | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; 15 | enum Surround{Prev, Next}; 16 | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; 17 | 18 | struct orgtype 19 | { 20 | double range; 21 | double dista; 22 | double angle[2]; 23 | double intersect; 24 | E_jump edj[2]; 25 | Feature ftype; 26 | orgtype() 27 | { 28 | range = 0; 29 | edj[Prev] = Nr_nor; 30 | edj[Next] = Nr_nor; 31 | ftype = Nor; 32 | intersect = 2; 33 | } 34 | }; 35 | 36 | namespace velodyne_ros { 37 | struct EIGEN_ALIGN16 Point { 38 | PCL_ADD_POINT4D; 39 | float intensity; 40 | float time; 41 | uint16_t ring; 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | }; 44 | } // namespace velodyne_ros 45 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 46 | (float, x, x) 47 | (float, y, y) 48 | (float, z, z) 49 | (float, intensity, intensity) 50 | (float, time, time) 51 | (uint16_t, ring, ring) 52 | ) 53 | 54 | namespace ouster_ros { 55 | struct EIGEN_ALIGN16 Point { 56 | PCL_ADD_POINT4D; 57 | float intensity; 58 | uint32_t t; 59 | uint16_t reflectivity; 60 | uint8_t ring; 61 | uint16_t ambient; 62 | uint32_t range; 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | }; 65 | } // namespace ouster_ros 66 | 67 | // clang-format off 68 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 69 | (float, x, x) 70 | (float, y, y) 71 | (float, z, z) 72 | (float, intensity, intensity) 73 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 74 | (std::uint32_t, t, t) 75 | (std::uint16_t, reflectivity, reflectivity) 76 | (std::uint8_t, ring, ring) 77 | (std::uint16_t, ambient, ambient) 78 | (std::uint32_t, range, range) 79 | ) 80 | 81 | class Preprocess 82 | { 83 | public: 84 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 85 | 86 | Preprocess(); 87 | ~Preprocess(); 88 | 89 | void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 90 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 91 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 92 | 93 | // sensor_msgs::PointCloud2::ConstPtr pointcloud; 94 | PointCloudXYZI pl_full, pl_corn, pl_surf; 95 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar 96 | vector typess[128]; //maximum 128 line lidar 97 | int lidar_type, point_filter_num, N_SCANS;; 98 | double blind; 99 | double max_scan_range; 100 | bool feature_enabled, given_offset_time; 101 | ros::Publisher pub_full, pub_surf, pub_corn; 102 | 103 | 104 | private: 105 | void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); 106 | void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 107 | void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 108 | void give_feature(PointCloudXYZI &pl, vector &types); 109 | void pub_func(PointCloudXYZI &pl, const ros::Time &ct); 110 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); 111 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); 112 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); 113 | 114 | int group_size; 115 | double disA, disB, inf_bound; 116 | double limit_maxmid, limit_midmin, limit_maxmin; 117 | double p2l_ratio; 118 | double jump_up_limit, jump_down_limit; 119 | double cos160; 120 | double edgea, edgeb; 121 | double smallp_intersect, smallp_ratio; 122 | double vx, vy, vz; 123 | }; 124 | --------------------------------------------------------------------------------