├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── glog.cmake ├── config ├── avia.yaml ├── bg_avia.yaml ├── bg_velodyne.yaml ├── ncd.yaml ├── nclt.yaml └── ulhk.yaml ├── figures ├── ig_hku.png ├── ig_lio_cover.png ├── ig_ncd.png └── ig_nclt.png ├── ig_lio_early_access.pdf ├── include └── ig_lio │ ├── SymmetricEigenSolver.h │ ├── faster_voxel_grid.h │ ├── lio.h │ ├── logger.hpp │ ├── point_type.h │ ├── pointcloud_preprocess.h │ ├── timer.h │ ├── utilities.hpp │ └── voxel_map.h ├── launch ├── lio_avia.launch ├── lio_bg_avia.launch ├── lio_bg_velodyne.launch ├── lio_ncd.launch ├── lio_nclt.launch └── lio_ulhk.launch ├── log └── .gitignore ├── package.xml ├── result └── .gitignore ├── rviz └── lio_show.rviz ├── src ├── SymmetricEigenSolver.cpp ├── faster_voxel_grid.cpp ├── ig_lio_node.cpp ├── lio.cpp ├── pointcloud_preprocess.cpp ├── timer.cpp └── voxel_map.cpp └── thirdparty └── sophus ├── README.md └── sophus ├── average.hpp ├── common.hpp ├── example_ensure_handler.cpp ├── formatstring.hpp ├── geometry.hpp ├── interpolate.hpp ├── interpolate_details.hpp ├── num_diff.hpp ├── rotation_matrix.hpp ├── rxso2.hpp ├── rxso3.hpp ├── se2.hpp ├── se3.hpp ├── sim2.hpp ├── sim3.hpp ├── sim_details.hpp ├── so2.hpp ├── so3.hpp ├── test_macros.hpp ├── types.hpp └── velocities.hpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ig_lio) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | set(CMAKE_BUILD_TYPE Release) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS} -Wall") 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 9 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 10 | 11 | ## Find catkin macros and libraries 12 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 13 | ## is used, also find other catkin packages 14 | find_package(catkin REQUIRED COMPONENTS 15 | roscpp 16 | rospy 17 | std_msgs 18 | roslib 19 | pcl_ros 20 | livox_ros_driver 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | find_package(Eigen3 REQUIRED) 26 | find_package(PCL 1.8 REQUIRED) 27 | find_package(TBB REQUIRED) 28 | 29 | include(cmake/glog.cmake) 30 | 31 | # sophus 32 | include_directories(${PROJECT_SOURCE_DIR}/thirdparty/sophus) 33 | 34 | catkin_package( 35 | # INCLUDE_DIRS include 36 | # LIBRARIES gicp_lio 37 | # CATKIN_DEPENDS roscpp rospy std_msgs 38 | # DEPENDS system_lib 39 | ) 40 | 41 | include_directories( 42 | include 43 | ) 44 | 45 | include_directories(SYSTEM 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | add_executable(${PROJECT_NAME}_node 50 | src/ig_lio_node 51 | src/pointcloud_preprocess.cpp 52 | src/lio.cpp 53 | src/voxel_map.cpp 54 | src/SymmetricEigenSolver.cpp 55 | src/timer.cpp 56 | src/faster_voxel_grid.cpp 57 | ) 58 | 59 | target_link_libraries(${PROJECT_NAME}_node 60 | ${catkin_LIBRARIES} 61 | glog 62 | gflags 63 | TBB::tbb 64 | ) 65 | 66 | install(TARGETS ${PROJECT_NAME}_node 67 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 68 | ) 69 | 70 | install(DIRECTORY include/${PROJECT_NAME}/ 71 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 72 | FILES_MATCHING PATTERN "*.h" 73 | PATTERN ".svn" EXCLUDE 74 | ) 75 | 76 | install(DIRECTORY launch/ 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 78 | PATTERN ".svn" EXCLUDE) 79 | 80 | install(DIRECTORY config/ 81 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 82 | PATTERN ".svn" EXCLUDE) 83 | 84 | install(DIRECTORY rviz/ 85 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz 86 | PATTERN ".svn" EXCLUDE) 87 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # iG-LIO 2 | 3 | 4 | This work proposes an incremental Generalized Iterative Closest Point (GICP) based tightly-coupled LiDAR-inertial odometry (LIO), iG-LIO, which addresses the challenges of integrating GICP into real-time LIO. The main contributions are as follows. 5 | 6 | - The raw GICP constraints are tightly-coupled with IMU constraints in a Maximum A Posteriori (MAP) estimation. 7 | - A voxel-based surface covariance estimator (VSCE) is proposed to improve the efficiency and accuracy of the surface covariance estimation. Compared to the kd-tree based methods, VSCE reduces processing time in dense scans while maintaining the accuracy of iG-LIO in sparse and small FOV scans. 8 | - An incremental voxel map is designed to represent the probabilistic models of surrounding environments. Compared to non-incremental methods (e.g., DLIO), it successfully reduces the time cost required for the nearest neighbor search and map management. 9 | - Extensive datasets collected from different FOV LiDARs are adopted to evaluate the efficiency and accuracy of the proposed iG-LIO. Even though iG-LIO keeps identical parameters across all datasets, the results show that it is more efficient than Faster-LIO and achieves competitive performance compared to state-of-the-art LIO systems. 10 | 11 | The experiment video can be found on **[YouTube ](https://youtu.be/zMktZdj4AAk), [bilibili](https://www.bilibili.com/video/BV1dt4y1Z7z7/)**. 12 | 13 | The paper is available in [PDF](ig_lio_early_access.pdf). 14 | 15 | ![ig_lio_cover](figures/ig_lio_cover.png) 16 | 17 | ## 1. Build 18 | 19 | ### 1.1 Docker Container 20 | 21 | The docker-based standard development environment is available at https://github.com/zijiechenrobotics/ig_lio_workspace 22 | 23 | ### 1.2 Build from source 24 | 25 | #### 1.2.1 Prerequisites 26 | 27 | :one: **Ubuntu and ROS** 28 | 29 | Ubuntu >= 18.04. And Ubuntu 20.04 is recommended. 30 | 31 | :two: **GCC & G++ (only for Ubuntu 18.04)** 32 | 33 | gcc & g++ >= 9 34 | 35 | :three: **TBB (only for Ubuntu 18.04)** 36 | 37 | TBB >= 2020. Please follow https://github.com/oneapi-src/oneTBB 38 | 39 | :four: **livox_ros_driver** 40 | 41 | ```bash 42 | git clone https://github.com/Livox-SDK/Livox-SDK 43 | cd Livox-SDK 44 | mkdir build 45 | cd build 46 | cmake .. 47 | make -j 48 | sudo make install 49 | ``` 50 | 51 | :five: **glog** 52 | 53 | ```bash 54 | sudo apt-get install -y libgoogle-glog-dev 55 | ``` 56 | 57 | #### 1.2.2 Build 58 | 59 | ```bash 60 | cd 61 | mkdir src 62 | cd src 63 | git clone https://github.com/zijiechenrobotics/ig_lio_workspace.git 64 | git clone https://github.com/Livox-SDK/livox_ros_driver 65 | cd .. 66 | catkin_make 67 | ``` 68 | 69 | ## 2. Run 70 | ### 2.1 NCLT Dataset 71 | 72 | Download NCLT from http://robots.engin.umich.edu/nclt/ 73 | 74 | ```bash 75 | source devel/setup.bash 76 | roslaunch ig_lio lio_nclt.launch 77 | ``` 78 | 79 | ### 2.2 NCD Dataset 80 | 81 | Download NCD from https://ori-drs.github.io/newer-college-dataset/ 82 | 83 | ```bash 84 | source devel/setup.bash 85 | roslaunch ig_lio lio_ncd.launch 86 | ``` 87 | 88 | ### 2.3 ULHK Dataset 89 | 90 | Download ULHK from https://github.com/weisongwen/UrbanLoco 91 | 92 | ```bash 93 | source devel/setup.bash 94 | roslaunch ig_lio lio_ulhk.launch 95 | ``` 96 | 97 | ### 2.4 AVIA Dataset 98 | 99 | Download AVIA from https://drive.google.com/drive/folders/1CGYEJ9-wWjr8INyan6q1BZz_5VtGB-fP (fast-lio) and https://github.com/ziv-lin/r3live_dataset (r3live) 100 | 101 | ```bash 102 | source devel/setup.bash 103 | roslaunch ig_lio lio_avia.launch 104 | ``` 105 | 106 | The fast-lio datasets miss the gravitational constant in the accelerometer. Please edit the `avia.ymal` 107 | 108 | ```bash 109 | # for fast-lio 110 | enable_acc_correct: true 111 | 112 | # for r3live 113 | enable_acc_correct: false 114 | ``` 115 | 116 | ### 2.5 Botanic Garden Dataset 117 | 118 | Download Botanic Garden from https://github.com/robot-pesg/BotanicGarden 119 | 120 | ```bash 121 | source devel/setup.bash 122 | # for avia 123 | roslaunch ig_lio lio_bg_avia.launch 124 | # for velodyne 125 | roslaunch ig_lio lio_bg_velodyne.launch 126 | ``` 127 | 128 | ### 2.6 Run with your own dataset 129 | 130 | :one: **Edit `.yaml` files in `ig_lio/config`** 131 | 132 | - `lidar_topic`: LiDAR topic name. 133 | - `imu_topic`: IMU topic name. 134 | - `lidar_type`: The type of LiDAR you use. Only support for Velodyne, Ouster, and Livox. 135 | - `min_radius` & `max_radius`: A range filter to remove laser point from the robot itself. 136 | - `enable_ahrs_initalization`: Set true or false. If the IMU message has orientation channel, iG-LIO can be initialized via AHRS. 137 | - `enable_acc_correct`: Set true or false. If the accelerometer miss the gravitational constant, please set true (e.g., fast-lio2 datasets). 138 | - `gravity`: **Make sure the gravity is correct**. Some datasets are 9.81, some datasets are -9.81, and even zero (e.g., ULHK). A simple debugging method is to observe the glog message. The normal range of ba_norm is 0~0.5. 139 | - `t_imu_lidar` & `R_imu_lidar`: The extrinsic parameters from LiDAR frame to IMU frame (i.e. the IMU is the base frame). 140 | 141 | :two: **Launch iG-LIO** 142 | 143 | ```bash 144 | source devel/setup.bash 145 | roslaunch ig_lio .launch 146 | 147 | rosbay play 148 | ``` 149 | 150 | 151 | ## 3. Details about all sequences in the paper 152 | 153 | We use abbreviations for all sequences due to limited space. The full names of all sequences are presented below. 154 | 155 | | Abbreviation | Name | Distance(km) | Sensor Type | 156 | | :----------: | :--------------------------------------------: | :----------: | :--------------------------: | 157 | | nclt_1 | 2012-01-15 | 7.58 | Velodyne HDL-32E | 158 | | nclt_2 | 2012-04-29 | 3.17 | Velodyne HDL-32E | 159 | | nclt_3 | 2012-05-11 | 6.12 | Velodyne HDL-32E | 160 | | nclt_4 | 2012-06-15 | 4.09 | Velodyne HDL-32E | 161 | | nclt_5 | 2013-01-10 | 1.14 | Velodyne HDL-32E | 162 | | ncd_1 | 01_short_experiment | 1.61 | Ouster OS1-64 | 163 | | ncd_2 | 02_long_experiment | 3.06 | Ouster OS1-64 | 164 | | ncd_3 | 05_quad_with_dynamics | 0.48 | Ouster OS1-64 | 165 | | ncd_4 | 06_dynamic_spinning | 0.09 | Ouster OS1-64 | 166 | | ncd_5 | 07_parkland_mound | 0.70 | Ouster OS1-64 | 167 | | ulhk_1 | HK-Data20190117 | 0.60 | Velodyne HDL-32E | 168 | | ulhk_2 | HK-Data20190426-2 | 0.74 | Velodyne HDL-32E | 169 | | bg_1 | 1006-01 | 0.76 | Velodyne VLP-16 & Livox AVIA | 170 | | bg_2 | 1008-03 | 0.74 | Velodyne VLP-16 & Livox AVIA | 171 | | avia_1 | hku_main_buiding | 0.96 | Livox AVIA | 172 | | avia_2 | outdoor_Mainbuilding_100Hz_2020-12-24-16-46-29 | 0.14 | Livox AVIA | 173 | | avia_3 | outdoor_run_100Hz_2020-12-27-17-12-19 | 0.09 | Livox AVIA | 174 | 175 | ## 4. Mapping Results 176 | 177 | We aligned the mapping results of iG-LIO with Google Earth and found that iG-LIO retains global consistency maps. 178 | 179 | ### NCLT 2012-05-11 180 | 181 | ![ig_nclt](figures/ig_nclt.png) 182 | 183 | ### Newer College Dataset 02_long_experiment 184 | 185 | ![ig_ncd](figures/ig_ncd.png) 186 | 187 | ### hku_main_building 188 | 189 | ![ig_hku](figures/ig_hku.png) 190 | 191 | ## 5. Paper 192 | Thanks for citing iG-LIO (RA-L 2024) if you use any of this code. 193 | 194 | ``` 195 | # IEEE Robotics and Automation Letters ( Early Access ) 196 | @ARTICLE{10380742, 197 | author={Chen, Zijie and Xu, Yong and Yuan, Shenghai and Xie, Lihua}, 198 | journal={IEEE Robotics and Automation Letters}, 199 | title={iG-LIO: An Incremental GICP-based Tightly-coupled LiDAR-inertial Odometry}, 200 | year={2024}, 201 | volume={}, 202 | number={}, 203 | pages={1-8}, 204 | doi={10.1109/LRA.2024.3349915}} 205 | ``` 206 | 207 | ## 6. Acknowledgements 208 | 209 | Thanks for the below great open-source project for providing references to this work. 210 | 211 | 1. LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time) 212 | 2. [FAST-LIO](https://github.com/hku-mars/FAST_LIO) 213 | 3. [Faster-LIO](https://github.com/gaoxiang12/faster-lio) 214 | 4. [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) 215 | 5. [SLICT](https://github.com/brytsknguyen/slict) 216 | 217 | Thanks for the following public dataset. 218 | 219 | 1. [NCLT](http://robots.engin.umich.edu/nclt/) 220 | 2. [Newer College Dataset](https://ori-drs.github.io/newer-college-dataset/) 221 | 3. [Botanic Garden](https://github.com/robot-pesg/BotanicGarden) 222 | 4. [R3live](https://github.com/ziv-lin/r3live_dataset) 223 | 224 | ## 7. Known Issues 225 | 226 | **What can’t iG-LIO do?** 227 | 228 | - The extremely narrow environment (e.g., some sequence in the Hilti SLAM Challenge). 229 | - The scene is very open and devoid of geometric features. 230 | 231 | ## 8. Time Line 232 | 233 | | Time | Event | 234 | | ------------ | ------------------------------------------------------------ | 235 | | Aug 13, 2023 | **😀** Paper submitted to IEEE Robotics and Automation Letters (RA-L) | 236 | | Nov 5, 2023 | **😭** Revise and resubmit | 237 | | Dec 22, 2023 | **🥳** Paper accepted for publication in RA-L | 238 | | Current | **🎉** Source code released | 239 | -------------------------------------------------------------------------------- /cmake/glog.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | 3 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 4 | 5 | if(WIN32) 6 | find_path(GLOG_INCLUDE_DIR glog/logging.h 7 | PATHS ${GLOG_ROOT_DIR}/src/windows) 8 | else() 9 | find_path(GLOG_INCLUDE_DIR glog/logging.h 10 | PATHS ${GLOG_ROOT_DIR}) 11 | endif() 12 | 13 | if(MSVC) 14 | find_library(GLOG_LIBRARY_RELEASE libglog_static 15 | PATHS ${GLOG_ROOT_DIR} 16 | PATH_SUFFIXES Release) 17 | 18 | find_library(GLOG_LIBRARY_DEBUG libglog_static 19 | PATHS ${GLOG_ROOT_DIR} 20 | PATH_SUFFIXES Debug) 21 | 22 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 23 | else() 24 | find_library(GLOG_LIBRARY glog 25 | PATHS ${GLOG_ROOT_DIR} 26 | PATH_SUFFIXES lib lib64) 27 | endif() 28 | 29 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 30 | 31 | if(GLOG_FOUND) 32 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 33 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 34 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 35 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 36 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 37 | endif() 38 | 39 | include_directories(${GLOG_INCLUDE_DIRS}) 40 | list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES}) 41 | -------------------------------------------------------------------------------- /config/avia.yaml: -------------------------------------------------------------------------------- 1 | lidar_topic: /livox/lidar 2 | imu_topic: /livox/imu 3 | lidar_type: livox # livox velodyne ouster 4 | min_radius: 0.5 5 | max_radius: 150.0 6 | point_filter_num: 3 7 | time_scale: 0.001 # nclt: 0.001 other: 1000.0 8 | enable_ahrs_initalization: false 9 | enable_acc_correct: true 10 | 11 | scan_resolution: 0.5 12 | voxel_map_resolution: 0.5 13 | max_iterations: 10 14 | 15 | acc_cov: 0.1 16 | gyr_cov: 0.1 17 | ba_cov: 0.000001 18 | bg_cov: 0.000001 19 | 20 | gicp_constraints_gain: 100.0 21 | point2plane_constraints_gain: 1000.0 22 | enable_undistort: true 23 | enable_outlier_rejection: true 24 | 25 | gravity: 9.80665 26 | init_ori_cov: 0.0001 27 | inti_pos_cov: 0.0001 28 | init_vel_cov: 100.0 29 | init_ba_cov: 0.000001 30 | init_bg_cov: 0.000001 31 | 32 | # avia 33 | t_imu_lidar : [0.04165, 0.02326, -0.0284] 34 | R_imu_lidar : [1, 0, 0, 35 | 0, 1, 0, 36 | 0, 0, 1] -------------------------------------------------------------------------------- /config/bg_avia.yaml: -------------------------------------------------------------------------------- 1 | lidar_topic: /livox/lidar 2 | imu_topic: /livox/imu 3 | lidar_type: livox # livox velodyne ouster 4 | min_radius: 0.5 5 | max_radius: 150.0 6 | point_filter_num: 3 7 | time_scale: 0.001 # nclt: 0.001 other: 1000.0 8 | enable_ahrs_initalization: false 9 | enable_acc_correct: true 10 | 11 | scan_resolution: 0.5 12 | voxel_map_resolution: 0.5 13 | max_iterations: 10 14 | 15 | acc_cov: 0.1 16 | gyr_cov: 0.1 17 | ba_cov: 0.000001 18 | bg_cov: 0.000001 19 | 20 | gicp_constraints_gain: 100.0 21 | point2plane_constraints_gain: 1000.0 22 | enable_undistort: true 23 | enable_outlier_rejection: true 24 | 25 | gravity: 9.80665 26 | init_ori_cov: 0.0001 27 | inti_pos_cov: 0.0001 28 | init_vel_cov: 100.0 29 | init_ba_cov: 0.000001 30 | init_bg_cov: 0.000001 31 | 32 | # avia 33 | t_imu_lidar : [0.04165, 0.02326, -0.0284] 34 | R_imu_lidar : [1, 0, 0, 35 | 0, 1, 0, 36 | 0, 0, 1] -------------------------------------------------------------------------------- /config/bg_velodyne.yaml: -------------------------------------------------------------------------------- 1 | lidar_topic: /velodyne_points 2 | imu_topic: /imu/data 3 | lidar_type: velodyne # livox velodyne ouster 4 | min_radius: 1.0 5 | max_radius: 150.0 6 | point_filter_num: 6 7 | time_scale: 1000.0 # nclt: 0.001 other: 1000.0 8 | enable_ahrs_initalization: false 9 | enable_acc_correct: false 10 | 11 | scan_resolution: 0.5 12 | voxel_map_resolution: 0.5 13 | max_iterations: 10 14 | 15 | acc_cov: 0.01 16 | gyr_cov: 0.0001 17 | ba_cov: 0.000001 18 | bg_cov: 0.000001 19 | 20 | gicp_constraints_gain: 100.0 21 | point2plane_constraints_gain: 1000.0 22 | enable_undistort: true 23 | enable_outlier_rejection: true 24 | 25 | gravity: 9.80665 26 | init_ori_cov: 0.0001 27 | inti_pos_cov: 0.0001 28 | init_vel_cov: 100.0 29 | init_ba_cov: 0.0001 30 | init_bg_cov: 0.0001 31 | 32 | # botanic graden 33 | t_imu_lidar : [0.0584867781527745, 0.00840419966766332,0.168915521980526] 34 | R_imu_lidar: [0.999678872580465, 0.0252865664429322, 0.00150422292234868, 35 | -0.0252723438960774, 0.999649431893338, -0.0078025434141585, 36 | -0.00170103929405540, 0.00776298237926191, 0.99996789371916] -------------------------------------------------------------------------------- /config/ncd.yaml: -------------------------------------------------------------------------------- 1 | lidar_topic: /os1_cloud_node/points 2 | imu_topic: /os1_cloud_node/imu 3 | lidar_type: ouster # livox velodyne ouster 4 | min_radius: 0.5 5 | max_radius: 150.0 6 | point_filter_num: 2 7 | time_scale: 0.001 # nclt: 0.001 other: 1000.0 8 | enable_ahrs_initalization: false 9 | enable_acc_correct: false 10 | 11 | scan_resolution: 0.5 12 | voxel_map_resolution: 0.5 13 | max_iterations: 10 14 | 15 | acc_cov: 0.001249 16 | gyr_cov: 0.000208 17 | ba_cov: 0.000106 18 | bg_cov: 0.000004 19 | 20 | gicp_constraints_gain: 100.0 21 | point2plane_constraints_gain: 1000.0 22 | enable_undistort: true 23 | enable_outlier_rejection: true 24 | 25 | gravity: 9.80665 26 | init_ori_cov: 0.0001 27 | inti_pos_cov: 0.0001 28 | init_vel_cov: 100.0 29 | init_ba_cov: 0.000001 30 | init_bg_cov: 0.000001 31 | 32 | # newer college 33 | t_imu_lidar : [-0.006253, 0.011775, 0.03055] 34 | R_imu_lidar : [-1, 0, 0, 35 | 0, -1, 0, 36 | 0, 0, 1] -------------------------------------------------------------------------------- /config/nclt.yaml: -------------------------------------------------------------------------------- 1 | lidar_topic: /points_raw 2 | imu_topic: /imu_raw 3 | lidar_type: velodyne # livox velodyne ouster 4 | min_radius: 0.5 5 | max_radius: 150.0 6 | point_filter_num: 6 7 | time_scale: 0.001 # nclt: 0.001 other: 1000.0 8 | enable_ahrs_initalization: false 9 | enable_acc_correct: false 10 | 11 | scan_resolution: 0.5 12 | voxel_map_resolution: 0.5 13 | max_iterations: 10 14 | 15 | acc_cov: 0.001 16 | gyr_cov: 0.0001 17 | ba_cov: 0.000001 18 | bg_cov: 0.000001 19 | 20 | gicp_constraints_gain: 100.0 21 | point2plane_constraints_gain: 1000.0 22 | enable_undistort: true 23 | enable_outlier_rejection: true 24 | 25 | gravity: 9.80665 26 | init_ori_cov: 0.0001 27 | inti_pos_cov: 0.0001 28 | init_vel_cov: 100.0 29 | init_ba_cov: 0.0001 30 | init_bg_cov: 0.0001 31 | 32 | # nclt 33 | t_imu_lidar : [0, 0, 0.28] 34 | R_imu_lidar : [1, 0, 0, 35 | 0, 1, 0, 36 | 0, 0, 1] -------------------------------------------------------------------------------- /config/ulhk.yaml: -------------------------------------------------------------------------------- 1 | # /velodyne_points_0 for HK-Data20190117 2 | # /velodyne_points for HK-Data20190426-2 3 | lidar_topic: /velodyne_points 4 | imu_topic: /imu/data 5 | lidar_type: velodyne # livox velodyne ouster 6 | min_radius: 2.0 7 | max_radius: 150.0 8 | point_filter_num: 6 9 | time_scale: 1000.0 # nclt: 0.001 other: 1000.0 10 | enable_ahrs_initalization: false 11 | enable_acc_correct: false 12 | 13 | scan_resolution: 0.5 14 | voxel_map_resolution: 0.5 15 | max_iterations: 10 16 | 17 | acc_cov: 0.1 18 | gyr_cov: 0.1 19 | ba_cov: 0.000001 20 | bg_cov: 0.000001 21 | 22 | gicp_constraints_gain: 100.0 23 | point2plane_constraints_gain: 1000.0 24 | enable_undistort: true 25 | enable_outlier_rejection: true 26 | 27 | # 0.0 for HK-Data20190117 28 | # 9.80665 for HK-Data20190426-2 29 | gravity: 9.80665 30 | init_ori_cov: 0.0001 31 | inti_pos_cov: 0.0001 32 | init_vel_cov: 100.0 33 | init_ba_cov: 0.0001 34 | init_bg_cov: 0.0001 35 | 36 | # urbanloco 37 | t_imu_lidar : [0.0, 0.0, 0.28] 38 | R_imu_lidar: [0, 1, 0, 39 | -1, 0, 0, 40 | 0, 0, 1] -------------------------------------------------------------------------------- /figures/ig_hku.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zijiechenrobotics/ig_lio/5d9a2ccd4cab58238f53058e6ffd93dea9ce624a/figures/ig_hku.png -------------------------------------------------------------------------------- /figures/ig_lio_cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zijiechenrobotics/ig_lio/5d9a2ccd4cab58238f53058e6ffd93dea9ce624a/figures/ig_lio_cover.png -------------------------------------------------------------------------------- /figures/ig_ncd.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zijiechenrobotics/ig_lio/5d9a2ccd4cab58238f53058e6ffd93dea9ce624a/figures/ig_ncd.png -------------------------------------------------------------------------------- /figures/ig_nclt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zijiechenrobotics/ig_lio/5d9a2ccd4cab58238f53058e6ffd93dea9ce624a/figures/ig_nclt.png -------------------------------------------------------------------------------- /ig_lio_early_access.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zijiechenrobotics/ig_lio/5d9a2ccd4cab58238f53058e6ffd93dea9ce624a/ig_lio_early_access.pdf -------------------------------------------------------------------------------- /include/ig_lio/SymmetricEigenSolver.h: -------------------------------------------------------------------------------- 1 | #ifndef SYMMETRIC_EIGEN_SOLVER_H_ 2 | #define SYMMETRIC_EIGEN_SOLVER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class SymmetricEigensolver3x3 { 10 | public: 11 | SymmetricEigensolver3x3(); 12 | 13 | SymmetricEigensolver3x3(const Eigen::Matrix3d input_matrix); 14 | 15 | void compute(); 16 | 17 | Eigen::Vector3d eigenvalues(); 18 | 19 | Eigen::Matrix3d eigenvectors(); 20 | 21 | private: 22 | void computeEigenvector0(double a00, 23 | double a01, 24 | double a02, 25 | double a11, 26 | double a12, 27 | double a22, 28 | int i0); 29 | 30 | void computeEigenvector1(double a00, 31 | double a01, 32 | double a02, 33 | double a11, 34 | double a12, 35 | double a22, 36 | int i0, 37 | int i1); 38 | 39 | void computeEigenvector2(int i0, int i1, int i2); 40 | 41 | void computeOrthogonalComplement(Eigen::Vector3d& w, 42 | Eigen::Vector3d& u, 43 | Eigen::Vector3d& v); 44 | 45 | Eigen::Vector3d cross(Eigen::Vector3d u, Eigen::Vector3d v); 46 | 47 | Eigen::Matrix3d input_; 48 | Eigen::Matrix3d evecs_; 49 | Eigen::Vector3d evals_; 50 | }; 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /include/ig_lio/faster_voxel_grid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel-based surface covariance estimator 3 | * @Autor: Zijie Chen 4 | * @Date: 2023-12-26 20:05:38 5 | */ 6 | 7 | #ifndef FASTER_VOXEL_GRID_H_ 8 | #define FASTER_VOXEL_GRID_H_ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "point_type.h" 17 | 18 | class FasterVoxelGrid { 19 | public: 20 | FasterVoxelGrid(double resolution) 21 | : resolution_(resolution) 22 | , inv_resolution_(1.0 / resolution) { 23 | voxel_map_ptr_ = std::make_shared(); 24 | voxel_array_ptr_ = std::make_shared(); 25 | 26 | search_range_.clear(); 27 | for (int x_gain = -1; x_gain <= 1; ++x_gain) { 28 | for (int y_gain = -1; y_gain <= 1; ++y_gain) { 29 | for (int z_gain = -1; z_gain <= 1; ++z_gain) { 30 | search_range_.emplace_back(Eigen::Vector3d(x_gain * resolution_, 31 | y_gain * resolution_, 32 | z_gain * resolution_)); 33 | } 34 | } 35 | } 36 | }; 37 | 38 | void Filter(const CloudPtr& input_cloud_ptr, 39 | CloudPtr& cloud_DS_ptr, 40 | CloudCovPtr& cloud_cov_ptr); 41 | 42 | size_t ComputeHashIndex(const Eigen::Vector3d& point); 43 | 44 | double resolution_{1.0}; 45 | double inv_resolution_{1.0}; 46 | 47 | struct Voxel { 48 | Voxel(){}; 49 | 50 | Eigen::Vector3d centorid_ = Eigen::Vector3d::Zero(); 51 | size_t N_{0}; 52 | }; 53 | 54 | using MyHashMap = tbb::concurrent_hash_map>; 55 | using MyVector = tbb::concurrent_vector>; 56 | using MyAccessor = MyHashMap::accessor; 57 | 58 | std::shared_ptr voxel_map_ptr_; 59 | std::shared_ptr voxel_array_ptr_; 60 | 61 | std::vector search_range_; 62 | 63 | double ava_precent_{0.0}; 64 | size_t frame_count_{0}; 65 | 66 | size_t HASH_P_{116101}; 67 | size_t MAX_N_{10000000000}; 68 | size_t min_points_per_grid_{6}; 69 | }; 70 | 71 | #endif -------------------------------------------------------------------------------- /include/ig_lio/lio.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO 3 | * @Autor: Zijie Chen 4 | * @Date: 2023-12-28 11:09:59 5 | */ 6 | 7 | #ifndef LIO_H_ 8 | #define LIO_H_ 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "ig_lio/faster_voxel_grid.h" 28 | #include "ig_lio/point_type.h" 29 | #include "ig_lio/utilities.hpp" 30 | #include "ig_lio/voxel_map.h" 31 | 32 | enum MeasurementType { LIDAR, GNSS }; 33 | enum GNSSStatus { RTK_FIXED, RTK_FLOAT, NONE }; 34 | 35 | struct SensorMeasurement { 36 | MeasurementType measurement_type_; 37 | // ros time 38 | double bag_time_{0.0}; 39 | // The time of the first laser point in the scan 40 | double lidar_start_time_{0.0}; 41 | // The time of the last laser point in the scan 42 | double lidar_end_time_{0.0}; 43 | CloudPtr cloud_ptr_{}; 44 | std::deque imu_buff_; 45 | 46 | Eigen::Matrix4d gnss_pose_ = Eigen::Matrix4d::Identity(); 47 | GNSSStatus gnss_status_ = GNSSStatus::NONE; 48 | bool has_gnss_ori_{false}; 49 | }; 50 | 51 | class LIO { 52 | public: 53 | struct Config { 54 | Config(){}; 55 | 56 | double init_ori_cov{1.0}; 57 | double init_pos_cov{1.0}; 58 | double init_vel_cov{1.0}; 59 | double init_ba_cov{1.0}; 60 | double init_bg_cov{1.0}; 61 | double acc_cov{1.0}; 62 | double gyr_cov{1.0}; 63 | double ba_cov{1.0}; 64 | double bg_cov{1.0}; 65 | 66 | double gravity{9.81}; 67 | Eigen::Matrix4d T_imu_lidar = Eigen::Matrix4d::Identity(); 68 | 69 | size_t max_iterations{30}; 70 | double gicp_constraint_gain{100.0}; 71 | double point2plane_constraint_gain{1000.0}; 72 | 73 | bool enable_undistort{true}; 74 | bool enable_outlier_rejection{true}; 75 | 76 | double current_scan_resolution{0.5}; 77 | double voxel_map_resolution{0.5}; 78 | 79 | double max_radius{150.0}; 80 | double min_radius{0.5}; 81 | }; 82 | 83 | LIO(Config config = Config()) 84 | : config_(config) 85 | , cloud_cov_ptr_(new CloudCovType()) 86 | , cloud_DS_ptr_(new CloudType()) { 87 | fast_voxel_grid_ptr_ = 88 | std::make_shared(config_.current_scan_resolution); 89 | 90 | VoxelMap::Config voxel_map_config; 91 | voxel_map_config.resolution = config_.voxel_map_resolution; 92 | voxel_map_config.search_method = "NEARBY_7"; // nearby_7 is more stable 93 | voxel_map_ptr_ = std::make_shared(voxel_map_config); 94 | 95 | correspondences_array_.reserve(10000); 96 | g_ = Eigen::Vector3d(0.0, 0.0, config_.gravity); 97 | } 98 | 99 | bool MeasurementUpdate(SensorMeasurement& sensor_measurement); 100 | 101 | bool Predict(const double time, 102 | const Eigen::Vector3d& acc_1, 103 | const Eigen::Vector3d& gyr_1); 104 | 105 | bool StaticInitialization(SensorMeasurement& sensor_measurement); 106 | 107 | bool AHRSInitialization(SensorMeasurement& sensor_measurement); 108 | 109 | bool InRadius(const PointType& p) { 110 | double radius = p.x * p.x + p.y * p.y + p.z * p.z; 111 | return (radius < (config_.max_radius * config_.max_radius) && 112 | radius > (config_.min_radius * config_.min_radius)); 113 | } 114 | 115 | bool IsInit() { return lio_init_; } 116 | 117 | Eigen::Matrix4d GetCurrentPose() { return curr_state_.pose; } 118 | 119 | Eigen::Vector3d GetCurrentVel() { return curr_state_.vel; } 120 | 121 | Eigen::Vector3d GetCurrentBa() { return curr_state_.ba; } 122 | 123 | Eigen::Vector3d GetCurrentBg() { return curr_state_.bg; } 124 | 125 | size_t GetFinalIterations() { return iter_num_; } 126 | 127 | private: 128 | static constexpr int IndexErrorOri{0}; 129 | static constexpr int IndexErrorPos{3}; 130 | static constexpr int IndexErrorVel{6}; 131 | static constexpr int IndexErrorBiasAcc{9}; 132 | static constexpr int IndexErrorBiasGyr{12}; 133 | 134 | static constexpr int IndexNoiseAccLast{0}; 135 | static constexpr int IndexNoiseGyrLast{3}; 136 | static constexpr int IndexNoiseAccCurr{6}; 137 | static constexpr int IndexNoiseGyrCurr{9}; 138 | static constexpr int IndexNoiseBiasAcc{12}; 139 | static constexpr int IndexNoiseBiasGyr{15}; 140 | 141 | struct PoseHistory { 142 | double time_ = 0.0; 143 | Eigen::Matrix4d T_ = Eigen::Matrix4d::Identity(); 144 | Eigen::Vector3d vel_ = Eigen::Vector3d::Zero(); 145 | Eigen::Vector3d un_acc_ = Eigen::Vector3d::Zero(); 146 | Eigen::Vector3d un_gyr_ = Eigen::Vector3d::Zero(); 147 | }; 148 | std::deque pose_history_; // for pointcloud 149 | 150 | struct Correspondence { 151 | public: 152 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 153 | Eigen::Vector3d mean_A = Eigen::Vector3d::Zero(); 154 | Eigen::Vector3d mean_B = Eigen::Vector3d::Zero(); 155 | Eigen::Matrix3d mahalanobis = Eigen::Matrix3d::Zero(); 156 | Eigen::Vector4d plane_coeff = Eigen::Vector4d::Zero(); 157 | }; 158 | std::vector> correspondences_buff_; 159 | tbb::concurrent_vector> 160 | correspondences_array_; 161 | 162 | struct State { 163 | public: 164 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 165 | Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); 166 | Eigen::Vector3d vel = Eigen::Vector3d::Zero(); 167 | Eigen::Vector3d ba = Eigen::Vector3d::Zero(); 168 | Eigen::Vector3d bg = Eigen::Vector3d::Zero(); 169 | }; 170 | 171 | bool NominalStateUpdate(const double dt, 172 | const Eigen::Vector3d& acc_0, 173 | const Eigen::Vector3d& acc_1, 174 | const Eigen::Vector3d& gyr_0, 175 | const Eigen::Vector3d& gyr_1, 176 | const Eigen::Matrix4d& T_prev, 177 | const Eigen::Vector3d& vel_prev, 178 | Eigen::Matrix4d& T_curr, 179 | Eigen::Vector3d& vel_curr, 180 | Eigen::Vector3d& un_acc, 181 | Eigen::Vector3d& un_gyr); 182 | 183 | bool ErrorStateUpdate(const double dt, 184 | const Eigen::Vector3d& acc_0, 185 | const Eigen::Vector3d& acc_1, 186 | const Eigen::Vector3d& gyr_0, 187 | const Eigen::Vector3d& gyr_1); 188 | 189 | bool UndistortPointCloud(const double bag_time, 190 | const double lidar_end_time, 191 | CloudPtr& cloud_ptr); 192 | 193 | bool StepOptimize(const SensorMeasurement& sensor_measurement, 194 | Eigen::Matrix& delta_x); 195 | 196 | double ConstructGICPConstraints(Eigen::Matrix& H, 197 | Eigen::Matrix& b); 198 | 199 | double ConstructPoint2PlaneConstraints(Eigen::Matrix& H, 200 | Eigen::Matrix& b); 201 | 202 | double ConstructImuPriorConstraints(Eigen::Matrix& H, 203 | Eigen::Matrix& b); 204 | 205 | double ComputeError(const SensorMeasurement& sensor_measurement, 206 | const State& state); 207 | 208 | bool IsConverged(const Eigen::Matrix& delta_x); 209 | 210 | bool ComputeFinalCovariance(const Eigen::Matrix& delta_x); 211 | 212 | bool CorrectState(const State& state, 213 | const Eigen::Matrix& delta_x, 214 | State& corrected_state); 215 | 216 | bool GNStep(const SensorMeasurement& sensor_measurement, 217 | Eigen::Matrix& H, 218 | Eigen::Matrix& b, 219 | const double y0, 220 | Eigen::Matrix& delta_x); 221 | 222 | Config config_; 223 | 224 | double lio_time_{0.0}; 225 | bool lio_init_{false}; 226 | 227 | Eigen::Vector3d g_ = Eigen::Vector3d(0.0, 0.0, 9.81); 228 | 229 | State curr_state_; 230 | State prev_state_; 231 | 232 | Eigen::Vector3d acc_0_ = Eigen::Vector3d::Zero(); 233 | Eigen::Vector3d gyr_0_ = Eigen::Vector3d::Zero(); 234 | 235 | Eigen::Matrix P_ = Eigen::Matrix::Zero(); 236 | Eigen::Matrix F_ = Eigen::Matrix::Zero(); 237 | Eigen::Matrix B_ = Eigen::Matrix::Zero(); 238 | Eigen::Matrix Q_ = Eigen::Matrix::Zero(); 239 | 240 | double transformation_epsilon_{0.001}; 241 | bool need_converge_{false}; 242 | size_t iter_num_{0}; 243 | Eigen::Matrix final_hessian_ = 244 | Eigen::Matrix::Zero(); 245 | 246 | size_t effect_feat_num_; 247 | CloudCovPtr cloud_cov_ptr_{}; 248 | CloudPtr cloud_DS_ptr_{}; 249 | std::shared_ptr voxel_map_ptr_{}; 250 | std::shared_ptr fast_voxel_grid_ptr_{}; 251 | size_t lidar_frame_count_{0}; 252 | size_t keyframe_count_{0}; 253 | 254 | // for initialization 255 | Eigen::Vector3d mean_acc_ = Eigen::Vector3d(0, 0, -1.0); 256 | Eigen::Vector3d mean_gyr_ = Eigen::Vector3d(0, 0, 0); 257 | // 258 | std::vector> imu_init_buff_; 259 | bool first_imu_frame_{true}; 260 | size_t imu_init_count_{0}; 261 | size_t max_init_count_{20}; 262 | 263 | Eigen::Matrix4d last_keyframe_pose_ = Eigen::Matrix4d::Identity(); 264 | 265 | size_t effect_const_num_{0}; 266 | double ava_effect_feat_num_{0.0}; 267 | }; 268 | 269 | #endif -------------------------------------------------------------------------------- /include/ig_lio/logger.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: glog module 3 | * @Autor: Zijie Chen 4 | * @Date: 2022-10-08 19:32:27 5 | */ 6 | 7 | #pragma once 8 | 9 | // #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | class Logger { 20 | public: 21 | Logger(int argc, char** argv); 22 | 23 | Logger(int argc, char** argv, std::string current_path); 24 | 25 | ~Logger(); 26 | }; 27 | 28 | // for common 29 | Logger::Logger(int argc, char** argv) { 30 | // config 31 | google::ParseCommandLineFlags(&argc, &argv, false); 32 | google::InitGoogleLogging(argv[0]); 33 | 34 | FLAGS_colorlogtostderr = true; 35 | FLAGS_logbufsecs = 0; 36 | FLAGS_max_log_size = 100; 37 | FLAGS_stop_logging_if_full_disk = true; 38 | FLAGS_alsologtostderr = true; 39 | FLAGS_log_prefix = true; 40 | 41 | auto current_path = boost::filesystem::current_path(); 42 | auto log_path = current_path / "log"; 43 | if (!boost::filesystem::exists(log_path)) { 44 | boost::filesystem::create_directories(log_path); 45 | } 46 | 47 | std::cout << "log path: " << log_path << std::endl; 48 | FLAGS_log_dir = log_path.string(); 49 | } 50 | 51 | // for ROS 52 | Logger::Logger(int argc, char** argv, std::string current_path) { 53 | // config 54 | google::ParseCommandLineFlags(&argc, &argv, false); 55 | google::InitGoogleLogging(argv[0]); 56 | 57 | FLAGS_colorlogtostderr = true; 58 | FLAGS_logbufsecs = 0; 59 | FLAGS_max_log_size = 100; 60 | FLAGS_stop_logging_if_full_disk = true; 61 | FLAGS_alsologtostderr = true; 62 | FLAGS_log_prefix = true; 63 | 64 | auto log_path = boost::filesystem::path(current_path) / "log"; 65 | if (!boost::filesystem::exists(log_path)) { 66 | boost::filesystem::create_directories(log_path); 67 | } 68 | 69 | std::cout << "log path: " << log_path << std::endl; 70 | FLAGS_log_dir = log_path.string(); 71 | } 72 | 73 | Logger::~Logger() { 74 | google::ShutdownGoogleLogging(); 75 | std::cout << "Logger Is Finshed!" << std::endl; 76 | } 77 | -------------------------------------------------------------------------------- /include/ig_lio/point_type.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: define point type 3 | * @Autor: Zijie Chen 4 | * @Date: 2023-12-25 22:28:00 5 | */ 6 | 7 | #ifndef POINT_TYPE_H_ 8 | #define POINT_TYPE_H_ 9 | 10 | #include 11 | #include 12 | 13 | using PointType = pcl::PointXYZINormal; 14 | using CloudType = pcl::PointCloud; 15 | using CloudPtr = CloudType::Ptr; 16 | 17 | struct PointWithCovariance { 18 | PCL_ADD_POINT4D 19 | int idx; 20 | float cov[6]; 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 22 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 23 | 24 | POINT_CLOUD_REGISTER_POINT_STRUCT( 25 | PointWithCovariance, 26 | (float, x, x)(float, y, y)(float, z, z)(int, idx, idx)(float[6], cov, cov)) 27 | 28 | using PointCovType = PointWithCovariance; 29 | using CloudCovType = pcl::PointCloud; 30 | using CloudCovPtr = CloudCovType::Ptr; 31 | 32 | #endif -------------------------------------------------------------------------------- /include/ig_lio/pointcloud_preprocess.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: preprocess point clouds 3 | * @Autor: Zijie Chen 4 | * @Date: 2023-12-28 09:45:32 5 | */ 6 | 7 | #ifndef POINTCLOUD_PREPROCESS_H_ 8 | #define POINTCLOUD_PREPROCESS_H_ 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "point_type.h" 20 | 21 | enum class LidarType { LIVOX, VELODYNE, OUSTER }; 22 | 23 | // for Velodyne LiDAR 24 | struct VelodynePointXYZIRT { 25 | PCL_ADD_POINT4D; 26 | PCL_ADD_INTENSITY; 27 | uint16_t ring; 28 | float time; 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | } EIGEN_ALIGN16; 31 | POINT_CLOUD_REGISTER_POINT_STRUCT( 32 | VelodynePointXYZIRT, 33 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( 34 | uint16_t, ring, ring)(float, time, time)) 35 | 36 | // for Ouster LiDAR 37 | struct OusterPointXYZIRT { 38 | PCL_ADD_POINT4D; 39 | float intensity; 40 | uint32_t t; 41 | uint16_t reflectivity; 42 | uint8_t ring; 43 | uint16_t noise; 44 | uint32_t range; 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | } EIGEN_ALIGN16; 47 | POINT_CLOUD_REGISTER_POINT_STRUCT( 48 | OusterPointXYZIRT, 49 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( 50 | uint32_t, t, t)(uint16_t, reflectivity, reflectivity)( 51 | uint8_t, ring, ring)(uint16_t, noise, noise)(uint32_t, range, range)) 52 | // struct OusterPointXYZIRT { 53 | // PCL_ADD_POINT4D; 54 | // float intensity; 55 | // double timestamp; 56 | // uint16_t reflectivity; 57 | // uint8_t ring; 58 | // uint16_t noise; 59 | // uint32_t range; 60 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | // } EIGEN_ALIGN16; 62 | // POINT_CLOUD_REGISTER_POINT_STRUCT( 63 | // OusterPointXYZIRT, 64 | // (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( 65 | // double, timestamp, timestamp)(uint16_t, reflectivity, reflectivity)( 66 | // uint8_t, ring, ring)(uint16_t, noise, noise)(uint32_t, range, range)) 67 | 68 | class PointCloudPreprocess { 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 71 | struct Config { 72 | Config(){}; 73 | 74 | int point_filter_num{4}; 75 | LidarType lidar_type = LidarType::VELODYNE; 76 | // only use for velodyne 77 | double time_scale{1000.0}; 78 | }; 79 | 80 | PointCloudPreprocess() = delete; 81 | 82 | PointCloudPreprocess(Config config = Config()) 83 | : config_(config) {} 84 | 85 | ~PointCloudPreprocess() = default; 86 | 87 | void Process(const livox_ros_driver::CustomMsg::ConstPtr& msg, 88 | pcl::PointCloud::Ptr& cloud_out, 89 | const double last_start_time = 0.0); 90 | 91 | void Process(const sensor_msgs::PointCloud2::ConstPtr& msg, 92 | pcl::PointCloud::Ptr& cloud_out); 93 | 94 | private: 95 | template 96 | bool HasInf(const T& p); 97 | 98 | template 99 | bool HasNan(const T& p); 100 | 101 | template 102 | bool IsNear(const T& p1, const T& p2); 103 | 104 | void ProcessVelodyne(const sensor_msgs::PointCloud2::ConstPtr& msg, 105 | pcl::PointCloud::Ptr& cloud_out); 106 | 107 | void ProcessOuster(const sensor_msgs::PointCloud2::ConstPtr& msg, 108 | pcl::PointCloud::Ptr& cloud_out); 109 | 110 | int num_scans_ = 128; 111 | bool has_time_ = false; 112 | 113 | Config config_; 114 | 115 | pcl::PointCloud cloud_sort_; 116 | }; 117 | 118 | #endif -------------------------------------------------------------------------------- /include/ig_lio/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef TIMER_H_ 2 | #define TIMER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class Timer { 12 | public: 13 | struct TimerRecord { 14 | TimerRecord() = default; 15 | TimerRecord(const std::string& name, double time_usage) { 16 | func_name_ = name; 17 | time_usage_in_ms_.emplace_back(time_usage); 18 | } 19 | std::string func_name_; 20 | std::vector time_usage_in_ms_; 21 | }; 22 | 23 | template 24 | void Evaluate(F&& func, const std::string& func_name) { 25 | auto t1 = std::chrono::high_resolution_clock::now(); 26 | std::forward(func)(); 27 | auto t2 = std::chrono::high_resolution_clock::now(); 28 | auto time_used = 29 | std::chrono::duration_cast>(t2 - t1) 30 | .count() * 31 | 1000; 32 | if (records_.find(func_name) != records_.end()) { 33 | records_[func_name].time_usage_in_ms_.emplace_back(time_used); 34 | } else { 35 | records_.insert({func_name, TimerRecord(func_name, time_used)}); 36 | } 37 | } 38 | 39 | void PrintAll(); 40 | 41 | /// clean the records 42 | void Clear() { records_.clear(); } 43 | 44 | private: 45 | std::map records_; 46 | }; 47 | 48 | #endif -------------------------------------------------------------------------------- /include/ig_lio/utilities.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: some tools 3 | * @Autor: Zijie Chen 4 | * @Date: 2023-12-27 23:43:58 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include 12 | 13 | template 14 | void ComputeMeanAndCovDiag(const C& data, 15 | D& mean, 16 | D& cov_diag, 17 | Getter&& getter) { 18 | size_t len = data.size(); 19 | assert(len > 1); 20 | // clang-format off 21 | mean = std::accumulate(data.begin(), data.end(), D::Zero().eval(), 22 | [&getter](const D& sum, const auto& data) -> D { return sum + getter(data); }) / len; 23 | cov_diag = std::accumulate(data.begin(), data.end(), D::Zero().eval(), 24 | [&mean, &getter](const D& sum, const auto& data) -> D { 25 | return sum + (getter(data) - mean).cwiseAbs2().eval(); 26 | }) / (len - 1); 27 | // clang-format on 28 | } 29 | 30 | inline bool EstimatePlane(Eigen::Vector4d& pca_result, 31 | const std::vector& point, 32 | const double threshold = 0.1) { 33 | if (point.size() < 3) { 34 | return false; 35 | } 36 | 37 | Eigen::MatrixXd A(point.size(), 3); 38 | Eigen::VectorXd b(point.size(), 1); 39 | 40 | A.setZero(); 41 | b.setOnes(); 42 | b *= -1.0; 43 | 44 | for (size_t j = 0; j < point.size(); j++) { 45 | A(j, 0) = point[j].x(); 46 | A(j, 1) = point[j].y(); 47 | A(j, 2) = point[j].z(); 48 | } 49 | 50 | Eigen::Vector3d normvec = A.colPivHouseholderQr().solve(b); 51 | 52 | double n_norm = normvec.norm(); 53 | pca_result(0) = normvec(0) / n_norm; 54 | pca_result(1) = normvec(1) / n_norm; 55 | pca_result(2) = normvec(2) / n_norm; 56 | pca_result(3) = 1.0 / n_norm; 57 | 58 | for (const auto& p : point) { 59 | Eigen::Vector4d temp(p.x(), p.y(), p.z(), 1.0); 60 | if (fabs(pca_result.dot(temp)) > threshold) { 61 | return false; 62 | } 63 | } 64 | return true; 65 | } 66 | 67 | inline void CauchyLossFunction(const double e, 68 | const double delta, 69 | Eigen::Vector3d& rho) { 70 | double dsqr = delta * delta; 71 | if (e <= dsqr) { // inlier 72 | rho[0] = e; 73 | rho[1] = 1.; 74 | rho[2] = 0.; 75 | } else { // outlier 76 | double sqrte = sqrt(e); // absolut value of the error 77 | // rho(e) = 2 * delta * e^(1/2) - delta^2 78 | rho[0] = 2 * sqrte * delta - dsqr; 79 | // rho'(e) = delta / sqrt(e) 80 | rho[1] = delta / sqrte; 81 | // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e 82 | rho[2] = -0.5 * rho[1] / e; 83 | } 84 | } -------------------------------------------------------------------------------- /include/ig_lio/voxel_map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: incremental voxel map 3 | * @Autor: Zijie Chen 4 | * @Date: 2023-12-27 23:44:38 5 | */ 6 | 7 | #ifndef VOXEL_MAP_H_ 8 | #define VOXEL_MAP_H_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include "ig_lio/point_type.h" 26 | 27 | struct Grid { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | Grid(size_t grid_max_points) { points_array_.reserve(2 * grid_max_points); } 31 | 32 | size_t hash_idx{0}; 33 | Eigen::Vector3d centroid_ = Eigen::Vector3d::Zero(); 34 | Eigen::Matrix3d cov_ = Eigen::Matrix3d::Zero(); 35 | Eigen::Matrix3d inv_cov_ = Eigen::Matrix3d::Zero(); 36 | Eigen::Matrix3d cov_sum_ = Eigen::Matrix3d::Zero(); 37 | Eigen::Vector3d points_sum_ = Eigen::Vector3d::Zero(); 38 | size_t points_num_{0}; 39 | bool is_valid_{false}; 40 | std::vector points_array_; 41 | }; 42 | 43 | struct point_hash_idx { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | Eigen::Vector3d point_; 47 | size_t hash_idx_; // 48 | 49 | point_hash_idx() = default; 50 | point_hash_idx(Eigen::Vector3d point, size_t hash_idx) 51 | : point_(point) 52 | , hash_idx_(hash_idx) {} 53 | 54 | bool operator<(const point_hash_idx& p) const { 55 | return (hash_idx_ < p.hash_idx_); 56 | } 57 | }; 58 | 59 | // for KNN 60 | struct point_distance { 61 | public: 62 | point_distance() = default; 63 | point_distance(const Eigen::Vector3d& point, 64 | const double distance, 65 | const size_t point_idx) 66 | : point_(point) 67 | , distance_(distance) 68 | , point_idx_(point_idx) {} 69 | 70 | inline bool operator()(const point_distance& p1, const point_distance& p2) { 71 | return p1.distance_ < p2.distance_; 72 | } 73 | 74 | inline bool operator<(const point_distance& rhs) { 75 | return distance_ < rhs.distance_; 76 | } 77 | 78 | Eigen::Vector3d point_; 79 | double distance_{0.0}; 80 | size_t point_idx_{0}; 81 | }; 82 | 83 | class VoxelMap { 84 | public: 85 | struct Config { 86 | Config(){}; 87 | 88 | double resolution{0.5}; 89 | // using "NEARBY_7" is the most stable 90 | std::string search_method = "NEARBY_7"; 91 | size_t capacity{5000000}; 92 | size_t grid_max_points{20}; 93 | }; 94 | 95 | VoxelMap() = delete; 96 | 97 | VoxelMap(Config config = Config()); 98 | 99 | bool AddCloud(const CloudPtr& input_cloud_ptr); 100 | 101 | bool GetSurroundingGrids(const PointType& point, std::vector& grids); 102 | 103 | size_t ComputeHashIndex(const Eigen::Vector3d& point); 104 | 105 | void ComputeCovariance(std::shared_ptr& grid_ptr); 106 | 107 | bool IsSameGrid(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2); 108 | 109 | bool GetCentroidAndCovariance(const size_t hash_idx, 110 | Eigen::Vector3d& centorid, 111 | Eigen::Matrix3d& cov); 112 | 113 | bool KNNByCondition(const Eigen::Vector3d& point, 114 | const size_t K, 115 | const double range, 116 | std::vector& results); 117 | 118 | size_t GetVoxelMapSize() { return voxel_map_.size(); } 119 | 120 | std::vector delta_P_; 121 | double resolution_{1.0}; 122 | double inv_resolution_{1.0}; 123 | size_t capacity_{5000000}; 124 | size_t grid_max_points_{20}; 125 | 126 | using MyHashMap = tbb::concurrent_hash_map>; 127 | using MyVector = tbb::concurrent_vector>; 128 | using MyAccessor = MyHashMap::accessor; 129 | 130 | std::shared_ptr temp_voxel_map_ptr_; 131 | std::shared_ptr temp_voxel_array_ptr_; 132 | 133 | std::unordered_map< 134 | size_t, 135 | typename std::list>>::iterator> 136 | voxel_map_; 137 | std::list>> grids_cache_; 138 | 139 | size_t HASH_P_{116101}; 140 | size_t MAX_N_{10000000000}; 141 | }; 142 | 143 | #endif -------------------------------------------------------------------------------- /launch/lio_avia.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/lio_bg_avia.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/lio_bg_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/lio_ncd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/lio_nclt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/lio_ulhk.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /log/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ig_lio 4 | 0.0.0 5 | The ig_lio package 6 | 7 | 8 | 9 | 10 | kris 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | livox_ros_driver 63 | livox_ros_driver 64 | livox_ros_driver 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /result/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore -------------------------------------------------------------------------------- /rviz/lio_show.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Odometry1/Shape1 8 | Splitter Ratio: 0.642276406288147 9 | Tree Height: 554 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: current_scan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Angle Tolerance: 0.10000000149011612 55 | Class: rviz/Odometry 56 | Covariance: 57 | Orientation: 58 | Alpha: 0.5 59 | Color: 255; 255; 127 60 | Color Style: Unique 61 | Frame: Local 62 | Offset: 1 63 | Scale: 1 64 | Value: true 65 | Position: 66 | Alpha: 0.30000001192092896 67 | Color: 204; 51; 204 68 | Scale: 1 69 | Value: true 70 | Value: true 71 | Enabled: true 72 | Keep: 1 73 | Name: Odometry 74 | Position Tolerance: 0.10000000149011612 75 | Shape: 76 | Alpha: 1 77 | Axes Length: 2 78 | Axes Radius: 0.5 79 | Color: 255; 25; 0 80 | Head Length: 0.30000001192092896 81 | Head Radius: 0.10000000149011612 82 | Shaft Length: 1 83 | Shaft Radius: 0.05000000074505806 84 | Value: Axes 85 | Topic: /lio_odom 86 | Unreliable: false 87 | Value: true 88 | - Alpha: 1 89 | Autocompute Intensity Bounds: true 90 | Autocompute Value Bounds: 91 | Max Value: 10 92 | Min Value: -10 93 | Value: true 94 | Axis: Z 95 | Channel Name: intensity 96 | Class: rviz/PointCloud2 97 | Color: 255; 255; 255 98 | Color Transformer: FlatColor 99 | Decay Time: 0 100 | Enabled: true 101 | Invert Rainbow: false 102 | Max Color: 255; 255; 255 103 | Min Color: 0; 0; 0 104 | Name: current_scan 105 | Position Transformer: XYZ 106 | Queue Size: 10 107 | Selectable: true 108 | Size (Pixels): 2 109 | Size (m): 0.009999999776482582 110 | Style: Points 111 | Topic: /current_scan 112 | Unreliable: false 113 | Use Fixed Frame: true 114 | Use rainbow: true 115 | Value: true 116 | - Alpha: 0.20000000298023224 117 | Autocompute Intensity Bounds: true 118 | Autocompute Value Bounds: 119 | Max Value: 10 120 | Min Value: -10 121 | Value: true 122 | Axis: Z 123 | Channel Name: intensity 124 | Class: rviz/PointCloud2 125 | Color: 255; 255; 255 126 | Color Transformer: Intensity 127 | Decay Time: 999999 128 | Enabled: false 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Min Color: 0; 0; 0 132 | Name: dense_map 133 | Position Transformer: XYZ 134 | Queue Size: 10 135 | Selectable: true 136 | Size (Pixels): 1 137 | Size (m): 0.009999999776482582 138 | Style: Points 139 | Topic: /current_scan 140 | Unreliable: false 141 | Use Fixed Frame: true 142 | Use rainbow: true 143 | Value: false 144 | - Alpha: 0.20000000298023224 145 | Autocompute Intensity Bounds: true 146 | Autocompute Value Bounds: 147 | Max Value: 10 148 | Min Value: -10 149 | Value: true 150 | Axis: Z 151 | Channel Name: intensity 152 | Class: rviz/PointCloud2 153 | Color: 255; 255; 255 154 | Color Transformer: Intensity 155 | Decay Time: 999999 156 | Enabled: true 157 | Invert Rainbow: false 158 | Max Color: 255; 255; 255 159 | Min Color: 0; 0; 0 160 | Name: sparse_map 161 | Position Transformer: XYZ 162 | Queue Size: 10 163 | Selectable: true 164 | Size (Pixels): 1 165 | Size (m): 0.009999999776482582 166 | Style: Points 167 | Topic: /keyframe_scan 168 | Unreliable: false 169 | Use Fixed Frame: true 170 | Use rainbow: true 171 | Value: true 172 | - Alpha: 0.800000011920929 173 | Autocompute Intensity Bounds: true 174 | Autocompute Value Bounds: 175 | Max Value: 10 176 | Min Value: -10 177 | Value: true 178 | Axis: Z 179 | Channel Name: intensity 180 | Class: rviz/PointCloud2 181 | Color: 255; 255; 255 182 | Color Transformer: Intensity 183 | Decay Time: 30 184 | Enabled: false 185 | Invert Rainbow: false 186 | Max Color: 255; 255; 255 187 | Min Color: 0; 0; 0 188 | Name: sparse_map(docker) 189 | Position Transformer: XYZ 190 | Queue Size: 10 191 | Selectable: true 192 | Size (Pixels): 1 193 | Size (m): 0.009999999776482582 194 | Style: Points 195 | Topic: /keyframe_scan 196 | Unreliable: false 197 | Use Fixed Frame: true 198 | Use rainbow: true 199 | Value: false 200 | - Alpha: 1 201 | Buffer Length: 1 202 | Class: rviz/Path 203 | Color: 25; 255; 0 204 | Enabled: true 205 | Head Diameter: 0.30000001192092896 206 | Head Length: 0.20000000298023224 207 | Length: 0.30000001192092896 208 | Line Style: Billboards 209 | Line Width: 0.20000000298023224 210 | Name: Path 211 | Offset: 212 | X: 0 213 | Y: 0 214 | Z: 0 215 | Pose Color: 255; 85; 255 216 | Pose Style: None 217 | Radius: 0.029999999329447746 218 | Shaft Diameter: 0.10000000149011612 219 | Shaft Length: 0.10000000149011612 220 | Topic: /path 221 | Unreliable: false 222 | Value: true 223 | Enabled: true 224 | Global Options: 225 | Background Color: 0; 0; 0 226 | Default Light: true 227 | Fixed Frame: world 228 | Frame Rate: 30 229 | Name: root 230 | Tools: 231 | - Class: rviz/Interact 232 | Hide Inactive Objects: true 233 | - Class: rviz/MoveCamera 234 | - Class: rviz/Select 235 | - Class: rviz/FocusCamera 236 | - Class: rviz/Measure 237 | - Class: rviz/SetInitialPose 238 | Theta std deviation: 0.2617993950843811 239 | Topic: /initialpose 240 | X std deviation: 0.5 241 | Y std deviation: 0.5 242 | - Class: rviz/SetGoal 243 | Topic: /move_base_simple/goal 244 | - Class: rviz/PublishPoint 245 | Single click: true 246 | Topic: /clicked_point 247 | Value: true 248 | Views: 249 | Current: 250 | Class: rviz/Orbit 251 | Distance: 78.08441925048828 252 | Enable Stereo Rendering: 253 | Stereo Eye Separation: 0.05999999865889549 254 | Stereo Focal Distance: 1 255 | Swap Stereo Eyes: false 256 | Value: false 257 | Focal Point: 258 | X: 0.04535719379782677 259 | Y: -2.343822479248047 260 | Z: -0.18604998290538788 261 | Focal Shape Fixed Size: true 262 | Focal Shape Size: 0.05000000074505806 263 | Invert Z Axis: false 264 | Name: Current View 265 | Near Clip Distance: 0.009999999776482582 266 | Pitch: 0.8247970342636108 267 | Target Frame: base_link 268 | Value: Orbit (rviz) 269 | Yaw: 4.038195610046387 270 | Saved: ~ 271 | Window Geometry: 272 | DecisionMakerPanel: 273 | collapsed: false 274 | Displays: 275 | collapsed: false 276 | Height: 705 277 | Hide Left Dock: false 278 | Hide Right Dock: true 279 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000267fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000267000000c900fffffffa000000020100000003fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000c00430061006d0065007200610000000000ffffffff0000000000000000fb000000100044006900730070006c0061007900730100000000000001360000015600fffffffb0000001600640065006200750067005f0069006d006100670065010000019f000001050000000000000000fb0000000a0049006d006100670065010000028e000000d20000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120049006d006100670065005f0072006100770000000000ffffffff0000000000000000fb0000000c00430061006d006500720061000000024e000001710000000000000000fb000000120049006d00610067006500200052006100770100000421000000160000000000000000fb0000000a0049006d00610067006501000002f4000000cb0000000000000000fb0000000a0049006d006100670065010000056c0000026c0000000000000000fb00000024004400650063006900730069006f006e004d0061006b0065007200500061006e0065006c00000002110000018f0000006e00ffffff000000010000012500000267fc0200000003fb0000000a00560069006500770073000000003d00000267000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f000000a8fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006ca0000005cfc0100000002fb0000000800540069006d00650000000000000006ca000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000003f40000026700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 280 | Selection: 281 | collapsed: false 282 | Time: 283 | collapsed: false 284 | Tool Properties: 285 | collapsed: false 286 | Views: 287 | collapsed: true 288 | Width: 1360 289 | X: 2067 290 | Y: 367 291 | -------------------------------------------------------------------------------- /src/SymmetricEigenSolver.cpp: -------------------------------------------------------------------------------- 1 | #include "ig_lio/SymmetricEigenSolver.h" 2 | 3 | SymmetricEigensolver3x3::SymmetricEigensolver3x3() { 4 | input_.setZero(); 5 | evecs_.setZero(); 6 | evals_.setZero(); 7 | } 8 | 9 | SymmetricEigensolver3x3::SymmetricEigensolver3x3( 10 | const Eigen::Matrix3d input_matrix) { 11 | input_ = input_matrix; 12 | evecs_.setZero(); 13 | evals_.setZero(); 14 | } 15 | 16 | void SymmetricEigensolver3x3::compute() { 17 | double a00 = input_(0, 0); 18 | double a01 = input_(0, 1); 19 | double a02 = input_(0, 2); 20 | double a11 = input_(1, 1); 21 | double a12 = input_(1, 2); 22 | double a22 = input_(2, 2); 23 | 24 | double max0 = (fabs(a00) > fabs(a01)) ? fabs(a00) : fabs(a01); 25 | double max1 = (fabs(a02) > fabs(a11)) ? fabs(a02) : fabs(a11); 26 | double max2 = (fabs(a12) > fabs(a22)) ? fabs(a12) : fabs(a22); 27 | 28 | double maxAbsElement = (max0 > max1) ? max0 : max1; 29 | 30 | maxAbsElement = (maxAbsElement > max2) ? maxAbsElement : max2; 31 | 32 | if (maxAbsElement == 0.0) { 33 | evecs_.setIdentity(); 34 | evals_.setZero(); 35 | 36 | return; 37 | } 38 | 39 | double invMaxAbsElement = 1.0 / maxAbsElement; 40 | 41 | a00 *= invMaxAbsElement; 42 | a01 *= invMaxAbsElement; 43 | a02 *= invMaxAbsElement; 44 | a11 *= invMaxAbsElement; 45 | a12 *= invMaxAbsElement; 46 | a22 *= invMaxAbsElement; 47 | 48 | double norm = a01 * a01 + a02 * a02 + a12 * a12; 49 | 50 | if (norm > 0.0) { 51 | double traceDiv3 = (a00 + a11 + a22) / 3.0; 52 | double b00 = a00 - traceDiv3; 53 | double b11 = a11 - traceDiv3; 54 | double b22 = a22 - traceDiv3; 55 | double denom = sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0); 56 | double c00 = b11 * b22 - a12 * a12; 57 | double c01 = a01 * b22 - a12 * a02; 58 | double c02 = a01 * a12 - b11 * a02; 59 | double det = (b00 * c00 - a01 * c01 + a02 * c02) / (denom * denom * denom); 60 | double halfDet = det * 0.5; 61 | 62 | halfDet = (halfDet > -1.0) ? halfDet : -1.0; 63 | halfDet = (halfDet < 1.0) ? halfDet : 1.0; 64 | 65 | double angle = acos(halfDet) / 3.0; 66 | double beta2 = cos(angle) * 2.0; 67 | double beta0 = cos(angle + M_PI * 2.0 / 3.0) * 2.0; 68 | double beta1 = -(beta0 + beta2); 69 | 70 | evals_(0) = traceDiv3 + denom * beta0; 71 | evals_(1) = traceDiv3 + denom * beta1; 72 | evals_(2) = traceDiv3 + denom * beta2; 73 | 74 | int i0, i2, i1 = 1; 75 | 76 | if (halfDet >= 0.0) { 77 | i0 = 2; 78 | i2 = 0; 79 | } else { 80 | i0 = 0; 81 | i2 = 2; 82 | } 83 | 84 | computeEigenvector0(a00, a01, a02, a11, a12, a22, i0); 85 | computeEigenvector1(a00, a01, a02, a11, a12, a22, i0, i1); 86 | computeEigenvector2(i0, i1, i2); 87 | 88 | } else { 89 | evals_(0) = a00; 90 | evals_(1) = a11; 91 | evals_(2) = a22; 92 | evecs_.setIdentity(); 93 | } 94 | 95 | evals_ *= maxAbsElement; 96 | } 97 | 98 | Eigen::Vector3d SymmetricEigensolver3x3::eigenvalues() { return evals_; } 99 | 100 | Eigen::Matrix3d SymmetricEigensolver3x3::eigenvectors() { return evecs_; } 101 | 102 | void SymmetricEigensolver3x3::computeEigenvector0(double a00, 103 | double a01, 104 | double a02, 105 | double a11, 106 | double a12, 107 | double a22, 108 | int i0) { 109 | Eigen::Matrix3d row_mat; 110 | double eval0 = evals_(i0); 111 | 112 | row_mat(0, 0) = a00 - eval0; 113 | row_mat(0, 1) = a01; 114 | row_mat(0, 2) = a02; 115 | row_mat(1, 0) = a01; 116 | row_mat(1, 1) = a11 - eval0; 117 | row_mat(1, 2) = a12; 118 | row_mat(2, 0) = a02; 119 | row_mat(2, 1) = a12; 120 | row_mat(2, 2) = a22 - eval0; 121 | 122 | // row0 is r0xr1, row1 is r0xr2, row2 is r1xr2 123 | Eigen::Matrix3d rxr; 124 | 125 | rxr.row(0) = cross(row_mat.row(0), row_mat.row(1)); 126 | rxr.row(1) = cross(row_mat.row(0), row_mat.row(2)); 127 | rxr.row(2) = cross(row_mat.row(1), row_mat.row(2)); 128 | 129 | double d0 = 130 | rxr(0, 0) * rxr(0, 0) + rxr(0, 1) * rxr(0, 1) * rxr(0, 2) * rxr(0, 2); 131 | double d1 = 132 | rxr(1, 0) * rxr(1, 0) + rxr(1, 1) * rxr(1, 1) * rxr(1, 2) * rxr(1, 2); 133 | double d2 = 134 | rxr(2, 0) * rxr(2, 0) + rxr(2, 1) * rxr(2, 1) * rxr(2, 2) * rxr(2, 2); 135 | 136 | double dmax = (d0 > d1) ? d0 : d1; 137 | int imax = (d0 > d1) ? 0 : 1; 138 | 139 | dmax = (d2 > dmax) ? d2 : dmax; 140 | imax = (d2 > dmax) ? 2 : imax; 141 | 142 | evecs_.col(i0) = rxr.row(imax) / sqrt(dmax); 143 | } 144 | 145 | void SymmetricEigensolver3x3::computeEigenvector1(double a00, 146 | double a01, 147 | double a02, 148 | double a11, 149 | double a12, 150 | double a22, 151 | int i0, 152 | int i1) { 153 | Eigen::Vector3d u, v; 154 | Eigen::Vector3d evec0 = evecs_.col(i0); 155 | 156 | computeOrthogonalComplement(evec0, u, v); 157 | 158 | Eigen::Vector3d au, av; 159 | double t0, t1, t2; 160 | double eval1 = evals_(i1); 161 | 162 | t0 = u(0); 163 | t1 = u(1); 164 | t2 = u(2); 165 | 166 | au(0) = (a00 - eval1) * t0 + a01 * t1 + a02 * t2; 167 | au(1) = a01 * t0 + (a11 - eval1) * t1 + a12 * t2; 168 | au(2) = a02 * t0 + a12 * t1 + (a22 - eval1) * t2; 169 | 170 | t0 = v(0); 171 | t1 = v(1); 172 | t2 = v(2); 173 | 174 | av(0) = (a00 - eval1) * t0 + a01 * t1 + a02 * t2; 175 | av(1) = a01 * t0 + (a11 - eval1) * t1 + a12 * t2; 176 | av(2) = a02 * t0 + a12 * t1 + (a22 - eval1) * t2; 177 | 178 | double m00 = u(0) * au(0) + u(1) * au(1) + u(2) * au(2); 179 | double m01 = u(0) * av(0) + u(1) * av(1) + u(2) * av(2); 180 | double m11 = v(0) * av(0) + v(1) * av(1) + v(2) * av(2); 181 | 182 | double abs_m00 = fabs(m00); 183 | double abs_m01 = fabs(m01); 184 | double abs_m11 = fabs(m11); 185 | 186 | if (abs_m00 > 0 || abs_m01 > 0 || abs_m11 > 0) { 187 | double u_mult = (abs_m00 >= abs_m11) ? m01 : m11; 188 | double v_mult = (abs_m00 >= abs_m11) ? m00 : m01; 189 | 190 | bool res = fabs(u_mult) >= fabs(v_mult); 191 | double* large = (res) ? &u_mult : &v_mult; 192 | double* small = (res) ? &v_mult : &u_mult; 193 | 194 | *small /= (*large); 195 | *large = 1.0 / sqrt(1.0 + (*small) * (*small)); 196 | *small *= (*large); 197 | 198 | u *= u_mult; 199 | v *= v_mult; 200 | evecs_.col(i1) = u - v; 201 | } else { 202 | evecs_.col(i1) = u; 203 | } 204 | } 205 | 206 | Eigen::Vector3d SymmetricEigensolver3x3::cross(Eigen::Vector3d u, 207 | Eigen::Vector3d v) { 208 | Eigen::Vector3d out; 209 | 210 | out(0) = u(1) * v(2) - u(2) * v(1); 211 | out(1) = u(2) * v(0) - u(0) * v(2); 212 | out(2) = u(0) * v(1) - u(1) * v(0); 213 | 214 | return out; 215 | } 216 | 217 | void SymmetricEigensolver3x3::computeOrthogonalComplement(Eigen::Vector3d& w, 218 | Eigen::Vector3d& u, 219 | Eigen::Vector3d& v) { 220 | bool c = (fabs(w(0)) > fabs(w(1))); 221 | 222 | double inv_length = (c) ? (1.0 / sqrt(w(0) * w(0) + w(2) * w(2))) 223 | : (1.0 / sqrt(w(1) * w(1) + w(2) * w(2))); 224 | 225 | u(0) = (c) ? -w(2) * inv_length : 0.0; 226 | u(1) = (c) ? 0.0 : w(2) * inv_length; 227 | u(2) = (c) ? w(0) * inv_length : -w(1) * inv_length; 228 | 229 | v = cross(w, u); 230 | } 231 | 232 | void SymmetricEigensolver3x3::computeEigenvector2(int i0, int i1, int i2) { 233 | Eigen::Vector3d evec0 = evecs_.col(i0); 234 | Eigen::Vector3d evec1 = evecs_.col(i1); 235 | 236 | evecs_.col(i2) = cross(evec0, evec1); 237 | } -------------------------------------------------------------------------------- /src/faster_voxel_grid.cpp: -------------------------------------------------------------------------------- 1 | #include "ig_lio/faster_voxel_grid.h" 2 | 3 | size_t FasterVoxelGrid::ComputeHashIndex(const Eigen::Vector3d& point) { 4 | double loc_xyz[3]; 5 | for (size_t i = 0; i < 3; ++i) { 6 | loc_xyz[i] = point[i] * inv_resolution_; 7 | if (loc_xyz[i] < 0) { 8 | loc_xyz[i] -= 1.0; 9 | } 10 | } 11 | 12 | size_t x = static_cast(loc_xyz[0]); 13 | size_t y = static_cast(loc_xyz[1]); 14 | size_t z = static_cast(loc_xyz[2]); 15 | 16 | return ((((z)*HASH_P_) % MAX_N_ + (y)) * HASH_P_) % MAX_N_ + (x); 17 | } 18 | 19 | void FasterVoxelGrid::Filter(const CloudPtr& input_cloud_ptr, 20 | CloudPtr& cloud_DS_ptr, 21 | CloudCovPtr& cloud_cov_ptr) { 22 | voxel_array_ptr_->reserve(input_cloud_ptr->size()); 23 | 24 | // Assign each points to the voxel_map 25 | tbb::parallel_for(tbb::blocked_range(0, input_cloud_ptr->size()), 26 | [&, this](tbb::blocked_range r) { 27 | for (size_t i = r.begin(); i < r.end(); ++i) { 28 | const Eigen::Vector3d point = 29 | input_cloud_ptr->points[i] 30 | .getVector3fMap() 31 | .template cast(); 32 | size_t hash_idx = ComputeHashIndex(point); 33 | 34 | MyAccessor accessor; 35 | voxel_map_ptr_->insert(accessor, hash_idx); 36 | 37 | // The grid does not exist, create it 38 | if (accessor->second == nullptr) { 39 | accessor->second = std::make_shared(); 40 | 41 | accessor->second->centorid_ = point; 42 | accessor->second->N_++; 43 | 44 | voxel_array_ptr_->emplace_back(accessor->second); 45 | } 46 | // The grid already exists, update it 47 | else { 48 | accessor->second->N_++; 49 | accessor->second->centorid_ += 50 | (point - accessor->second->centorid_) / 51 | static_cast(accessor->second->N_); 52 | } 53 | } 54 | }); 55 | 56 | // Voxel-based surface covariance estimator 57 | cloud_DS_ptr->resize(voxel_array_ptr_->size()); 58 | cloud_cov_ptr->resize(voxel_array_ptr_->size()); 59 | size_t point_with_cov_count = 0; 60 | tbb::parallel_for( 61 | tbb::blocked_range(0, voxel_array_ptr_->size()), 62 | [&, this](tbb::blocked_range r) { 63 | for (size_t i = r.begin(); i < r.end(); ++i) { 64 | cloud_DS_ptr->points[i].getVector3fMap() = 65 | voxel_array_ptr_->at(i)->centorid_.cast(); 66 | 67 | cloud_cov_ptr->points[i].getVector3fMap() = 68 | voxel_array_ptr_->at(i)->centorid_.cast(); 69 | 70 | Eigen::Matrix3d modified_cov = Eigen::Matrix3d::Zero(); 71 | size_t points_num = 0; 72 | Eigen::Vector3d points_sum = Eigen::Vector3d::Zero(); 73 | Eigen::Matrix3d cov_sum = Eigen::Matrix3d::Zero(); 74 | for (size_t j = 0; j < search_range_.size(); ++j) { 75 | Eigen::Vector3d near_point = 76 | voxel_array_ptr_->at(i)->centorid_ + search_range_[j]; 77 | size_t hash_idx = ComputeHashIndex(near_point); 78 | 79 | MyAccessor accessor; 80 | if (voxel_map_ptr_->find(accessor, hash_idx)) { 81 | points_sum += accessor->second->centorid_; 82 | cov_sum += accessor->second->centorid_ * 83 | accessor->second->centorid_.transpose(); 84 | points_num++; 85 | } 86 | } 87 | 88 | if (points_num >= min_points_per_grid_) { 89 | Eigen::Vector3d centroid = 90 | points_sum / static_cast(points_num); 91 | Eigen::Matrix3d cov = 92 | (cov_sum - points_sum * centroid.transpose()) / 93 | (static_cast(points_num) - 1.0); 94 | 95 | Eigen::JacobiSVD svd( 96 | cov, Eigen::ComputeFullU | Eigen::ComputeFullV); 97 | 98 | Eigen::Vector3d values(1, 1, 1e-3); 99 | 100 | modified_cov = 101 | svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); 102 | 103 | point_with_cov_count++; 104 | } 105 | 106 | cloud_cov_ptr->points[i].cov[0] = modified_cov(0, 0); 107 | cloud_cov_ptr->points[i].cov[1] = modified_cov(0, 1); 108 | cloud_cov_ptr->points[i].cov[2] = modified_cov(0, 2); 109 | cloud_cov_ptr->points[i].cov[3] = modified_cov(1, 1); 110 | cloud_cov_ptr->points[i].cov[4] = modified_cov(1, 2); 111 | cloud_cov_ptr->points[i].cov[5] = modified_cov(2, 2); 112 | } 113 | }); 114 | 115 | frame_count_++; 116 | double current_precent = (static_cast(point_with_cov_count) / 117 | static_cast(cloud_cov_ptr->size())) * 118 | 100.0; 119 | ava_precent_ += 120 | (current_precent - ava_precent_) / static_cast(frame_count_); 121 | 122 | LOG(INFO) << "point with cov size: " << point_with_cov_count 123 | << " total points: " << cloud_cov_ptr->size() 124 | << " precent: " << current_precent 125 | << ", ava_precent: " << ava_precent_; 126 | 127 | // reset voxel_map and voxel_array 128 | voxel_array_ptr_->clear(); 129 | voxel_map_ptr_->clear(); 130 | } 131 | -------------------------------------------------------------------------------- /src/pointcloud_preprocess.cpp: -------------------------------------------------------------------------------- 1 | #include "ig_lio/pointcloud_preprocess.h" 2 | #include "ig_lio/timer.h" 3 | 4 | extern Timer timer; 5 | 6 | void PointCloudPreprocess::Process( 7 | const livox_ros_driver::CustomMsg::ConstPtr& msg, 8 | pcl::PointCloud::Ptr& cloud_out, 9 | const double last_start_time) { 10 | double time_offset = 11 | (msg->header.stamp.toSec() - last_start_time) * 1000.0; // ms 12 | 13 | for (size_t i = 1; i < msg->point_num; ++i) { 14 | if ((msg->points[i].line < num_scans_) && 15 | ((msg->points[i].tag & 0x30) == 0x10 || 16 | (msg->points[i].tag & 0x30) == 0x00) && 17 | !HasInf(msg->points[i]) && !HasNan(msg->points[i]) && 18 | !IsNear(msg->points[i], msg->points[i - 1]) && 19 | (i % config_.point_filter_num == 0)) { 20 | PointType point; 21 | point.normal_x = 0; 22 | point.normal_y = 0; 23 | point.normal_z = 0; 24 | point.x = msg->points[i].x; 25 | point.y = msg->points[i].y; 26 | point.z = msg->points[i].z; 27 | point.intensity = msg->points[i].reflectivity; 28 | point.curvature = time_offset + msg->points[i].offset_time * 1e-6; // ms 29 | cloud_out->push_back(point); 30 | } 31 | } 32 | } 33 | 34 | void PointCloudPreprocess::Process( 35 | const sensor_msgs::PointCloud2::ConstPtr& msg, 36 | pcl::PointCloud::Ptr& cloud_out) { 37 | switch (config_.lidar_type) { 38 | case LidarType::VELODYNE: 39 | ProcessVelodyne(msg, cloud_out); 40 | break; 41 | case LidarType::OUSTER: 42 | ProcessOuster(msg, cloud_out); 43 | break; 44 | default: 45 | LOG(INFO) << "Error LiDAR Type!!!" << std::endl; 46 | exit(0); 47 | } 48 | } 49 | 50 | void PointCloudPreprocess::ProcessVelodyne( 51 | const sensor_msgs::PointCloud2::ConstPtr& msg, 52 | pcl::PointCloud::Ptr& cloud_out) { 53 | pcl::PointCloud cloud_origin; 54 | pcl::fromROSMsg(*msg, cloud_origin); 55 | 56 | // These variables only works when no point timestamps given 57 | int plsize = cloud_origin.size(); 58 | double omega_l = 3.61; // scan angular velocity 59 | std::vector is_first(num_scans_, true); 60 | std::vector yaw_fp(num_scans_, 0.0); // yaw of first scan point 61 | std::vector yaw_last(num_scans_, 0.0); // yaw of last scan point 62 | std::vector time_last(num_scans_, 0.0); // last offset time 63 | if (cloud_origin.back().time > 0) { 64 | has_time_ = true; 65 | } else { 66 | LOG(INFO) << "origin cloud has not timestamp"; 67 | has_time_ = false; 68 | double yaw_first = 69 | atan2(cloud_origin.points[0].y, cloud_origin.points[0].x) * 57.29578; 70 | double yaw_end = yaw_first; 71 | int layer_first = cloud_origin.points[0].ring; 72 | for (uint i = plsize - 1; i > 0; i--) { 73 | if (cloud_origin.points[i].ring == layer_first) { 74 | yaw_end = atan2(cloud_origin.points[i].y, cloud_origin.points[i].x) * 75 | 57.29578; 76 | break; 77 | } 78 | } 79 | } 80 | 81 | cloud_out->reserve(cloud_origin.size()); 82 | 83 | for (size_t i = 0; i < cloud_origin.size(); ++i) { 84 | if ((i % config_.point_filter_num == 0) && !HasInf(cloud_origin.at(i)) && 85 | !HasNan(cloud_origin.at(i))) { 86 | PointType point; 87 | point.normal_x = 0; 88 | point.normal_y = 0; 89 | point.normal_z = 0; 90 | point.x = cloud_origin.at(i).x; 91 | point.y = cloud_origin.at(i).y; 92 | point.z = cloud_origin.at(i).z; 93 | point.intensity = cloud_origin.at(i).intensity; 94 | if (has_time_) { 95 | // curvature unit: ms 96 | point.curvature = cloud_origin.at(i).time * config_.time_scale; 97 | // std::cout<push_back(point); 133 | } 134 | } 135 | } 136 | 137 | void PointCloudPreprocess::ProcessOuster( 138 | const sensor_msgs::PointCloud2::ConstPtr& msg, 139 | pcl::PointCloud::Ptr& cloud_out) { 140 | pcl::PointCloud cloud_origin; 141 | pcl::fromROSMsg(*msg, cloud_origin); 142 | 143 | for (size_t i = 0; i < cloud_origin.size(); ++i) { 144 | if ((i % config_.point_filter_num == 0) && !HasInf(cloud_origin.at(i)) && 145 | !HasNan(cloud_origin.at(i))) { 146 | PointType point; 147 | point.normal_x = 0; 148 | point.normal_y = 0; 149 | point.normal_z = 0; 150 | point.x = cloud_origin.at(i).x; 151 | point.y = cloud_origin.at(i).y; 152 | point.z = cloud_origin.at(i).z; 153 | point.intensity = cloud_origin.at(i).intensity; 154 | // ms 155 | point.curvature = cloud_origin.at(i).t * 1e-6; 156 | cloud_out->push_back(point); 157 | } 158 | } 159 | } 160 | 161 | template 162 | inline bool PointCloudPreprocess::HasInf(const T& p) { 163 | return (std::isinf(p.x) || std::isinf(p.y) || std::isinf(p.z)); 164 | } 165 | 166 | template 167 | inline bool PointCloudPreprocess::HasNan(const T& p) { 168 | return (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)); 169 | } 170 | 171 | template 172 | inline bool PointCloudPreprocess::IsNear(const T& p1, const T& p2) { 173 | return ((abs(p1.x - p2.x) < 1e-7) || (abs(p1.y - p2.y) < 1e-7) || 174 | (abs(p1.z - p2.z) < 1e-7)); 175 | } 176 | -------------------------------------------------------------------------------- /src/timer.cpp: -------------------------------------------------------------------------------- 1 | #include "ig_lio/timer.h" 2 | 3 | /// print the run time 4 | void Timer::PrintAll() { 5 | LOG(INFO) << ">>> ===== Printing run time ====="; 6 | for (const auto& r : records_) { 7 | double time_temp = std::accumulate(r.second.time_usage_in_ms_.begin(), 8 | r.second.time_usage_in_ms_.end(), 9 | 0.0) / 10 | double(r.second.time_usage_in_ms_.size()); 11 | LOG(INFO) << "> [ " << r.first << " ] average time usage: " << time_temp 12 | << " ms , called times: " << r.second.time_usage_in_ms_.size(); 13 | } 14 | } -------------------------------------------------------------------------------- /src/voxel_map.cpp: -------------------------------------------------------------------------------- 1 | #include "ig_lio/voxel_map.h" 2 | 3 | VoxelMap::VoxelMap(Config config) { 4 | resolution_ = config.resolution; 5 | inv_resolution_ = 1.0 / resolution_; 6 | capacity_ = config.capacity; 7 | grid_max_points_ = config.grid_max_points; 8 | 9 | delta_P_.clear(); 10 | if (config.search_method == "NEARBY_1") { 11 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); 12 | } else if (config.search_method == "NEARBY_7") { 13 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); 14 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, 0.0)); 15 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, 0.0)); 16 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, 0.0)); 17 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, 0.0)); 18 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, resolution_)); 19 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, -resolution_)); 20 | } else if (config.search_method == "NEARBY_19") { 21 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); 22 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, 0.0)); 23 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, 0.0)); 24 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, 0.0)); 25 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, 0.0)); 26 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, resolution_)); 27 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, -resolution_)); 28 | delta_P_.push_back(Eigen::Vector3d(resolution_, resolution_, 0.0)); 29 | delta_P_.push_back(Eigen::Vector3d(resolution_, -resolution_, 0.0)); 30 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, resolution_)); 31 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, -resolution_)); 32 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, resolution_)); 33 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, -resolution_)); 34 | delta_P_.push_back(Eigen::Vector3d(-resolution_, resolution_, 0.0)); 35 | delta_P_.push_back(Eigen::Vector3d(-resolution_, -resolution_, 0.0)); 36 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, resolution_)); 37 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, -resolution_)); 38 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, resolution_)); 39 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, -resolution_)); 40 | } else if (config.search_method == "NEARBY_26") { 41 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); 42 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, 0.0)); 43 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, 0.0)); 44 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, 0.0)); 45 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, 0.0)); 46 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, resolution_)); 47 | delta_P_.push_back(Eigen::Vector3d(0.0, 0.0, -resolution_)); 48 | delta_P_.push_back(Eigen::Vector3d(resolution_, resolution_, 0.0)); 49 | delta_P_.push_back(Eigen::Vector3d(resolution_, -resolution_, 0.0)); 50 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, resolution_)); 51 | delta_P_.push_back(Eigen::Vector3d(resolution_, 0.0, -resolution_)); 52 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, resolution_)); 53 | delta_P_.push_back(Eigen::Vector3d(0.0, resolution_, -resolution_)); 54 | delta_P_.push_back(Eigen::Vector3d(-resolution_, resolution_, 0.0)); 55 | delta_P_.push_back(Eigen::Vector3d(-resolution_, -resolution_, 0.0)); 56 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, resolution_)); 57 | delta_P_.push_back(Eigen::Vector3d(-resolution_, 0.0, -resolution_)); 58 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, resolution_)); 59 | delta_P_.push_back(Eigen::Vector3d(0.0, -resolution_, -resolution_)); 60 | delta_P_.push_back(Eigen::Vector3d(resolution_, resolution_, resolution_)); 61 | delta_P_.push_back(Eigen::Vector3d(resolution_, resolution_, -resolution_)); 62 | delta_P_.push_back(Eigen::Vector3d(resolution_, -resolution_, resolution_)); 63 | delta_P_.push_back( 64 | Eigen::Vector3d(resolution_, -resolution_, -resolution_)); 65 | delta_P_.push_back(Eigen::Vector3d(-resolution_, resolution_, resolution_)); 66 | delta_P_.push_back( 67 | Eigen::Vector3d(-resolution_, resolution_, -resolution_)); 68 | delta_P_.push_back( 69 | Eigen::Vector3d(-resolution_, -resolution_, resolution_)); 70 | delta_P_.push_back( 71 | Eigen::Vector3d(-resolution_, -resolution_, -resolution_)); 72 | } 73 | 74 | temp_voxel_map_ptr_ = std::make_shared(); 75 | temp_voxel_array_ptr_ = std::make_shared(); 76 | } 77 | 78 | bool VoxelMap::AddCloud(const CloudPtr& input_cloud_ptr) { 79 | if (input_cloud_ptr->empty()) { 80 | LOG(INFO) << "input cloud is empty"; 81 | return false; 82 | } 83 | 84 | std::vector point_buff; 85 | point_buff.resize(input_cloud_ptr->size()); 86 | 87 | // Follow the algorithm 1 in the paper 88 | tbb::parallel_for( 89 | tbb::blocked_range(0, input_cloud_ptr->size()), 90 | [&, this](tbb::blocked_range r) { 91 | for (size_t i = r.begin(); i < r.end(); ++i) { 92 | point_hash_idx p; 93 | p.point_ = input_cloud_ptr->points[i].getVector3fMap().cast(); 94 | p.hash_idx_ = ComputeHashIndex(p.point_); 95 | point_buff[i] = p; 96 | } 97 | }); 98 | 99 | tbb::parallel_sort(point_buff.begin(), point_buff.end()); 100 | 101 | size_t error_grids = 0; 102 | for (size_t i = 0; i < point_buff.size();) { 103 | size_t j = i; 104 | size_t curr_hash_idx = point_buff.at(i).hash_idx_; 105 | Eigen::Vector3d point_sum = Eigen::Vector3d::Zero(); 106 | Eigen::Matrix3d cov_sum = Eigen::Matrix3d::Zero(); 107 | size_t count = 0; 108 | std::vector points_array_temp; 109 | for (; j < point_buff.size(); ++j) { 110 | if (curr_hash_idx == point_buff.at(j).hash_idx_) { 111 | // prevent hash collision 112 | if (IsSameGrid(point_buff.at(i).point_, point_buff.at(j).point_)) { 113 | point_sum += point_buff.at(j).point_; 114 | cov_sum += 115 | point_buff.at(j).point_ * point_buff.at(j).point_.transpose(); 116 | count++; 117 | points_array_temp.emplace_back(point_buff.at(j).point_); 118 | } else { 119 | // hash collision! 120 | error_grids++; 121 | } 122 | 123 | } else { 124 | break; 125 | } 126 | } 127 | 128 | auto iter = voxel_map_.find(curr_hash_idx); 129 | if (iter == voxel_map_.end()) { 130 | // create a new grid 131 | std::shared_ptr grid = std::make_shared(grid_max_points_); 132 | grid->points_sum_ = point_sum; 133 | grid->points_num_ = count; 134 | grid->cov_sum_ = cov_sum; 135 | grid->points_array_ = points_array_temp; 136 | // compute centroid 137 | grid->centroid_ = 138 | grid->points_sum_ / static_cast(grid->points_num_); 139 | // compute covariance 140 | ComputeCovariance(grid); 141 | 142 | // LRU cache 143 | grids_cache_.push_front({curr_hash_idx, grid}); 144 | voxel_map_.insert({curr_hash_idx, grids_cache_.begin()}); 145 | if (voxel_map_.size() >= capacity_) { 146 | voxel_map_.erase(grids_cache_.back().first); 147 | grids_cache_.pop_back(); 148 | } 149 | } else { 150 | Eigen::Vector3d centroid = point_sum / static_cast(count); 151 | // prevent hash collision 152 | if (!IsSameGrid(iter->second->second->centroid_, centroid)) { 153 | // hash collision! 154 | error_grids++; 155 | // remove this grid 156 | grids_cache_.erase(iter->second); 157 | 158 | // create a new grid 159 | std::shared_ptr grid = std::make_shared(grid_max_points_); 160 | grid->points_sum_ = point_sum; 161 | grid->points_num_ = count; 162 | grid->cov_sum_ = cov_sum; 163 | grid->points_array_ = points_array_temp; 164 | // compute centroid 165 | grid->centroid_ = 166 | grid->points_sum_ / static_cast(grid->points_num_); 167 | // compute covariance 168 | ComputeCovariance(grid); 169 | 170 | grids_cache_.push_front({curr_hash_idx, grid}); 171 | voxel_map_[curr_hash_idx] = grids_cache_.begin(); 172 | } else { 173 | // If the number of points is greater than 50, the probability has 174 | // stabilized and no need to update. 175 | if (iter->second->second->points_num_ < 50) { 176 | iter->second->second->points_sum_ += point_sum; 177 | iter->second->second->points_num_ += count; 178 | iter->second->second->cov_sum_ += cov_sum; 179 | // compute centroid 180 | iter->second->second->centroid_ = 181 | iter->second->second->points_sum_ / 182 | static_cast(iter->second->second->points_num_); 183 | // compute covariance 184 | ComputeCovariance(iter->second->second); 185 | 186 | // Improve KNN efficiency by limiting the number of points per 187 | // grid. 188 | if (iter->second->second->points_num_ < grid_max_points_) { 189 | iter->second->second->points_array_.insert( 190 | iter->second->second->points_array_.end(), 191 | points_array_temp.begin(), 192 | points_array_temp.end()); 193 | } 194 | } 195 | 196 | grids_cache_.splice(grids_cache_.begin(), grids_cache_, iter->second); 197 | voxel_map_[curr_hash_idx] = grids_cache_.begin(); 198 | } 199 | } 200 | i = j; 201 | } 202 | 203 | LOG(INFO) << "total_point " << input_cloud_ptr->size() 204 | << " error_point: " << error_grids; 205 | return true; 206 | } 207 | 208 | void VoxelMap::ComputeCovariance(std::shared_ptr& grid_ptr) { 209 | if (grid_ptr->points_num_ >= 6) { 210 | Eigen::Matrix3d covariance = 211 | (grid_ptr->cov_sum_ - 212 | grid_ptr->points_sum_ * grid_ptr->centroid_.transpose()) / 213 | (static_cast(grid_ptr->points_num_) - 1.0); 214 | 215 | Eigen::JacobiSVD svd( 216 | covariance, Eigen::ComputeFullU | Eigen::ComputeFullV); 217 | 218 | Eigen::Vector3d values(1, 1, 1e-3); 219 | Eigen::Matrix3d modified_cov = 220 | svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); 221 | 222 | grid_ptr->inv_cov_ = modified_cov.inverse(); 223 | grid_ptr->cov_ = modified_cov; 224 | grid_ptr->is_valid_ = true; 225 | } else { 226 | // The number of points is too little to calculate a valid probability. 227 | grid_ptr->is_valid_ = false; 228 | } 229 | } 230 | 231 | bool VoxelMap::KNNByCondition(const Eigen::Vector3d& point, 232 | const size_t K, 233 | const double range, 234 | std::vector& results) { 235 | std::vector point_dist; 236 | point_dist.reserve(delta_P_.size() * grid_max_points_); 237 | double range2 = range * range; 238 | 239 | for (const auto& delta : delta_P_) { 240 | Eigen::Vector3d nearby_point = point + delta; 241 | size_t hash_idx = ComputeHashIndex(nearby_point); 242 | Eigen::Vector3d centroid; 243 | 244 | auto iter = voxel_map_.find(hash_idx); 245 | if (iter != voxel_map_.end() && 246 | IsSameGrid(nearby_point, iter->second->second->centroid_)) { 247 | for (const auto& p : iter->second->second->points_array_) { 248 | double dist = (point - p).squaredNorm(); 249 | 250 | if (dist < range2) { 251 | point_dist.emplace_back(point_distance( 252 | p, dist, (&p - iter->second->second->points_array_.data()))); 253 | } 254 | } 255 | } 256 | } 257 | 258 | if (point_dist.empty()) { 259 | return false; 260 | } 261 | 262 | if (point_dist.size() > K) { 263 | std::nth_element( 264 | point_dist.begin(), point_dist.begin() + K - 1, point_dist.end()); 265 | point_dist.resize(K); 266 | } 267 | std::nth_element(point_dist.begin(), point_dist.begin(), point_dist.end()); 268 | 269 | results.clear(); 270 | for (const auto& it : point_dist) { 271 | results.emplace_back(it.point_); 272 | } 273 | return true; 274 | } 275 | 276 | bool VoxelMap::GetCentroidAndCovariance(const size_t hash_idx, 277 | Eigen::Vector3d& centorid, 278 | Eigen::Matrix3d& cov) { 279 | auto iter = voxel_map_.find(hash_idx); 280 | if (iter != voxel_map_.end() && iter->second->second->is_valid_) { 281 | centorid = iter->second->second->centroid_; 282 | cov = iter->second->second->cov_; 283 | return true; 284 | } else { 285 | return false; 286 | } 287 | } 288 | 289 | size_t VoxelMap::ComputeHashIndex(const Eigen::Vector3d& point) { 290 | double loc_xyz[3]; 291 | for (size_t i = 0; i < 3; ++i) { 292 | loc_xyz[i] = point[i] * inv_resolution_; 293 | if (loc_xyz[i] < 0) { 294 | loc_xyz[i] -= 1.0; 295 | } 296 | } 297 | 298 | size_t x = static_cast(loc_xyz[0]); 299 | size_t y = static_cast(loc_xyz[1]); 300 | size_t z = static_cast(loc_xyz[2]); 301 | 302 | return ((((z)*HASH_P_) % MAX_N_ + (y)) * HASH_P_) % MAX_N_ + (x); 303 | } 304 | 305 | bool VoxelMap::IsSameGrid(const Eigen::Vector3d& p1, 306 | const Eigen::Vector3d& p2) { 307 | int hx_1 = floor(p1.x() * inv_resolution_); 308 | int hy_1 = floor(p1.y() * inv_resolution_); 309 | int hz_1 = floor(p1.z() * inv_resolution_); 310 | int hx_2 = floor(p2.x() * inv_resolution_); 311 | int hy_2 = floor(p2.y() * inv_resolution_); 312 | int hz_2 = floor(p2.z() * inv_resolution_); 313 | 314 | return ((hx_1 == hx_2) && (hy_1 == hy_2) && (hz_1 == hz_2)); 315 | } -------------------------------------------------------------------------------- /thirdparty/sophus/README.md: -------------------------------------------------------------------------------- 1 | # sophus 2 | 3 | sophus for SE(3) and SO(3) -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/average.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Calculation of biinvariant means. 3 | 4 | #ifndef SOPHUS_AVERAGE_HPP 5 | #define SOPHUS_AVERAGE_HPP 6 | 7 | #include "common.hpp" 8 | #include "rxso2.hpp" 9 | #include "rxso3.hpp" 10 | #include "se2.hpp" 11 | #include "se3.hpp" 12 | #include "sim2.hpp" 13 | #include "sim3.hpp" 14 | #include "so2.hpp" 15 | #include "so3.hpp" 16 | 17 | namespace Sophus { 18 | 19 | /// Calculates mean iteratively. 20 | /// 21 | /// Returns ``nullopt`` if it does not converge. 22 | /// 23 | template 24 | optional iterativeMean( 25 | SequenceContainer const& foo_Ts_bar, int max_num_iterations) { 26 | size_t N = foo_Ts_bar.size(); 27 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 28 | 29 | using Group = typename SequenceContainer::value_type; 30 | using Scalar = typename Group::Scalar; 31 | using Tangent = typename Group::Tangent; 32 | 33 | // This implements the algorithm in the beginning of Sec. 4.2 in 34 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 35 | Group foo_T_average = foo_Ts_bar.front(); 36 | Scalar w = Scalar(1. / N); 37 | for (int i = 0; i < max_num_iterations; ++i) { 38 | Tangent average; 39 | setToZero(average); 40 | for (Group const& foo_T_bar : foo_Ts_bar) { 41 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 42 | } 43 | Group foo_T_newaverage = foo_T_average * Group::exp(average); 44 | if (squaredNorm( 45 | (foo_T_newaverage.inverse() * foo_T_average).log()) < 46 | Constants::epsilon()) { 47 | return foo_T_newaverage; 48 | } 49 | 50 | foo_T_average = foo_T_newaverage; 51 | } 52 | // LCOV_EXCL_START 53 | return nullopt; 54 | // LCOV_EXCL_STOP 55 | } 56 | 57 | #ifdef DOXYGEN_SHOULD_SKIP_THIS 58 | /// Mean implementation for any Lie group. 59 | template 60 | optional average( 61 | SequenceContainer const& foo_Ts_bar); 62 | #else 63 | 64 | // Mean implementation for SO(2). 65 | template 67 | enable_if_t< 68 | std::is_same >::value, 69 | optional > 70 | average(SequenceContainer const& foo_Ts_bar) { 71 | // This implements rotational part of Proposition 12 from Sec. 6.2 of 72 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 73 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 74 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 75 | SO2 foo_T_average = foo_Ts_bar.front(); 76 | Scalar w = Scalar(1. / N); 77 | 78 | Scalar average(0); 79 | for (SO2 const& foo_T_bar : foo_Ts_bar) { 80 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 81 | } 82 | return foo_T_average * SO2::exp(average); 83 | } 84 | 85 | // Mean implementation for RxSO(2). 86 | template 88 | enable_if_t< 89 | std::is_same >::value, 90 | optional > 91 | average(SequenceContainer const& foo_Ts_bar) { 92 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 93 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 94 | RxSO2 foo_T_average = foo_Ts_bar.front(); 95 | Scalar w = Scalar(1. / N); 96 | 97 | Vector2 average(Scalar(0), Scalar(0)); 98 | for (RxSO2 const& foo_T_bar : foo_Ts_bar) { 99 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 100 | } 101 | return foo_T_average * RxSO2::exp(average); 102 | } 103 | 104 | namespace details { 105 | template 106 | void getQuaternion(T const&); 107 | 108 | template 109 | Eigen::Quaternion getUnitQuaternion(SO3 const& R) { 110 | return R.unit_quaternion(); 111 | } 112 | 113 | template 114 | Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { 115 | return sR.so3().unit_quaternion(); 116 | } 117 | 118 | template 120 | Eigen::Quaternion averageUnitQuaternion( 121 | SequenceContainer const& foo_Ts_bar) { 122 | // This: http://stackoverflow.com/a/27410865/1221742 123 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 124 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 125 | Eigen::Matrix Q(4, N); 126 | int i = 0; 127 | Scalar w = Scalar(1. / N); 128 | for (auto const& foo_T_bar : foo_Ts_bar) { 129 | Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); 130 | ++i; 131 | } 132 | 133 | Eigen::Matrix QQt = Q * Q.transpose(); 134 | // TODO: Figure out why we can't use SelfAdjointEigenSolver here. 135 | Eigen::EigenSolver > es(QQt); 136 | 137 | std::complex max_eigenvalue = es.eigenvalues()[0]; 138 | Eigen::Matrix, 4, 1> max_eigenvector = 139 | es.eigenvectors().col(0); 140 | 141 | for (int i = 1; i < 4; i++) { 142 | if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { 143 | max_eigenvalue = es.eigenvalues()[i]; 144 | max_eigenvector = es.eigenvectors().col(i); 145 | } 146 | } 147 | Eigen::Quaternion quat; 148 | quat.coeffs() << // 149 | max_eigenvector[0].real(), // 150 | max_eigenvector[1].real(), // 151 | max_eigenvector[2].real(), // 152 | max_eigenvector[3].real(); 153 | return quat; 154 | } 155 | } // namespace details 156 | 157 | // Mean implementation for SO(3). 158 | // 159 | // TODO: Detect degenerated cases and return nullopt. 160 | template 162 | enable_if_t< 163 | std::is_same >::value, 164 | optional > 165 | average(SequenceContainer const& foo_Ts_bar) { 166 | return SO3(details::averageUnitQuaternion(foo_Ts_bar)); 167 | } 168 | 169 | // Mean implementation for R x SO(3). 170 | template 172 | enable_if_t< 173 | std::is_same >::value, 174 | optional > 175 | average(SequenceContainer const& foo_Ts_bar) { 176 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 177 | 178 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 179 | Scalar scale_sum = Scalar(0); 180 | using std::exp; 181 | using std::log; 182 | for (RxSO3 const& foo_T_bar : foo_Ts_bar) { 183 | scale_sum += log(foo_T_bar.scale()); 184 | } 185 | return RxSO3(exp(scale_sum / Scalar(N)), 186 | SO3(details::averageUnitQuaternion(foo_Ts_bar))); 187 | } 188 | 189 | template 191 | enable_if_t< 192 | std::is_same >::value, 193 | optional > 194 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 195 | // TODO: Implement Proposition 12 from Sec. 6.2 of 196 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 197 | return iterativeMean(foo_Ts_bar, max_num_iterations); 198 | } 199 | 200 | template 202 | enable_if_t< 203 | std::is_same >::value, 204 | optional > 205 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 206 | return iterativeMean(foo_Ts_bar, max_num_iterations); 207 | } 208 | 209 | template 211 | enable_if_t< 212 | std::is_same >::value, 213 | optional > 214 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 215 | return iterativeMean(foo_Ts_bar, max_num_iterations); 216 | } 217 | 218 | template 220 | enable_if_t< 221 | std::is_same >::value, 222 | optional > 223 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 224 | return iterativeMean(foo_Ts_bar, max_num_iterations); 225 | } 226 | 227 | #endif // DOXYGEN_SHOULD_SKIP_THIS 228 | 229 | } // namespace Sophus 230 | 231 | #endif // SOPHUS_AVERAGE_HPP 232 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/common.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Common functionality. 3 | 4 | #ifndef SOPHUS_COMMON_HPP 5 | #define SOPHUS_COMMON_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #if !defined(SOPHUS_DISABLE_ENSURES) 16 | #include "formatstring.hpp" 17 | #endif //!defined(SOPHUS_DISABLE_ENSURES) 18 | 19 | // following boost's assert.hpp 20 | #undef SOPHUS_ENSURE 21 | 22 | // ENSURES are similar to ASSERTS, but they are always checked for (including in 23 | // release builds). At the moment there are no ASSERTS in Sophus which should 24 | // only be used for checks which are performance critical. 25 | 26 | #ifdef __GNUC__ 27 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 28 | #elif (_MSC_VER >= 1310) 29 | #define SOPHUS_FUNCTION __FUNCTION__ 30 | #else 31 | #define SOPHUS_FUNCTION "unknown" 32 | #endif 33 | 34 | // Make sure this compiles with older versions of Eigen which do not have 35 | // EIGEN_DEVICE_FUNC defined. 36 | #ifndef EIGEN_DEVICE_FUNC 37 | #define EIGEN_DEVICE_FUNC 38 | #endif 39 | 40 | // Make sure this compiles with older versions of Eigen which do not have 41 | // EIGEN_DEFAULT_COPY_CONSTRUCTOR defined 42 | #ifndef EIGEN_DEFAULT_COPY_CONSTRUCTOR 43 | #if EIGEN_HAS_CXX11 44 | #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default; 45 | #else 46 | #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) 47 | #endif 48 | #ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS 49 | #error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS" 50 | #endif 51 | #define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ 52 | EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ 53 | EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived) 54 | #endif 55 | 56 | #ifndef SOPHUS_INHERIT_ASSIGNMENT_OPERATORS 57 | #ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS 58 | #error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS" 59 | #endif 60 | #define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) 61 | #endif 62 | 63 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 64 | 65 | #if defined(SOPHUS_DISABLE_ENSURES) 66 | 67 | #define SOPHUS_ENSURE(expr, ...) ((void)0) 68 | 69 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 70 | 71 | namespace Sophus { 72 | void ensureFailed(char const* function, char const* file, int line, 73 | char const* description); 74 | } 75 | 76 | #define SOPHUS_ENSURE(expr, ...) \ 77 | ((expr) ? ((void)0) \ 78 | : ::Sophus::ensureFailed( \ 79 | SOPHUS_FUNCTION, __FILE__, __LINE__, \ 80 | Sophus::details::FormatString(__VA_ARGS__).c_str())) 81 | #else 82 | // LCOV_EXCL_START 83 | 84 | namespace Sophus { 85 | template 86 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, 87 | char const* description, Args&&... args) { 88 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 89 | function, file, line); 90 | #ifdef __CUDACC__ 91 | std::printf("%s", description); 92 | #else 93 | std::cout << details::FormatString(description, std::forward(args)...) 94 | << std::endl; 95 | std::abort(); 96 | #endif 97 | } 98 | } // namespace Sophus 99 | 100 | // LCOV_EXCL_STOP 101 | #define SOPHUS_ENSURE(expr, ...) \ 102 | ((expr) ? ((void)0) \ 103 | : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 104 | ##__VA_ARGS__)) 105 | #endif 106 | 107 | namespace Sophus { 108 | 109 | template 110 | struct Constants { 111 | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } 112 | 113 | SOPHUS_FUNC static Scalar epsilonSqrt() { 114 | using std::sqrt; 115 | return sqrt(epsilon()); 116 | } 117 | 118 | SOPHUS_FUNC static Scalar pi() { 119 | return Scalar(3.141592653589793238462643383279502884); 120 | } 121 | }; 122 | 123 | template <> 124 | struct Constants { 125 | SOPHUS_FUNC static float constexpr epsilon() { 126 | return static_cast(1e-5); 127 | } 128 | 129 | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } 130 | 131 | SOPHUS_FUNC static float constexpr pi() { 132 | return 3.141592653589793238462643383279502884f; 133 | } 134 | }; 135 | 136 | /// Nullopt type of lightweight optional class. 137 | struct nullopt_t { 138 | explicit constexpr nullopt_t() {} 139 | }; 140 | 141 | constexpr nullopt_t nullopt{}; 142 | 143 | /// Lightweight optional implementation which requires ``T`` to have a 144 | /// default constructor. 145 | /// 146 | /// TODO: Replace with std::optional once Sophus moves to c++17. 147 | /// 148 | template 149 | class optional { 150 | public: 151 | optional() : is_valid_(false) {} 152 | 153 | optional(nullopt_t) : is_valid_(false) {} 154 | 155 | optional(T const& type) : type_(type), is_valid_(true) {} 156 | 157 | explicit operator bool() const { return is_valid_; } 158 | 159 | T const* operator->() const { 160 | SOPHUS_ENSURE(is_valid_, "must be valid"); 161 | return &type_; 162 | } 163 | 164 | T* operator->() { 165 | SOPHUS_ENSURE(is_valid_, "must be valid"); 166 | return &type_; 167 | } 168 | 169 | T const& operator*() const { 170 | SOPHUS_ENSURE(is_valid_, "must be valid"); 171 | return type_; 172 | } 173 | 174 | T& operator*() { 175 | SOPHUS_ENSURE(is_valid_, "must be valid"); 176 | return type_; 177 | } 178 | 179 | private: 180 | T type_; 181 | bool is_valid_; 182 | }; 183 | 184 | template 185 | using enable_if_t = typename std::enable_if::type; 186 | 187 | template 188 | struct IsUniformRandomBitGenerator { 189 | static const bool value = std::is_unsigned::value && 190 | std::is_unsigned::value && 191 | std::is_unsigned::value; 192 | }; 193 | } // namespace Sophus 194 | 195 | #endif // SOPHUS_COMMON_HPP 196 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/example_ensure_handler.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include "common.hpp" 24 | 25 | #include 26 | #include 27 | 28 | namespace Sophus { 29 | void ensureFailed(const char* function, const char* file, int line, 30 | const char* description) { 31 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 32 | file, function, line); 33 | std::printf("Description: %s\n", description); 34 | std::abort(); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/formatstring.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// FormatString functionality 3 | 4 | #ifndef SOPHUS_FORMATSTRING_HPP 5 | #define SOPHUS_FORMATSTRING_HPP 6 | 7 | #include 8 | 9 | namespace Sophus { 10 | namespace details { 11 | 12 | // Following: http://stackoverflow.com/a/22759544 13 | template 14 | class IsStreamable { 15 | private: 16 | template 17 | static auto test(int) 18 | -> decltype(std::declval() << std::declval(), 19 | std::true_type()); 20 | 21 | template 22 | static auto test(...) -> std::false_type; 23 | 24 | public: 25 | static bool const value = decltype(test(0))::value; 26 | }; 27 | 28 | template 29 | class ArgToStream { 30 | public: 31 | static void impl(std::stringstream& stream, T&& arg) { 32 | stream << std::forward(arg); 33 | } 34 | }; 35 | 36 | inline void FormatStream(std::stringstream& stream, char const* text) { 37 | stream << text; 38 | return; 39 | } 40 | 41 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack 42 | template 43 | void FormatStream(std::stringstream& stream, char const* text, T&& arg, 44 | Args&&... args) { 45 | static_assert(IsStreamable::value, 46 | "One of the args has no ostream overload!"); 47 | for (; *text != '\0'; ++text) { 48 | if (*text == '%') { 49 | ArgToStream::impl(stream, std::forward(arg)); 50 | FormatStream(stream, text + 1, std::forward(args)...); 51 | return; 52 | } 53 | stream << *text; 54 | } 55 | stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 56 | << " args unused."; 57 | return; 58 | } 59 | 60 | template 61 | std::string FormatString(char const* text, Args&&... args) { 62 | std::stringstream stream; 63 | FormatStream(stream, text, std::forward(args)...); 64 | return stream.str(); 65 | } 66 | 67 | inline std::string FormatString() { return std::string(); } 68 | 69 | } // namespace details 70 | } // namespace Sophus 71 | 72 | #endif //SOPHUS_FORMATSTRING_HPP 73 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/geometry.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Transformations between poses and hyperplanes. 3 | 4 | #ifndef GEOMETRY_HPP 5 | #define GEOMETRY_HPP 6 | 7 | #include "se2.hpp" 8 | #include "se3.hpp" 9 | #include "so2.hpp" 10 | #include "so3.hpp" 11 | #include "types.hpp" 12 | 13 | namespace Sophus { 14 | 15 | /// Takes in a rotation ``R_foo_plane`` and returns the corresponding line 16 | /// normal along the y-axis (in reference frame ``foo``). 17 | /// 18 | template 19 | Vector2 normalFromSO2(SO2 const& R_foo_line) { 20 | return R_foo_line.matrix().col(1); 21 | } 22 | 23 | /// Takes in line normal in reference frame foo and constructs a corresponding 24 | /// rotation matrix ``R_foo_line``. 25 | /// 26 | /// Precondition: ``normal_foo`` must not be close to zero. 27 | /// 28 | template 29 | SO2 SO2FromNormal(Vector2 normal_foo) { 30 | SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", 31 | normal_foo.transpose()); 32 | normal_foo.normalize(); 33 | return SO2(normal_foo.y(), -normal_foo.x()); 34 | } 35 | 36 | /// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane 37 | /// normal along the z-axis 38 | /// (in reference frame ``foo``). 39 | /// 40 | template 41 | Vector3 normalFromSO3(SO3 const& R_foo_plane) { 42 | return R_foo_plane.matrix().col(2); 43 | } 44 | 45 | /// Takes in plane normal in reference frame foo and constructs a corresponding 46 | /// rotation matrix ``R_foo_plane``. 47 | /// 48 | /// Note: The ``plane`` frame is defined as such that the normal points along 49 | /// the positive z-axis. One can specify hints for the x-axis and y-axis 50 | /// of the ``plane`` frame. 51 | /// 52 | /// Preconditions: 53 | /// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to 54 | /// zero. 55 | /// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. 56 | /// 57 | template 58 | Matrix3 rotationFromNormal(Vector3 const& normal_foo, 59 | Vector3 xDirHint_foo = Vector3(T(1), T(0), 60 | T(0)), 61 | Vector3 yDirHint_foo = Vector3(T(0), T(1), 62 | T(0))) { 63 | SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), 64 | "xDirHint (%) and yDirHint (%) must be perpendicular.", 65 | xDirHint_foo.transpose(), yDirHint_foo.transpose()); 66 | using std::abs; 67 | using std::sqrt; 68 | T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); 69 | T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); 70 | T const normal_foo_sqr_length = normal_foo.squaredNorm(); 71 | SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", 72 | xDirHint_foo.transpose()); 73 | SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", 74 | yDirHint_foo.transpose()); 75 | SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", 76 | normal_foo.transpose()); 77 | 78 | Matrix3 basis_foo; 79 | basis_foo.col(2) = normal_foo; 80 | 81 | if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 82 | xDirHint_foo.normalize(); 83 | } 84 | if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 85 | yDirHint_foo.normalize(); 86 | } 87 | if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { 88 | basis_foo.col(2).normalize(); 89 | } 90 | 91 | T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); 92 | T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); 93 | if (abs_x_dot_z < abs_y_dot_z) { 94 | // basis_foo.z and xDirHint are far from parallel. 95 | basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); 96 | basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); 97 | } else { 98 | // basis_foo.z and yDirHint are far from parallel. 99 | basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); 100 | basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); 101 | } 102 | T det = basis_foo.determinant(); 103 | // sanity check 104 | SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), 105 | "Determinant of basis is not 1, but %. Basis is \n%\n", det, 106 | basis_foo); 107 | return basis_foo; 108 | } 109 | 110 | /// Takes in plane normal in reference frame foo and constructs a corresponding 111 | /// rotation matrix ``R_foo_plane``. 112 | /// 113 | /// See ``rotationFromNormal`` for details. 114 | /// 115 | template 116 | SO3 SO3FromNormal(Vector3 const& normal_foo) { 117 | return SO3(rotationFromNormal(normal_foo)); 118 | } 119 | 120 | /// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in 121 | /// reference frame ``foo``. 122 | /// 123 | /// Note: The plane is defined by X-axis of the ``line`` frame. 124 | /// 125 | template 126 | Line2 lineFromSE2(SE2 const& T_foo_line) { 127 | return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); 128 | } 129 | 130 | /// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. 131 | /// 132 | /// Note: The line is defined by X-axis of the frame ``line``. 133 | /// 134 | template 135 | SE2 SE2FromLine(Line2 const& line_foo) { 136 | T const d = line_foo.offset(); 137 | Vector2 const n = line_foo.normal(); 138 | SO2 const R_foo_plane = SO2FromNormal(n); 139 | return SE2(R_foo_plane, -d * n); 140 | } 141 | 142 | /// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in 143 | /// reference frame ``foo``. 144 | /// 145 | /// Note: The plane is defined by XY-plane of the frame ``plane``. 146 | /// 147 | template 148 | Plane3 planeFromSE3(SE3 const& T_foo_plane) { 149 | return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); 150 | } 151 | 152 | /// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. 153 | /// 154 | /// Note: The plane is defined by XY-plane of the frame ``plane``. 155 | /// 156 | template 157 | SE3 SE3FromPlane(Plane3 const& plane_foo) { 158 | T const d = plane_foo.offset(); 159 | Vector3 const n = plane_foo.normal(); 160 | SO3 const R_foo_plane = SO3FromNormal(n); 161 | return SE3(R_foo_plane, -d * n); 162 | } 163 | 164 | /// Takes in a hyperplane and returns unique representation by ensuring that the 165 | /// ``offset`` is not negative. 166 | /// 167 | template 168 | Eigen::Hyperplane makeHyperplaneUnique( 169 | Eigen::Hyperplane const& plane) { 170 | if (plane.offset() >= 0) { 171 | return plane; 172 | } 173 | 174 | return Eigen::Hyperplane(-plane.normal(), -plane.offset()); 175 | } 176 | 177 | } // namespace Sophus 178 | 179 | #endif // GEOMETRY_HPP 180 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/interpolate.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Interpolation for Lie groups. 3 | 4 | #ifndef SOPHUS_INTERPOLATE_HPP 5 | #define SOPHUS_INTERPOLATE_HPP 6 | 7 | #include 8 | 9 | #include "interpolate_details.hpp" 10 | 11 | namespace Sophus { 12 | 13 | /// This function interpolates between two Lie group elements ``foo_T_bar`` 14 | /// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. 15 | /// 16 | /// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` 17 | /// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns 18 | /// ``foo_T_bar``. 19 | /// 20 | /// (Since interpolation on Lie groups is inverse-invariant, we can equivalently 21 | /// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the 22 | /// return value being ``quiz_T_foo``.) 23 | /// 24 | /// Precondition: ``p`` must be in [0, 1]. 25 | /// 26 | template 27 | enable_if_t::supported, G> interpolate( 28 | G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { 29 | using Scalar = typename G::Scalar; 30 | Scalar inter_p(p); 31 | SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), 32 | "p (%) must in [0, 1]."); 33 | return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); 34 | } 35 | 36 | } // namespace Sophus 37 | 38 | #endif // SOPHUS_INTERPOLATE_HPP 39 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/interpolate_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP 2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP 3 | 4 | #include "rxso2.hpp" 5 | #include "rxso3.hpp" 6 | #include "se2.hpp" 7 | #include "se3.hpp" 8 | #include "sim2.hpp" 9 | #include "sim3.hpp" 10 | #include "so2.hpp" 11 | #include "so3.hpp" 12 | 13 | namespace Sophus { 14 | namespace interp_details { 15 | 16 | template 17 | struct Traits; 18 | 19 | template 20 | struct Traits> { 21 | static bool constexpr supported = true; 22 | 23 | static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { 24 | using std::abs; 25 | Scalar angle = foo_T_bar.log(); 26 | return abs(abs(angle) - Constants::pi()) < 27 | Constants::epsilon(); 28 | } 29 | }; 30 | 31 | template 32 | struct Traits> { 33 | static bool constexpr supported = true; 34 | 35 | static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { 36 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 37 | } 38 | }; 39 | 40 | template 41 | struct Traits> { 42 | static bool constexpr supported = true; 43 | 44 | static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { 45 | using std::abs; 46 | Scalar angle = foo_T_bar.logAndTheta().theta; 47 | return abs(abs(angle) - Constants::pi()) < 48 | Constants::epsilon(); 49 | } 50 | }; 51 | 52 | template 53 | struct Traits> { 54 | static bool constexpr supported = true; 55 | 56 | static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { 57 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 58 | } 59 | }; 60 | 61 | template 62 | struct Traits> { 63 | static bool constexpr supported = true; 64 | 65 | static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { 66 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 67 | } 68 | }; 69 | 70 | template 71 | struct Traits> { 72 | static bool constexpr supported = true; 73 | 74 | static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { 75 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 76 | } 77 | }; 78 | 79 | template 80 | struct Traits> { 81 | static bool constexpr supported = true; 82 | 83 | static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { 84 | return Traits>::hasShortestPathAmbiguity( 85 | foo_T_bar.rxso2().so2()); 86 | ; 87 | } 88 | }; 89 | 90 | template 91 | struct Traits> { 92 | static bool constexpr supported = true; 93 | 94 | static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { 95 | return Traits>::hasShortestPathAmbiguity( 96 | foo_T_bar.rxso3().so3()); 97 | ; 98 | } 99 | }; 100 | 101 | } // namespace interp_details 102 | } // namespace Sophus 103 | 104 | #endif // SOPHUS_INTERPOLATE_DETAILS_HPP 105 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/num_diff.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Numerical differentiation using finite differences 3 | 4 | #ifndef SOPHUS_NUM_DIFF_HPP 5 | #define SOPHUS_NUM_DIFF_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "types.hpp" 12 | 13 | namespace Sophus { 14 | 15 | namespace details { 16 | template 17 | class Curve { 18 | public: 19 | template 20 | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { 21 | using ReturnType = decltype(curve(t)); 22 | static_assert(std::is_floating_point::value, 23 | "Scalar must be a floating point type."); 24 | static_assert(IsFloatingPoint::value, 25 | "ReturnType must be either a floating point scalar, " 26 | "vector or matrix."); 27 | 28 | return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); 29 | } 30 | }; 31 | 32 | template 33 | class VectorField { 34 | public: 35 | static Eigen::Matrix num_diff( 36 | std::function(Sophus::Vector)> 37 | vector_field, 38 | Sophus::Vector const& a, Scalar eps) { 39 | static_assert(std::is_floating_point::value, 40 | "Scalar must be a floating point type."); 41 | Eigen::Matrix J; 42 | Sophus::Vector h; 43 | h.setZero(); 44 | for (int i = 0; i < M; ++i) { 45 | h[i] = eps; 46 | J.col(i) = 47 | (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); 48 | h[i] = Scalar(0); 49 | } 50 | 51 | return J; 52 | } 53 | }; 54 | 55 | template 56 | class VectorField { 57 | public: 58 | static Eigen::Matrix num_diff( 59 | std::function(Scalar)> vector_field, 60 | Scalar const& a, Scalar eps) { 61 | return details::Curve::num_diff(std::move(vector_field), a, eps); 62 | } 63 | }; 64 | } // namespace details 65 | 66 | /// Calculates the derivative of a curve at a point ``t``. 67 | /// 68 | /// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it 69 | /// returns either a Scalar, a vector or a matrix. 70 | /// 71 | template 72 | auto curveNumDiff(Fn curve, Scalar t, 73 | Scalar h = Constants::epsilonSqrt()) 74 | -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { 75 | return details::Curve::num_diff(std::move(curve), t, h); 76 | } 77 | 78 | /// Calculates the derivative of a vector field at a point ``a``. 79 | /// 80 | /// Here, a vector field is a function from a vector space to another vector 81 | /// space. 82 | /// 83 | template 84 | Eigen::Matrix vectorFieldNumDiff( 85 | Fn vector_field, ScalarOrVector const& a, 86 | Scalar eps = Constants::epsilonSqrt()) { 87 | return details::VectorField::num_diff(std::move(vector_field), 88 | a, eps); 89 | } 90 | 91 | } // namespace Sophus 92 | 93 | #endif // SOPHUS_NUM_DIFF_HPP 94 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/rotation_matrix.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Rotation matrix helper functions. 3 | 4 | #ifndef SOPHUS_ROTATION_MATRIX_HPP 5 | #define SOPHUS_ROTATION_MATRIX_HPP 6 | 7 | #include 8 | #include 9 | 10 | #include "types.hpp" 11 | 12 | namespace Sophus { 13 | 14 | /// Takes in arbitrary square matrix and returns true if it is 15 | /// orthogonal. 16 | template 17 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { 18 | using Scalar = typename D::Scalar; 19 | static int const N = D::RowsAtCompileTime; 20 | static int const M = D::ColsAtCompileTime; 21 | 22 | static_assert(N == M, "must be a square matrix"); 23 | static_assert(N >= 2, "must have compile time dimension >= 2"); 24 | 25 | return (R * R.transpose() - Matrix::Identity()).norm() < 26 | Constants::epsilon(); 27 | } 28 | 29 | /// Takes in arbitrary square matrix and returns true if it is 30 | /// "scaled-orthogonal" with positive determinant. 31 | /// 32 | template 33 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { 34 | using Scalar = typename D::Scalar; 35 | static int const N = D::RowsAtCompileTime; 36 | static int const M = D::ColsAtCompileTime; 37 | using std::pow; 38 | using std::sqrt; 39 | 40 | Scalar det = sR.determinant(); 41 | 42 | if (det <= Scalar(0)) { 43 | return false; 44 | } 45 | 46 | Scalar scale_sqr = pow(det, Scalar(2. / N)); 47 | 48 | static_assert(N == M, "must be a square matrix"); 49 | static_assert(N >= 2, "must have compile time dimension >= 2"); 50 | 51 | return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) 52 | .template lpNorm() < 53 | sqrt(Constants::epsilon()); 54 | } 55 | 56 | /// Takes in arbitrary square matrix (2x2 or larger) and returns closest 57 | /// orthogonal matrix with positive determinant. 58 | template 59 | SOPHUS_FUNC enable_if_t< 60 | std::is_floating_point::value, 61 | Matrix> 62 | makeRotationMatrix(Eigen::MatrixBase const& R) { 63 | using Scalar = typename D::Scalar; 64 | static int const N = D::RowsAtCompileTime; 65 | static int const M = D::ColsAtCompileTime; 66 | 67 | static_assert(N == M, "must be a square matrix"); 68 | static_assert(N >= 2, "must have compile time dimension >= 2"); 69 | 70 | Eigen::JacobiSVD> svd( 71 | R, Eigen::ComputeFullU | Eigen::ComputeFullV); 72 | 73 | // Determine determinant of orthogonal matrix U*V'. 74 | Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); 75 | // Starting from the identity matrix D, set the last entry to d (+1 or 76 | // -1), so that det(U*D*V') = 1. 77 | Matrix Diag = Matrix::Identity(); 78 | Diag(N - 1, N - 1) = d; 79 | return svd.matrixU() * Diag * svd.matrixV().transpose(); 80 | } 81 | 82 | } // namespace Sophus 83 | 84 | #endif // SOPHUS_ROTATION_MATRIX_HPP 85 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/rxso2.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Direct product R X SO(2) - rotation and scaling in 2d. 3 | 4 | #ifndef SOPHUS_RXSO2_HPP 5 | #define SOPHUS_RXSO2_HPP 6 | 7 | #include "so2.hpp" 8 | 9 | namespace Sophus { 10 | template 11 | class RxSO2; 12 | using RxSO2d = RxSO2; 13 | using RxSO2f = RxSO2; 14 | } // namespace Sophus 15 | 16 | namespace Eigen { 17 | namespace internal { 18 | 19 | template 20 | struct traits> { 21 | static constexpr int Options = Options_; 22 | using Scalar = Scalar_; 23 | using ComplexType = Sophus::Vector2; 24 | }; 25 | 26 | template 27 | struct traits, Options_>> 28 | : traits> { 29 | static constexpr int Options = Options_; 30 | using Scalar = Scalar_; 31 | using ComplexType = Map, Options>; 32 | }; 33 | 34 | template 35 | struct traits const, Options_>> 36 | : traits const> { 37 | static constexpr int Options = Options_; 38 | using Scalar = Scalar_; 39 | using ComplexType = Map const, Options>; 40 | }; 41 | } // namespace internal 42 | } // namespace Eigen 43 | 44 | namespace Sophus { 45 | 46 | /// RxSO2 base type - implements RxSO2 class but is storage agnostic 47 | /// 48 | /// This class implements the group ``R+ x SO(2)``, the direct product of the 49 | /// group of positive scalar 2x2 matrices (= isomorph to the positive 50 | /// real numbers) and the two-dimensional special orthogonal group SO(2). 51 | /// Geometrically, it is the group of rotation and scaling in two dimensions. 52 | /// As a matrix groups, R+ x SO(2) consists of matrices of the form ``s * R`` 53 | /// where ``R`` is an orthogonal matrix with ``det(R) = 1`` and ``s > 0`` 54 | /// being a positive real number. In particular, it has the following form: 55 | /// 56 | /// | s * cos(theta) s * -sin(theta) | 57 | /// | s * sin(theta) s * cos(theta) | 58 | /// 59 | /// where ``theta`` being the rotation angle. Internally, it is represented by 60 | /// the first column of the rotation matrix, or in other words by a non-zero 61 | /// complex number. 62 | /// 63 | /// R+ x SO(2) is not compact, but a commutative group. First it is not compact 64 | /// since the scale factor is not bound. Second it is commutative since 65 | /// ``sR(alpha, s1) * sR(beta, s2) = sR(beta, s2) * sR(alpha, s1)``, simply 66 | /// because ``alpha + beta = beta + alpha`` and ``s1 * s2 = s2 * s1`` with 67 | /// ``alpha`` and ``beta`` being rotation angles and ``s1``, ``s2`` being scale 68 | /// factors. 69 | /// 70 | /// This class has the explicit class invariant that the scale ``s`` is not 71 | /// too close to zero. Strictly speaking, it must hold that: 72 | /// 73 | /// ``complex().norm() >= Constants::epsilon()``. 74 | /// 75 | /// In order to obey this condition, group multiplication is implemented with 76 | /// saturation such that a product always has a scale which is equal or greater 77 | /// this threshold. 78 | template 79 | class RxSO2Base { 80 | public: 81 | static constexpr int Options = Eigen::internal::traits::Options; 82 | using Scalar = typename Eigen::internal::traits::Scalar; 83 | using ComplexType = typename Eigen::internal::traits::ComplexType; 84 | using ComplexTemporaryType = Sophus::Vector2; 85 | 86 | /// Degrees of freedom of manifold, number of dimensions in tangent space 87 | /// (one for rotation and one for scaling). 88 | static int constexpr DoF = 2; 89 | /// Number of internal parameters used (complex number is a tuple). 90 | static int constexpr num_parameters = 2; 91 | /// Group transformations are 2x2 matrices. 92 | static int constexpr N = 2; 93 | using Transformation = Matrix; 94 | using Point = Vector2; 95 | using HomogeneousPoint = Vector3; 96 | using Line = ParametrizedLine2; 97 | using Tangent = Vector; 98 | using Adjoint = Matrix; 99 | 100 | /// For binary operations the return type is determined with the 101 | /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map 102 | /// types, as well as other compatible scalar types such as Ceres::Jet and 103 | /// double scalars with RxSO2 operations. 104 | template 105 | using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< 106 | Scalar, typename OtherDerived::Scalar>::ReturnType; 107 | 108 | template 109 | using RxSO2Product = RxSO2>; 110 | 111 | template 112 | using PointProduct = Vector2>; 113 | 114 | template 115 | using HomogeneousPointProduct = Vector3>; 116 | 117 | /// Adjoint transformation 118 | /// 119 | /// This function return the adjoint transformation ``Ad`` of the group 120 | /// element ``A`` such that for all ``x`` it holds that 121 | /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. 122 | /// 123 | /// For RxSO(2), it simply returns the identity matrix. 124 | /// 125 | SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); } 126 | 127 | /// Returns rotation angle. 128 | /// 129 | SOPHUS_FUNC Scalar angle() const { return SO2(complex()).log(); } 130 | 131 | /// Returns copy of instance casted to NewScalarType. 132 | /// 133 | template 134 | SOPHUS_FUNC RxSO2 cast() const { 135 | return RxSO2(complex().template cast()); 136 | } 137 | 138 | /// This provides unsafe read/write access to internal data. RxSO(2) is 139 | /// represented by a complex number (two parameters). When using direct 140 | /// write access, the user needs to take care of that the complex number is 141 | /// not set close to zero. 142 | /// 143 | /// Note: The first parameter represents the real part, while the 144 | /// second parameter represent the imaginary part. 145 | /// 146 | SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); } 147 | 148 | /// Const version of data() above. 149 | /// 150 | SOPHUS_FUNC Scalar const* data() const { return complex().data(); } 151 | 152 | /// Returns group inverse. 153 | /// 154 | SOPHUS_FUNC RxSO2 inverse() const { 155 | Scalar squared_scale = complex().squaredNorm(); 156 | return RxSO2(complex().x() / squared_scale, 157 | -complex().y() / squared_scale); 158 | } 159 | 160 | /// Logarithmic map 161 | /// 162 | /// Computes the logarithm, the inverse of the group exponential which maps 163 | /// element of the group (scaled rotation matrices) to elements of the tangent 164 | /// space (rotation-vector plus logarithm of scale factor). 165 | /// 166 | /// To be specific, this function computes ``vee(logmat(.))`` with 167 | /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator 168 | /// of RxSO2. 169 | /// 170 | SOPHUS_FUNC Tangent log() const { 171 | using std::log; 172 | Tangent theta_sigma; 173 | theta_sigma[1] = log(scale()); 174 | theta_sigma[0] = SO2(complex()).log(); 175 | return theta_sigma; 176 | } 177 | 178 | /// Returns 2x2 matrix representation of the instance. 179 | /// 180 | /// For RxSO2, the matrix representation is an scaled orthogonal matrix ``sR`` 181 | /// with ``det(R)=s^2``, thus a scaled rotation matrix ``R`` with scale 182 | /// ``s``. 183 | /// 184 | SOPHUS_FUNC Transformation matrix() const { 185 | Transformation sR; 186 | // clang-format off 187 | sR << complex()[0], -complex()[1], 188 | complex()[1], complex()[0]; 189 | // clang-format on 190 | return sR; 191 | } 192 | 193 | /// Assignment operator. 194 | /// 195 | SOPHUS_FUNC RxSO2Base& operator=(RxSO2Base const& other) = default; 196 | 197 | /// Assignment-like operator from OtherDerived. 198 | /// 199 | template 200 | SOPHUS_FUNC RxSO2Base& operator=( 201 | RxSO2Base const& other) { 202 | complex_nonconst() = other.complex(); 203 | return *this; 204 | } 205 | 206 | /// Group multiplication, which is rotation concatenation and scale 207 | /// multiplication. 208 | /// 209 | /// Note: This function performs saturation for products close to zero in 210 | /// order to ensure the class invariant. 211 | /// 212 | template 213 | SOPHUS_FUNC RxSO2Product operator*( 214 | RxSO2Base const& other) const { 215 | using ResultT = ReturnScalar; 216 | 217 | Scalar lhs_real = complex().x(); 218 | Scalar lhs_imag = complex().y(); 219 | typename OtherDerived::Scalar const& rhs_real = other.complex().x(); 220 | typename OtherDerived::Scalar const& rhs_imag = other.complex().y(); 221 | /// complex multiplication 222 | typename RxSO2Product::ComplexType result_complex( 223 | lhs_real * rhs_real - lhs_imag * rhs_imag, 224 | lhs_real * rhs_imag + lhs_imag * rhs_real); 225 | 226 | const ResultT squared_scale = result_complex.squaredNorm(); 227 | 228 | if (squared_scale < 229 | Constants::epsilon() * Constants::epsilon()) { 230 | /// Saturation to ensure class invariant. 231 | result_complex.normalize(); 232 | result_complex *= Constants::epsilon(); 233 | } 234 | return RxSO2Product(result_complex); 235 | } 236 | 237 | /// Group action on 2-points. 238 | /// 239 | /// This function rotates a 2 dimensional point ``p`` by the SO2 element 240 | /// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor ``s``: 241 | /// 242 | /// ``p_bar = s * (bar_R_foo * p_foo)``. 243 | /// 244 | template ::value>::type> 247 | SOPHUS_FUNC PointProduct operator*( 248 | Eigen::MatrixBase const& p) const { 249 | return matrix() * p; 250 | } 251 | 252 | /// Group action on homogeneous 2-points. See above for more details. 253 | /// 254 | template ::value>::type> 257 | SOPHUS_FUNC HomogeneousPointProduct operator*( 258 | Eigen::MatrixBase const& p) const { 259 | const auto rsp = *this * p.template head<2>(); 260 | return HomogeneousPointProduct(rsp(0), rsp(1), p(2)); 261 | } 262 | 263 | /// Group action on lines. 264 | /// 265 | /// This function rotates a parameterized line ``l(t) = o + t * d`` by the SO2 266 | /// element and scales it by the scale factor 267 | /// 268 | /// Origin ``o`` is rotated and scaled 269 | /// Direction ``d`` is rotated (preserving it's norm) 270 | /// 271 | SOPHUS_FUNC Line operator*(Line const& l) const { 272 | return Line((*this) * l.origin(), (*this) * l.direction() / scale()); 273 | } 274 | 275 | /// In-place group multiplication. This method is only valid if the return 276 | /// type of the multiplication is compatible with this SO2's Scalar type. 277 | /// 278 | /// Note: This function performs saturation for products close to zero in 279 | /// order to ensure the class invariant. 280 | /// 281 | template >::value>::type> 284 | SOPHUS_FUNC RxSO2Base& operator*=( 285 | RxSO2Base const& other) { 286 | *static_cast(this) = *this * other; 287 | return *this; 288 | } 289 | 290 | /// Returns internal parameters of RxSO(2). 291 | /// 292 | /// It returns (c[0], c[1]), with c being the complex number. 293 | /// 294 | SOPHUS_FUNC Sophus::Vector params() const { 295 | return complex(); 296 | } 297 | 298 | /// Sets non-zero complex 299 | /// 300 | /// Precondition: ``z`` must not be close to zero. 301 | SOPHUS_FUNC void setComplex(Vector2 const& z) { 302 | SOPHUS_ENSURE(z.squaredNorm() > Constants::epsilon() * 303 | Constants::epsilon(), 304 | "Scale factor must be greater-equal epsilon."); 305 | static_cast(this)->complex_nonconst() = z; 306 | } 307 | 308 | /// Accessor of complex. 309 | /// 310 | SOPHUS_FUNC ComplexType const& complex() const { 311 | return static_cast(this)->complex(); 312 | } 313 | 314 | /// Returns rotation matrix. 315 | /// 316 | SOPHUS_FUNC Transformation rotationMatrix() const { 317 | ComplexTemporaryType norm_quad = complex(); 318 | norm_quad.normalize(); 319 | return SO2(norm_quad).matrix(); 320 | } 321 | 322 | /// Returns scale. 323 | /// 324 | SOPHUS_FUNC 325 | Scalar scale() const { return complex().norm(); } 326 | 327 | /// Setter of rotation angle, leaves scale as is. 328 | /// 329 | SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2(theta)); } 330 | 331 | /// Setter of complex using rotation matrix ``R``, leaves scale as is. 332 | /// 333 | /// Precondition: ``R`` must be orthogonal with determinant of one. 334 | /// 335 | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { 336 | setSO2(SO2(R)); 337 | } 338 | 339 | /// Sets scale and leaves rotation as is. 340 | /// 341 | SOPHUS_FUNC void setScale(Scalar const& scale) { 342 | using std::sqrt; 343 | complex_nonconst().normalize(); 344 | complex_nonconst() *= scale; 345 | } 346 | 347 | /// Setter of complex number using scaled rotation matrix ``sR``. 348 | /// 349 | /// Precondition: The 2x2 matrix must be "scaled orthogonal" 350 | /// and have a positive determinant. 351 | /// 352 | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { 353 | SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR), 354 | "sR must be scaled orthogonal:\n %", sR); 355 | complex_nonconst() = sR.col(0); 356 | } 357 | 358 | /// Setter of SO(2) rotations, leaves scale as is. 359 | /// 360 | SOPHUS_FUNC void setSO2(SO2 const& so2) { 361 | using std::sqrt; 362 | Scalar saved_scale = scale(); 363 | complex_nonconst() = so2.unit_complex(); 364 | complex_nonconst() *= saved_scale; 365 | } 366 | 367 | SOPHUS_FUNC SO2 so2() const { return SO2(complex()); } 368 | 369 | protected: 370 | /// Mutator of complex is private to ensure class invariant. 371 | /// 372 | SOPHUS_FUNC ComplexType& complex_nonconst() { 373 | return static_cast(this)->complex_nonconst(); 374 | } 375 | }; 376 | 377 | /// RxSO2 using storage; derived from RxSO2Base. 378 | template 379 | class RxSO2 : public RxSO2Base> { 380 | public: 381 | using Base = RxSO2Base>; 382 | using Scalar = Scalar_; 383 | using Transformation = typename Base::Transformation; 384 | using Point = typename Base::Point; 385 | using HomogeneousPoint = typename Base::HomogeneousPoint; 386 | using Tangent = typename Base::Tangent; 387 | using Adjoint = typename Base::Adjoint; 388 | using ComplexMember = Eigen::Matrix; 389 | 390 | /// ``Base`` is friend so complex_nonconst can be accessed from ``Base``. 391 | friend class RxSO2Base>; 392 | 393 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 394 | 395 | /// Default constructor initializes complex number to identity rotation and 396 | /// scale to 1. 397 | /// 398 | SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {} 399 | 400 | /// Copy constructor 401 | /// 402 | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default; 403 | 404 | /// Copy-like constructor from OtherDerived. 405 | /// 406 | template 407 | SOPHUS_FUNC RxSO2(RxSO2Base const& other) 408 | : complex_(other.complex()) {} 409 | 410 | /// Constructor from scaled rotation matrix 411 | /// 412 | /// Precondition: rotation matrix need to be scaled orthogonal with 413 | /// determinant of ``s^2``. 414 | /// 415 | SOPHUS_FUNC explicit RxSO2(Transformation const& sR) { 416 | this->setScaledRotationMatrix(sR); 417 | } 418 | 419 | /// Constructor from scale factor and rotation matrix ``R``. 420 | /// 421 | /// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant 422 | /// of 1 and ``scale`` must to be close to zero. 423 | /// 424 | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R) 425 | : RxSO2((scale * SO2(R).unit_complex()).eval()) {} 426 | 427 | /// Constructor from scale factor and SO2 428 | /// 429 | /// Precondition: ``scale`` must be close to zero. 430 | /// 431 | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2 const& so2) 432 | : RxSO2((scale * so2.unit_complex()).eval()) {} 433 | 434 | /// Constructor from complex number. 435 | /// 436 | /// Precondition: complex number must not be close to zero. 437 | /// 438 | SOPHUS_FUNC explicit RxSO2(Vector2 const& z) : complex_(z) { 439 | SOPHUS_ENSURE(complex_.squaredNorm() >= Constants::epsilon() * 440 | Constants::epsilon(), 441 | "Scale factor must be greater-equal epsilon: % vs %", 442 | complex_.squaredNorm(), 443 | Constants::epsilon() * Constants::epsilon()); 444 | } 445 | 446 | /// Constructor from complex number. 447 | /// 448 | /// Precondition: complex number must not be close to zero. 449 | /// 450 | SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag) 451 | : RxSO2(Vector2(real, imag)) {} 452 | 453 | /// Accessor of complex. 454 | /// 455 | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; } 456 | 457 | /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. 458 | /// 459 | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { 460 | return generator(i); 461 | } 462 | /// Group exponential 463 | /// 464 | /// This functions takes in an element of tangent space (= rotation angle 465 | /// plus logarithm of scale) and returns the corresponding element of the 466 | /// group RxSO2. 467 | /// 468 | /// To be more specific, this function computes ``expmat(hat(theta))`` 469 | /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the 470 | /// hat()-operator of RSO2. 471 | /// 472 | SOPHUS_FUNC static RxSO2 exp(Tangent const& a) { 473 | using std::exp; 474 | 475 | Scalar const theta = a[0]; 476 | Scalar const sigma = a[1]; 477 | Scalar s = exp(sigma); 478 | Vector2 z = SO2::exp(theta).unit_complex(); 479 | z *= s; 480 | return RxSO2(z); 481 | } 482 | 483 | /// Returns the ith infinitesimal generators of ``R+ x SO(2)``. 484 | /// 485 | /// The infinitesimal generators of RxSO2 are: 486 | /// 487 | /// ``` 488 | /// | 0 -1 | 489 | /// G_0 = | 1 0 | 490 | /// 491 | /// | 1 0 | 492 | /// G_1 = | 0 1 | 493 | /// ``` 494 | /// 495 | /// Precondition: ``i`` must be 0, or 1. 496 | /// 497 | SOPHUS_FUNC static Transformation generator(int i) { 498 | SOPHUS_ENSURE(i >= 0 && i <= 1, "i should be 0 or 1."); 499 | Tangent e; 500 | e.setZero(); 501 | e[i] = Scalar(1); 502 | return hat(e); 503 | } 504 | 505 | /// hat-operator 506 | /// 507 | /// It takes in the 2-vector representation ``a`` (= rotation angle plus 508 | /// logarithm of scale) and returns the corresponding matrix representation 509 | /// of Lie algebra element. 510 | /// 511 | /// Formally, the hat()-operator of RxSO2 is defined as 512 | /// 513 | /// ``hat(.): R^2 -> R^{2x2}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2) 514 | /// 515 | /// with ``G_i`` being the ith infinitesimal generator of RxSO2. 516 | /// 517 | /// The corresponding inverse is the vee()-operator, see below. 518 | /// 519 | SOPHUS_FUNC static Transformation hat(Tangent const& a) { 520 | Transformation A; 521 | // clang-format off 522 | A << a(1), -a(0), 523 | a(0), a(1); 524 | // clang-format on 525 | return A; 526 | } 527 | 528 | /// Lie bracket 529 | /// 530 | /// It computes the Lie bracket of RxSO(2). To be more specific, it computes 531 | /// 532 | /// ``[omega_1, omega_2]_rxso2 := vee([hat(omega_1), hat(omega_2)])`` 533 | /// 534 | /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the 535 | /// hat()-operator and ``vee(.)`` the vee()-operator of RxSO2. 536 | /// 537 | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { 538 | Vector2 res; 539 | res.setZero(); 540 | return res; 541 | } 542 | 543 | /// Draw uniform sample from RxSO(2) manifold. 544 | /// 545 | /// The scale factor is drawn uniformly in log2-space from [-1, 1], 546 | /// hence the scale is in [0.5, 2)]. 547 | /// 548 | template 549 | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) { 550 | std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); 551 | using std::exp2; 552 | return RxSO2(exp2(uniform(generator)), 553 | SO2::sampleUniform(generator)); 554 | } 555 | 556 | /// vee-operator 557 | /// 558 | /// It takes the 2x2-matrix representation ``Omega`` and maps it to the 559 | /// corresponding vector representation of Lie algebra. 560 | /// 561 | /// This is the inverse of the hat()-operator, see above. 562 | /// 563 | /// Precondition: ``Omega`` must have the following structure: 564 | /// 565 | /// | d -x | 566 | /// | x d | 567 | /// 568 | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { 569 | using std::abs; 570 | return Tangent(Omega(1, 0), Omega(0, 0)); 571 | } 572 | 573 | protected: 574 | SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; } 575 | 576 | ComplexMember complex_; 577 | }; 578 | 579 | } // namespace Sophus 580 | 581 | namespace Eigen { 582 | 583 | /// Specialization of Eigen::Map for ``RxSO2``; derived from RxSO2Base. 584 | /// 585 | /// Allows us to wrap RxSO2 objects around POD array (e.g. external z style 586 | /// complex). 587 | template 588 | class Map, Options> 589 | : public Sophus::RxSO2Base, Options>> { 590 | using Base = Sophus::RxSO2Base, Options>>; 591 | 592 | public: 593 | using Scalar = Scalar_; 594 | using Transformation = typename Base::Transformation; 595 | using Point = typename Base::Point; 596 | using HomogeneousPoint = typename Base::HomogeneousPoint; 597 | using Tangent = typename Base::Tangent; 598 | using Adjoint = typename Base::Adjoint; 599 | 600 | /// ``Base`` is friend so complex_nonconst can be accessed from ``Base``. 601 | friend class Sophus::RxSO2Base, Options>>; 602 | 603 | // LCOV_EXCL_START 604 | SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map); 605 | // LCOV_EXCL_STOP 606 | using Base::operator*=; 607 | using Base::operator*; 608 | 609 | SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {} 610 | 611 | /// Accessor of complex. 612 | /// 613 | SOPHUS_FUNC 614 | Map, Options> const& complex() const { 615 | return complex_; 616 | } 617 | 618 | protected: 619 | SOPHUS_FUNC Map, Options>& complex_nonconst() { 620 | return complex_; 621 | } 622 | 623 | Map, Options> complex_; 624 | }; 625 | 626 | /// Specialization of Eigen::Map for ``RxSO2 const``; derived from RxSO2Base. 627 | /// 628 | /// Allows us to wrap RxSO2 objects around POD array (e.g. external z style 629 | /// complex). 630 | template 631 | class Map const, Options> 632 | : public Sophus::RxSO2Base const, Options>> { 633 | public: 634 | using Base = Sophus::RxSO2Base const, Options>>; 635 | using Scalar = Scalar_; 636 | using Transformation = typename Base::Transformation; 637 | using Point = typename Base::Point; 638 | using HomogeneousPoint = typename Base::HomogeneousPoint; 639 | using Tangent = typename Base::Tangent; 640 | using Adjoint = typename Base::Adjoint; 641 | 642 | using Base::operator*=; 643 | using Base::operator*; 644 | 645 | SOPHUS_FUNC 646 | Map(Scalar const* coeffs) : complex_(coeffs) {} 647 | 648 | /// Accessor of complex. 649 | /// 650 | SOPHUS_FUNC 651 | Map const, Options> const& complex() const { 652 | return complex_; 653 | } 654 | 655 | protected: 656 | Map const, Options> const complex_; 657 | }; 658 | } // namespace Eigen 659 | 660 | #endif /// SOPHUS_RXSO2_HPP 661 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/sim_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_SIM_DETAILS_HPP 2 | #define SOPHUS_SIM_DETAILS_HPP 3 | 4 | #include "types.hpp" 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | Matrix calcW(Matrix const& Omega, 11 | Scalar const theta, Scalar const sigma) { 12 | using std::abs; 13 | using std::cos; 14 | using std::exp; 15 | using std::sin; 16 | static Matrix const I = Matrix::Identity(); 17 | static Scalar const one(1); 18 | static Scalar const half(0.5); 19 | Matrix const Omega2 = Omega * Omega; 20 | Scalar const scale = exp(sigma); 21 | Scalar A, B, C; 22 | if (abs(sigma) < Constants::epsilon()) { 23 | C = one; 24 | if (abs(theta) < Constants::epsilon()) { 25 | A = half; 26 | B = Scalar(1. / 6.); 27 | } else { 28 | Scalar theta_sq = theta * theta; 29 | A = (one - cos(theta)) / theta_sq; 30 | B = (theta - sin(theta)) / (theta_sq * theta); 31 | } 32 | } else { 33 | C = (scale - one) / sigma; 34 | if (abs(theta) < Constants::epsilon()) { 35 | Scalar sigma_sq = sigma * sigma; 36 | A = ((sigma - one) * scale + one) / sigma_sq; 37 | B = (scale * half * sigma_sq + scale - one - sigma * scale) / 38 | (sigma_sq * sigma); 39 | } else { 40 | Scalar theta_sq = theta * theta; 41 | Scalar a = scale * sin(theta); 42 | Scalar b = scale * cos(theta); 43 | Scalar c = theta_sq + sigma * sigma; 44 | A = (a * sigma + (one - b) * theta) / (theta * c); 45 | B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); 46 | } 47 | } 48 | return A * Omega + B * Omega2 + C * I; 49 | } 50 | 51 | template 52 | Matrix calcWInv(Matrix const& Omega, 53 | Scalar const theta, Scalar const sigma, 54 | Scalar const scale) { 55 | using std::abs; 56 | using std::cos; 57 | using std::sin; 58 | static Matrix const I = Matrix::Identity(); 59 | static Scalar const half(0.5); 60 | static Scalar const one(1); 61 | static Scalar const two(2); 62 | Matrix const Omega2 = Omega * Omega; 63 | Scalar const scale_sq = scale * scale; 64 | Scalar const theta_sq = theta * theta; 65 | Scalar const sin_theta = sin(theta); 66 | Scalar const cos_theta = cos(theta); 67 | 68 | Scalar a, b, c; 69 | if (abs(sigma * sigma) < Constants::epsilon()) { 70 | c = one - half * sigma; 71 | a = -half; 72 | if (abs(theta_sq) < Constants::epsilon()) { 73 | b = Scalar(1. / 12.); 74 | } else { 75 | b = (theta * sin_theta + two * cos_theta - two) / 76 | (two * theta_sq * (cos_theta - one)); 77 | } 78 | } else { 79 | Scalar const scale_cu = scale_sq * scale; 80 | c = sigma / (scale - one); 81 | if (abs(theta_sq) < Constants::epsilon()) { 82 | a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); 83 | b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / 84 | (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); 85 | } else { 86 | Scalar const s_sin_theta = scale * sin_theta; 87 | Scalar const s_cos_theta = scale * cos_theta; 88 | a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / 89 | (theta * (scale_sq - two * s_cos_theta + one)); 90 | b = -scale * 91 | (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - 92 | scale * sigma + sigma * cos_theta - sigma) / 93 | (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + 94 | two * s_cos_theta + scale - one)); 95 | } 96 | } 97 | return a * Omega + b * Omega2 + c * I; 98 | } 99 | 100 | } // namespace details 101 | } // namespace Sophus 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/so2.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Special orthogonal group SO(2) - rotation in 2d. 3 | 4 | #ifndef SOPHUS_SO2_HPP 5 | #define SOPHUS_SO2_HPP 6 | 7 | #include 8 | #include 9 | 10 | // Include only the selective set of Eigen headers that we need. 11 | // This helps when using Sophus with unusual compilers, like nvcc. 12 | #include 13 | 14 | #include "rotation_matrix.hpp" 15 | #include "types.hpp" 16 | 17 | namespace Sophus { 18 | template 19 | class SO2; 20 | using SO2d = SO2; 21 | using SO2f = SO2; 22 | } // namespace Sophus 23 | 24 | namespace Eigen { 25 | namespace internal { 26 | 27 | template 28 | struct traits> { 29 | static constexpr int Options = Options_; 30 | using Scalar = Scalar_; 31 | using ComplexType = Sophus::Vector2; 32 | }; 33 | 34 | template 35 | struct traits, Options_>> 36 | : traits> { 37 | static constexpr int Options = Options_; 38 | using Scalar = Scalar_; 39 | using ComplexType = Map, Options>; 40 | }; 41 | 42 | template 43 | struct traits const, Options_>> 44 | : traits const> { 45 | static constexpr int Options = Options_; 46 | using Scalar = Scalar_; 47 | using ComplexType = Map const, Options>; 48 | }; 49 | } // namespace internal 50 | } // namespace Eigen 51 | 52 | namespace Sophus { 53 | 54 | /// SO2 base type - implements SO2 class but is storage agnostic. 55 | /// 56 | /// SO(2) is the group of rotations in 2d. As a matrix group, it is the set of 57 | /// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being 58 | /// the transpose of ``R``) and have a positive determinant. In particular, the 59 | /// determinant is 1. Let ``theta`` be the rotation angle, the rotation matrix 60 | /// can be written in close form: 61 | /// 62 | /// | cos(theta) -sin(theta) | 63 | /// | sin(theta) cos(theta) | 64 | /// 65 | /// As a matter of fact, the first column of those matrices is isomorph to the 66 | /// set of unit complex numbers U(1). Thus, internally, SO2 is represented as 67 | /// complex number with length 1. 68 | /// 69 | /// SO(2) is a compact and commutative group. First it is compact since the set 70 | /// of rotation matrices is a closed and bounded set. Second it is commutative 71 | /// since ``R(alpha) * R(beta) = R(beta) * R(alpha)``, simply because ``alpha + 72 | /// beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles 73 | /// (about the same axis). 74 | /// 75 | /// Class invariant: The 2-norm of ``unit_complex`` must be close to 1. 76 | /// Technically speaking, it must hold that: 77 | /// 78 | /// ``|unit_complex().squaredNorm() - 1| <= Constants::epsilon()``. 79 | template 80 | class SO2Base { 81 | public: 82 | static constexpr int Options = Eigen::internal::traits::Options; 83 | using Scalar = typename Eigen::internal::traits::Scalar; 84 | using ComplexT = typename Eigen::internal::traits::ComplexType; 85 | using ComplexTemporaryType = Sophus::Vector2; 86 | 87 | /// Degrees of freedom of manifold, number of dimensions in tangent space (one 88 | /// since we only have in-plane rotations). 89 | static int constexpr DoF = 1; 90 | /// Number of internal parameters used (complex numbers are a tuples). 91 | static int constexpr num_parameters = 2; 92 | /// Group transformations are 2x2 matrices. 93 | static int constexpr N = 2; 94 | using Transformation = Matrix; 95 | using Point = Vector2; 96 | using HomogeneousPoint = Vector3; 97 | using Line = ParametrizedLine2; 98 | using Tangent = Scalar; 99 | using Adjoint = Scalar; 100 | 101 | /// For binary operations the return type is determined with the 102 | /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map 103 | /// types, as well as other compatible scalar types such as Ceres::Jet and 104 | /// double scalars with SO2 operations. 105 | template 106 | using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< 107 | Scalar, typename OtherDerived::Scalar>::ReturnType; 108 | 109 | template 110 | using SO2Product = SO2>; 111 | 112 | template 113 | using PointProduct = Vector2>; 114 | 115 | template 116 | using HomogeneousPointProduct = Vector3>; 117 | 118 | /// Adjoint transformation 119 | /// 120 | /// This function return the adjoint transformation ``Ad`` of the group 121 | /// element ``A`` such that for all ``x`` it holds that 122 | /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. 123 | /// 124 | /// It simply ``1``, since ``SO(2)`` is a commutative group. 125 | /// 126 | SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); } 127 | 128 | /// Returns copy of instance casted to NewScalarType. 129 | /// 130 | template 131 | SOPHUS_FUNC SO2 cast() const { 132 | return SO2(unit_complex().template cast()); 133 | } 134 | 135 | /// This provides unsafe read/write access to internal data. SO(2) is 136 | /// represented by a unit complex number (two parameters). When using direct 137 | /// write access, the user needs to take care of that the complex number stays 138 | /// normalized. 139 | /// 140 | SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); } 141 | 142 | /// Const version of data() above. 143 | /// 144 | SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); } 145 | 146 | /// Returns group inverse. 147 | /// 148 | SOPHUS_FUNC SO2 inverse() const { 149 | return SO2(unit_complex().x(), -unit_complex().y()); 150 | } 151 | 152 | /// Logarithmic map 153 | /// 154 | /// Computes the logarithm, the inverse of the group exponential which maps 155 | /// element of the group (rotation matrices) to elements of the tangent space 156 | /// (rotation angles). 157 | /// 158 | /// To be specific, this function computes ``vee(logmat(.))`` with 159 | /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator 160 | /// of SO(2). 161 | /// 162 | SOPHUS_FUNC Scalar log() const { 163 | using std::atan2; 164 | return atan2(unit_complex().y(), unit_complex().x()); 165 | } 166 | 167 | /// It re-normalizes ``unit_complex`` to unit length. 168 | /// 169 | /// Note: Because of the class invariant, there is typically no need to call 170 | /// this function directly. 171 | /// 172 | SOPHUS_FUNC void normalize() { 173 | using std::sqrt; 174 | Scalar length = sqrt(unit_complex().x() * unit_complex().x() + 175 | unit_complex().y() * unit_complex().y()); 176 | SOPHUS_ENSURE(length >= Constants::epsilon(), 177 | "Complex number should not be close to zero!"); 178 | unit_complex_nonconst().x() /= length; 179 | unit_complex_nonconst().y() /= length; 180 | } 181 | 182 | /// Returns 2x2 matrix representation of the instance. 183 | /// 184 | /// For SO(2), the matrix representation is an orthogonal matrix ``R`` with 185 | /// ``det(R)=1``, thus the so-called "rotation matrix". 186 | /// 187 | SOPHUS_FUNC Transformation matrix() const { 188 | Scalar const& real = unit_complex().x(); 189 | Scalar const& imag = unit_complex().y(); 190 | Transformation R; 191 | // clang-format off 192 | R << 193 | real, -imag, 194 | imag, real; 195 | // clang-format on 196 | return R; 197 | } 198 | 199 | /// Assignment operator 200 | /// 201 | SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) = default; 202 | 203 | /// Assignment-like operator from OtherDerived. 204 | /// 205 | template 206 | SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) { 207 | unit_complex_nonconst() = other.unit_complex(); 208 | return *this; 209 | } 210 | 211 | /// Group multiplication, which is rotation concatenation. 212 | /// 213 | template 214 | SOPHUS_FUNC SO2Product operator*( 215 | SO2Base const& other) const { 216 | using ResultT = ReturnScalar; 217 | Scalar const lhs_real = unit_complex().x(); 218 | Scalar const lhs_imag = unit_complex().y(); 219 | typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x(); 220 | typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y(); 221 | // complex multiplication 222 | ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag; 223 | ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real; 224 | 225 | ResultT const squared_norm = 226 | result_real * result_real + result_imag * result_imag; 227 | // We can assume that the squared-norm is close to 1 since we deal with a 228 | // unit complex number. Due to numerical precision issues, there might 229 | // be a small drift after pose concatenation. Hence, we need to renormalizes 230 | // the complex number here. 231 | // Since squared-norm is close to 1, we do not need to calculate the costly 232 | // square-root, but can use an approximation around 1 (see 233 | // http://stackoverflow.com/a/12934750 for details). 234 | if (squared_norm != ResultT(1.0)) { 235 | ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm); 236 | return SO2Product(result_real * scale, result_imag * scale); 237 | } 238 | return SO2Product(result_real, result_imag); 239 | } 240 | 241 | /// Group action on 2-points. 242 | /// 243 | /// This function rotates a 2 dimensional point ``p`` by the SO2 element 244 | /// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. 245 | /// 246 | template ::value>::type> 249 | SOPHUS_FUNC PointProduct operator*( 250 | Eigen::MatrixBase const& p) const { 251 | Scalar const& real = unit_complex().x(); 252 | Scalar const& imag = unit_complex().y(); 253 | return PointProduct(real * p[0] - imag * p[1], 254 | imag * p[0] + real * p[1]); 255 | } 256 | 257 | /// Group action on homogeneous 2-points. 258 | /// 259 | /// This function rotates a homogeneous 2 dimensional point ``p`` by the SO2 260 | /// element ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. 261 | /// 262 | template ::value>::type> 265 | SOPHUS_FUNC HomogeneousPointProduct operator*( 266 | Eigen::MatrixBase const& p) const { 267 | Scalar const& real = unit_complex().x(); 268 | Scalar const& imag = unit_complex().y(); 269 | return HomogeneousPointProduct( 270 | real * p[0] - imag * p[1], imag * p[0] + real * p[1], p[2]); 271 | } 272 | 273 | /// Group action on lines. 274 | /// 275 | /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO2 276 | /// element: 277 | /// 278 | /// Both direction ``d`` and origin ``o`` are rotated as a 2 dimensional point 279 | /// 280 | SOPHUS_FUNC Line operator*(Line const& l) const { 281 | return Line((*this) * l.origin(), (*this) * l.direction()); 282 | } 283 | 284 | /// In-place group multiplication. This method is only valid if the return 285 | /// type of the multiplication is compatible with this SO2's Scalar type. 286 | /// 287 | template >::value>::type> 290 | SOPHUS_FUNC SO2Base operator*=(SO2Base const& other) { 291 | *static_cast(this) = *this * other; 292 | return *this; 293 | } 294 | 295 | /// Returns derivative of this * SO2::exp(x) wrt. x at x=0. 296 | /// 297 | SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() 298 | const { 299 | return Matrix(-unit_complex()[1], 300 | unit_complex()[0]); 301 | } 302 | 303 | /// Returns internal parameters of SO(2). 304 | /// 305 | /// It returns (c[0], c[1]), with c being the unit complex number. 306 | /// 307 | SOPHUS_FUNC Sophus::Vector params() const { 308 | return unit_complex(); 309 | } 310 | 311 | /// Takes in complex number / tuple and normalizes it. 312 | /// 313 | /// Precondition: The complex number must not be close to zero. 314 | /// 315 | SOPHUS_FUNC void setComplex(Point const& complex) { 316 | unit_complex_nonconst() = complex; 317 | normalize(); 318 | } 319 | 320 | /// Accessor of unit quaternion. 321 | /// 322 | SOPHUS_FUNC 323 | ComplexT const& unit_complex() const { 324 | return static_cast(this)->unit_complex(); 325 | } 326 | 327 | private: 328 | /// Mutator of unit_complex is private to ensure class invariant. That is 329 | /// the complex number must stay close to unit length. 330 | /// 331 | SOPHUS_FUNC 332 | ComplexT& unit_complex_nonconst() { 333 | return static_cast(this)->unit_complex_nonconst(); 334 | } 335 | }; 336 | 337 | /// SO2 using default storage; derived from SO2Base. 338 | template 339 | class SO2 : public SO2Base> { 340 | public: 341 | using Base = SO2Base>; 342 | static int constexpr DoF = Base::DoF; 343 | static int constexpr num_parameters = Base::num_parameters; 344 | 345 | using Scalar = Scalar_; 346 | using Transformation = typename Base::Transformation; 347 | using Point = typename Base::Point; 348 | using HomogeneousPoint = typename Base::HomogeneousPoint; 349 | using Tangent = typename Base::Tangent; 350 | using Adjoint = typename Base::Adjoint; 351 | using ComplexMember = Vector2; 352 | 353 | /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``. 354 | friend class SO2Base>; 355 | 356 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 357 | 358 | /// Default constructor initializes unit complex number to identity rotation. 359 | /// 360 | SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {} 361 | 362 | /// Copy constructor 363 | /// 364 | SOPHUS_FUNC SO2(SO2 const& other) = default; 365 | 366 | /// Copy-like constructor from OtherDerived. 367 | /// 368 | template 369 | SOPHUS_FUNC SO2(SO2Base const& other) 370 | : unit_complex_(other.unit_complex()) {} 371 | 372 | /// Constructor from rotation matrix 373 | /// 374 | /// Precondition: rotation matrix need to be orthogonal with determinant of 1. 375 | /// 376 | SOPHUS_FUNC explicit SO2(Transformation const& R) 377 | : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)), 378 | Scalar(0.5) * (R(1, 0) - R(0, 1))) { 379 | SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); 380 | SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", 381 | R.determinant()); 382 | } 383 | 384 | /// Constructor from pair of real and imaginary number. 385 | /// 386 | /// Precondition: The pair must not be close to zero. 387 | /// 388 | SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag) 389 | : unit_complex_(real, imag) { 390 | Base::normalize(); 391 | } 392 | 393 | /// Constructor from 2-vector. 394 | /// 395 | /// Precondition: The vector must not be close to zero. 396 | /// 397 | template 398 | SOPHUS_FUNC explicit SO2(Eigen::MatrixBase const& complex) 399 | : unit_complex_(complex) { 400 | static_assert(std::is_same::value, 401 | "must be same Scalar type"); 402 | Base::normalize(); 403 | } 404 | 405 | /// Constructor from an rotation angle. 406 | /// 407 | SOPHUS_FUNC explicit SO2(Scalar theta) { 408 | unit_complex_nonconst() = SO2::exp(theta).unit_complex(); 409 | } 410 | 411 | /// Accessor of unit complex number 412 | /// 413 | SOPHUS_FUNC ComplexMember const& unit_complex() const { 414 | return unit_complex_; 415 | } 416 | 417 | /// Group exponential 418 | /// 419 | /// This functions takes in an element of tangent space (= rotation angle 420 | /// ``theta``) and returns the corresponding element of the group SO(2). 421 | /// 422 | /// To be more specific, this function computes ``expmat(hat(omega))`` 423 | /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the 424 | /// hat()-operator of SO(2). 425 | /// 426 | SOPHUS_FUNC static SO2 exp(Tangent const& theta) { 427 | using std::cos; 428 | using std::sin; 429 | return SO2(cos(theta), sin(theta)); 430 | } 431 | 432 | /// Returns derivative of exp(x) wrt. x. 433 | /// 434 | SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( 435 | Tangent const& theta) { 436 | using std::cos; 437 | using std::sin; 438 | return Sophus::Matrix(-sin(theta), cos(theta)); 439 | } 440 | 441 | /// Returns derivative of exp(x) wrt. x_i at x=0. 442 | /// 443 | SOPHUS_FUNC static Sophus::Matrix 444 | Dx_exp_x_at_0() { 445 | return Sophus::Matrix(Scalar(0), Scalar(1)); 446 | } 447 | 448 | /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. 449 | /// 450 | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) { 451 | return generator(); 452 | } 453 | 454 | /// Returns the infinitesimal generators of SO(2). 455 | /// 456 | /// The infinitesimal generators of SO(2) is: 457 | /// 458 | /// | 0 1 | 459 | /// | -1 0 | 460 | /// 461 | SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); } 462 | 463 | /// hat-operator 464 | /// 465 | /// It takes in the scalar representation ``theta`` (= rotation angle) and 466 | /// returns the corresponding matrix representation of Lie algebra element. 467 | /// 468 | /// Formally, the hat()-operator of SO(2) is defined as 469 | /// 470 | /// ``hat(.): R^2 -> R^{2x2}, hat(theta) = theta * G`` 471 | /// 472 | /// with ``G`` being the infinitesimal generator of SO(2). 473 | /// 474 | /// The corresponding inverse is the vee()-operator, see below. 475 | /// 476 | SOPHUS_FUNC static Transformation hat(Tangent const& theta) { 477 | Transformation Omega; 478 | // clang-format off 479 | Omega << 480 | Scalar(0), -theta, 481 | theta, Scalar(0); 482 | // clang-format on 483 | return Omega; 484 | } 485 | 486 | /// Returns closed SO2 given arbitrary 2x2 matrix. 487 | /// 488 | template 489 | static SOPHUS_FUNC enable_if_t::value, SO2> 490 | fitToSO2(Transformation const& R) { 491 | return SO2(makeRotationMatrix(R)); 492 | } 493 | 494 | /// Lie bracket 495 | /// 496 | /// It returns the Lie bracket of SO(2). Since SO(2) is a commutative group, 497 | /// the Lie bracket is simple ``0``. 498 | /// 499 | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { 500 | return Scalar(0); 501 | } 502 | 503 | /// Draw uniform sample from SO(2) manifold. 504 | /// 505 | template 506 | static SO2 sampleUniform(UniformRandomBitGenerator& generator) { 507 | static_assert(IsUniformRandomBitGenerator::value, 508 | "generator must meet the UniformRandomBitGenerator concept"); 509 | std::uniform_real_distribution uniform(-Constants::pi(), 510 | Constants::pi()); 511 | return SO2(uniform(generator)); 512 | } 513 | 514 | /// vee-operator 515 | /// 516 | /// It takes the 2x2-matrix representation ``Omega`` and maps it to the 517 | /// corresponding scalar representation of Lie algebra. 518 | /// 519 | /// This is the inverse of the hat()-operator, see above. 520 | /// 521 | /// Precondition: ``Omega`` must have the following structure: 522 | /// 523 | /// | 0 -a | 524 | /// | a 0 | 525 | /// 526 | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { 527 | using std::abs; 528 | return Omega(1, 0); 529 | } 530 | 531 | protected: 532 | /// Mutator of complex number is protected to ensure class invariant. 533 | /// 534 | SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; } 535 | 536 | ComplexMember unit_complex_; 537 | }; 538 | 539 | } // namespace Sophus 540 | 541 | namespace Eigen { 542 | 543 | /// Specialization of Eigen::Map for ``SO2``; derived from SO2Base. 544 | /// 545 | /// Allows us to wrap SO2 objects around POD array (e.g. external c style 546 | /// complex number / tuple). 547 | template 548 | class Map, Options> 549 | : public Sophus::SO2Base, Options>> { 550 | public: 551 | using Base = Sophus::SO2Base, Options>>; 552 | using Scalar = Scalar_; 553 | 554 | using Transformation = typename Base::Transformation; 555 | using Point = typename Base::Point; 556 | using HomogeneousPoint = typename Base::HomogeneousPoint; 557 | using Tangent = typename Base::Tangent; 558 | using Adjoint = typename Base::Adjoint; 559 | 560 | /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``. 561 | friend class Sophus::SO2Base, Options>>; 562 | 563 | // LCOV_EXCL_START 564 | SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map); 565 | // LCOV_EXCL_STOP 566 | 567 | using Base::operator*=; 568 | using Base::operator*; 569 | 570 | SOPHUS_FUNC 571 | Map(Scalar* coeffs) : unit_complex_(coeffs) {} 572 | 573 | /// Accessor of unit complex number. 574 | /// 575 | SOPHUS_FUNC 576 | Map, Options> const& unit_complex() const { 577 | return unit_complex_; 578 | } 579 | 580 | protected: 581 | /// Mutator of unit_complex is protected to ensure class invariant. 582 | /// 583 | SOPHUS_FUNC 584 | Map, Options>& unit_complex_nonconst() { 585 | return unit_complex_; 586 | } 587 | 588 | Map, Options> unit_complex_; 589 | }; 590 | 591 | /// Specialization of Eigen::Map for ``SO2 const``; derived from SO2Base. 592 | /// 593 | /// Allows us to wrap SO2 objects around POD array (e.g. external c style 594 | /// complex number / tuple). 595 | template 596 | class Map const, Options> 597 | : public Sophus::SO2Base const, Options>> { 598 | public: 599 | using Base = Sophus::SO2Base const, Options>>; 600 | using Scalar = Scalar_; 601 | using Transformation = typename Base::Transformation; 602 | using Point = typename Base::Point; 603 | using HomogeneousPoint = typename Base::HomogeneousPoint; 604 | using Tangent = typename Base::Tangent; 605 | using Adjoint = typename Base::Adjoint; 606 | 607 | using Base::operator*=; 608 | using Base::operator*; 609 | 610 | SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {} 611 | 612 | /// Accessor of unit complex number. 613 | /// 614 | SOPHUS_FUNC Map const, Options> const& unit_complex() 615 | const { 616 | return unit_complex_; 617 | } 618 | 619 | protected: 620 | /// Mutator of unit_complex is protected to ensure class invariant. 621 | /// 622 | Map const, Options> const unit_complex_; 623 | }; 624 | } // namespace Eigen 625 | 626 | #endif // SOPHUS_SO2_HPP 627 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/test_macros.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPUHS_TESTS_MACROS_HPP 2 | #define SOPUHS_TESTS_MACROS_HPP 3 | 4 | #include 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | class Metric { 11 | public: 12 | static Scalar impl(Scalar s0, Scalar s1) { 13 | using std::abs; 14 | return abs(s0 - s1); 15 | } 16 | }; 17 | 18 | template 19 | class Metric> { 20 | public: 21 | static Scalar impl(const Eigen::Matrix& v0, 22 | const Eigen::Matrix& v1) { 23 | return (v0 - v1).norm(); 24 | } 25 | }; 26 | 27 | template 28 | auto metric(const T& v0, const T& v1) -> decltype(Metric::impl(v0, v1)) { 29 | return Metric::impl(v0, v1); 30 | } 31 | 32 | template 33 | class Pretty { 34 | public: 35 | static std::string impl(Scalar s) { return FormatString("%", s); } 36 | }; 37 | 38 | template 39 | class Pretty> { 40 | public: 41 | static std::string impl(const Eigen::Matrix& v) { 42 | return FormatString("\n%\n", v); 43 | } 44 | }; 45 | 46 | template 47 | std::string pretty(const T& v) { 48 | return Pretty::impl(v); 49 | } 50 | 51 | template 52 | void testFailed(bool& passed, const char* func, const char* file, int line, 53 | const std::string& msg) { 54 | std::cerr << FormatString("Test failed in function %, file %, line %\n", func, 55 | file, line); 56 | std::cerr << msg << "\n\n"; 57 | passed = false; 58 | } 59 | } // namespace details 60 | 61 | void processTestResult(bool passed) { 62 | if (!passed) { 63 | std::cerr << "failed!" << std::endl << std::endl; 64 | exit(-1); 65 | } 66 | std::cerr << "passed." << std::endl << std::endl; 67 | } 68 | } // namespace Sophus 69 | 70 | #define SOPHUS_STRINGIFY(x) #x 71 | 72 | // GenericTests whether left is equal to right given a threshold. 73 | // The in-out parameter passed will be set to false if test fails. 74 | #define SOPHUS_TEST_EQUAL(passed, left, right, ...) \ 75 | do { \ 76 | if (left != right) { \ 77 | std::string msg = Sophus::details::FormatString( \ 78 | "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ 79 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 80 | Sophus::details::pretty(right)); \ 81 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 82 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 83 | msg); \ 84 | } \ 85 | } while (false) 86 | 87 | // GenericTests whether left is equal to right given a threshold. 88 | // The in-out parameter passed will be set to false if test fails. 89 | #define SOPHUS_TEST_NEQ(passed, left, right, ...) \ 90 | do { \ 91 | if (left == right) { \ 92 | std::string msg = Sophus::details::FormatString( \ 93 | "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ 94 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 95 | Sophus::details::pretty(right)); \ 96 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 97 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 98 | msg); \ 99 | } \ 100 | } while (false) 101 | 102 | // GenericTests whether left is approximatly equal to right given a threshold. 103 | // The in-out parameter passed will be set to false if test fails. 104 | #define SOPHUS_TEST_APPROX(passed, left, right, thr, ...) \ 105 | do { \ 106 | auto nrm = Sophus::details::metric((left), (right)); \ 107 | if (!(nrm < (thr))) { \ 108 | std::string msg = Sophus::details::FormatString( \ 109 | "% (=%) is not approx % (=%); % is % \n", SOPHUS_STRINGIFY(left), \ 110 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 111 | Sophus::details::pretty(right), SOPHUS_STRINGIFY(thr), \ 112 | Sophus::details::pretty(thr)); \ 113 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 114 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 115 | msg); \ 116 | } \ 117 | } while (false) 118 | 119 | #endif // SOPUHS_TESTS_MACROS_HPP 120 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/types.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Common type aliases. 3 | 4 | #ifndef SOPHUS_TYPES_HPP 5 | #define SOPHUS_TYPES_HPP 6 | 7 | #include 8 | #include "common.hpp" 9 | 10 | namespace Sophus { 11 | 12 | template 13 | using Vector = Eigen::Matrix; 14 | 15 | template 16 | using Vector2 = Vector; 17 | using Vector2f = Vector2; 18 | using Vector2d = Vector2; 19 | 20 | template 21 | using Vector3 = Vector; 22 | using Vector3f = Vector3; 23 | using Vector3d = Vector3; 24 | 25 | template 26 | using Vector4 = Vector; 27 | using Vector4f = Vector4; 28 | using Vector4d = Vector4; 29 | 30 | template 31 | using Vector6 = Vector; 32 | using Vector6f = Vector6; 33 | using Vector6d = Vector6; 34 | 35 | template 36 | using Vector7 = Vector; 37 | using Vector7f = Vector7; 38 | using Vector7d = Vector7; 39 | 40 | template 41 | using Matrix = Eigen::Matrix; 42 | 43 | template 44 | using Matrix2 = Matrix; 45 | using Matrix2f = Matrix2; 46 | using Matrix2d = Matrix2; 47 | 48 | template 49 | using Matrix3 = Matrix; 50 | using Matrix3f = Matrix3; 51 | using Matrix3d = Matrix3; 52 | 53 | template 54 | using Matrix4 = Matrix; 55 | using Matrix4f = Matrix4; 56 | using Matrix4d = Matrix4; 57 | 58 | template 59 | using Matrix6 = Matrix; 60 | using Matrix6f = Matrix6; 61 | using Matrix6d = Matrix6; 62 | 63 | template 64 | using Matrix7 = Matrix; 65 | using Matrix7f = Matrix7; 66 | using Matrix7d = Matrix7; 67 | 68 | template 69 | using ParametrizedLine = Eigen::ParametrizedLine; 70 | 71 | template 72 | using ParametrizedLine3 = ParametrizedLine; 73 | using ParametrizedLine3f = ParametrizedLine3; 74 | using ParametrizedLine3d = ParametrizedLine3; 75 | 76 | template 77 | using ParametrizedLine2 = ParametrizedLine; 78 | using ParametrizedLine2f = ParametrizedLine2; 79 | using ParametrizedLine2d = ParametrizedLine2; 80 | 81 | namespace details { 82 | template 83 | class MaxMetric { 84 | public: 85 | static Scalar impl(Scalar s0, Scalar s1) { 86 | using std::abs; 87 | return abs(s0 - s1); 88 | } 89 | }; 90 | 91 | template 92 | class MaxMetric> { 93 | public: 94 | static Scalar impl(Matrix const& p0, 95 | Matrix const& p1) { 96 | return (p0 - p1).template lpNorm(); 97 | } 98 | }; 99 | 100 | template 101 | class SetToZero { 102 | public: 103 | static void impl(Scalar& s) { s = Scalar(0); } 104 | }; 105 | 106 | template 107 | class SetToZero> { 108 | public: 109 | static void impl(Matrix& v) { v.setZero(); } 110 | }; 111 | 112 | template 113 | class SetElementAt; 114 | 115 | template 116 | class SetElementAt { 117 | public: 118 | static void impl(Scalar& s, Scalar value, int at) { 119 | SOPHUS_ENSURE(at == 0, "is %", at); 120 | s = value; 121 | } 122 | }; 123 | 124 | template 125 | class SetElementAt, Scalar> { 126 | public: 127 | static void impl(Vector& v, Scalar value, int at) { 128 | SOPHUS_ENSURE(at >= 0 && at < N, "is %", at); 129 | v[at] = value; 130 | } 131 | }; 132 | 133 | template 134 | class SquaredNorm { 135 | public: 136 | static Scalar impl(Scalar const& s) { return s * s; } 137 | }; 138 | 139 | template 140 | class SquaredNorm> { 141 | public: 142 | static Scalar impl(Matrix const& s) { return s.squaredNorm(); } 143 | }; 144 | 145 | template 146 | class Transpose { 147 | public: 148 | static Scalar impl(Scalar const& s) { return s; } 149 | }; 150 | 151 | template 152 | class Transpose> { 153 | public: 154 | static Matrix impl(Matrix const& s) { 155 | return s.transpose(); 156 | } 157 | }; 158 | } // namespace details 159 | 160 | /// Returns maximum metric between two points ``p0`` and ``p1``, with ``p0, p1`` 161 | /// being matrices or a scalars. 162 | /// 163 | template 164 | auto maxMetric(T const& p0, T const& p1) 165 | -> decltype(details::MaxMetric::impl(p0, p1)) { 166 | return details::MaxMetric::impl(p0, p1); 167 | } 168 | 169 | /// Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 170 | /// 171 | template 172 | void setToZero(T& p) { 173 | return details::SetToZero::impl(p); 174 | } 175 | 176 | /// Sets ``i``th component of ``p`` to ``value``, with ``p`` being a 177 | /// matrix or a scalar. If ``p`` is a scalar, ``i`` must be ``0``. 178 | /// 179 | template 180 | void setElementAt(T& p, Scalar value, int i) { 181 | return details::SetElementAt::impl(p, value, i); 182 | } 183 | 184 | /// Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar. 185 | /// 186 | template 187 | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm::impl(p)) { 188 | return details::SquaredNorm::impl(p); 189 | } 190 | 191 | /// Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a 192 | /// scalar. 193 | /// 194 | template 195 | auto transpose(T const& p) -> decltype(details::Transpose::impl(T())) { 196 | return details::Transpose::impl(p); 197 | } 198 | 199 | template 200 | struct IsFloatingPoint { 201 | static bool const value = std::is_floating_point::value; 202 | }; 203 | 204 | template 205 | struct IsFloatingPoint> { 206 | static bool const value = std::is_floating_point::value; 207 | }; 208 | 209 | template 210 | struct GetScalar { 211 | using Scalar = Scalar_; 212 | }; 213 | 214 | template 215 | struct GetScalar> { 216 | using Scalar = Scalar_; 217 | }; 218 | 219 | /// If the Vector type is of fixed size, then IsFixedSizeVector::value will be 220 | /// true. 221 | template ::type> 225 | struct IsFixedSizeVector : std::true_type {}; 226 | 227 | /// Planes in 3d are hyperplanes. 228 | template 229 | using Plane3 = Eigen::Hyperplane; 230 | using Plane3d = Plane3; 231 | using Plane3f = Plane3; 232 | 233 | /// Lines in 2d are hyperplanes. 234 | template 235 | using Line2 = Eigen::Hyperplane; 236 | using Line2d = Line2; 237 | using Line2f = Line2; 238 | 239 | } // namespace Sophus 240 | 241 | #endif // SOPHUS_TYPES_HPP 242 | -------------------------------------------------------------------------------- /thirdparty/sophus/sophus/velocities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_VELOCITIES_HPP 2 | #define SOPHUS_VELOCITIES_HPP 3 | 4 | #include 5 | 6 | #include "num_diff.hpp" 7 | #include "se3.hpp" 8 | 9 | namespace Sophus { 10 | namespace experimental { 11 | // Experimental since the API will certainly change drastically in the future. 12 | 13 | // Transforms velocity vector by rotation ``foo_R_bar``. 14 | // 15 | // Note: vel_bar can be either a linear or a rotational velocity vector. 16 | // 17 | template 18 | Vector3 transformVelocity(SO3 const& foo_R_bar, 19 | Vector3 const& vel_bar) { 20 | // For rotational velocities note that: 21 | // 22 | // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) 23 | // = vee(hat(Adj(foo_R_bar) * vel_bar)) 24 | // = Adj(foo_R_bar) * vel_bar 25 | // = foo_R_bar * vel_bar. 26 | // 27 | return foo_R_bar * vel_bar; 28 | } 29 | 30 | // Transforms velocity vector by pose ``foo_T_bar``. 31 | // 32 | // Note: vel_bar can be either a linear or a rotational velocity vector. 33 | // 34 | template 35 | Vector3 transformVelocity(SE3 const& foo_T_bar, 36 | Vector3 const& vel_bar) { 37 | return transformVelocity(foo_T_bar.so3(), vel_bar); 38 | } 39 | 40 | // finite difference approximation of instantanious velocity in frame foo 41 | // 42 | template 43 | Vector3 finiteDifferenceRotationalVelocity( 44 | std::function(Scalar)> const& foo_R_bar, Scalar t, 45 | Scalar h = Constants::epsilon()) { 46 | // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor 47 | // 48 | // W = dR(t)/dt * R^{-1}(t) 49 | Matrix3 dR_dt_in_frame_foo = curveNumDiff( 50 | [&foo_R_bar](Scalar t0) -> Matrix3 { 51 | return foo_R_bar(t0).matrix(); 52 | }, 53 | t, h); 54 | // velocity tensor 55 | Matrix3 W_in_frame_foo = 56 | dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); 57 | return SO3::vee(W_in_frame_foo); 58 | } 59 | 60 | // finite difference approximation of instantanious velocity in frame foo 61 | // 62 | template 63 | Vector3 finiteDifferenceRotationalVelocity( 64 | std::function(Scalar)> const& foo_T_bar, Scalar t, 65 | Scalar h = Constants::epsilon()) { 66 | return finiteDifferenceRotationalVelocity( 67 | [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, 68 | h); 69 | } 70 | 71 | } // namespace experimental 72 | } // namespace Sophus 73 | 74 | #endif // SOPHUS_VELOCITIES_HPP 75 | --------------------------------------------------------------------------------