Repository: YWL0720/I2EKF-LO Branch: master Commit: 8d2158cda30e Files: 164 Total size: 1.3 MB Directory structure: gitextract_qkvnzuum/ ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── Thirdparty/ │ ├── Sophus/ │ │ ├── .clang-format │ │ ├── CMakeLists.txt │ │ ├── LICENSE.txt │ │ ├── README.rst │ │ ├── Sophus.code-workspace │ │ ├── SophusConfig.cmake.in │ │ ├── appveyor.yml │ │ ├── cmake_modules/ │ │ │ └── FindEigen3.cmake │ │ ├── doxyfile │ │ ├── doxyrest-config.lua │ │ ├── examples/ │ │ │ ├── CMakeLists.txt │ │ │ └── HelloSO3.cpp │ │ ├── generate_stubs.py │ │ ├── make_docs.sh │ │ ├── package.xml │ │ ├── rst-dir/ │ │ │ ├── conf.py │ │ │ ├── page_index.rst │ │ │ └── pysophus.rst │ │ ├── run_format.sh │ │ ├── scripts/ │ │ │ ├── install_docs_deps.sh │ │ │ ├── install_linux_deps.sh │ │ │ ├── install_linux_fmt_deps.sh │ │ │ ├── install_osx_deps.sh │ │ │ └── run_cpp_tests.sh │ │ ├── setup.py │ │ ├── sophus/ │ │ │ ├── average.hpp │ │ │ ├── cartesian.hpp │ │ │ ├── ceres_local_parameterization.hpp │ │ │ ├── ceres_manifold.hpp │ │ │ ├── ceres_typetraits.hpp │ │ │ ├── common.hpp │ │ │ ├── example_ensure_handler.cpp │ │ │ ├── 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 │ │ │ ├── spline.hpp │ │ │ ├── test_macros.hpp │ │ │ ├── types.hpp │ │ │ └── velocities.hpp │ │ ├── sophus_pybind/ │ │ │ ├── README │ │ │ ├── SE3PyBind.h │ │ │ ├── SO3PyBind.h │ │ │ ├── SophusPyBind.h │ │ │ ├── bindings.cpp │ │ │ ├── examples/ │ │ │ │ └── sophus_quickstart_tutorial.ipynb │ │ │ └── tests/ │ │ │ └── sophusPybindTests.py │ │ ├── sophus_pybind-stubs/ │ │ │ ├── py.typed │ │ │ └── sophus_pybind.pyi │ │ ├── sympy/ │ │ │ ├── cpp_gencode/ │ │ │ │ ├── Se2_Dx_exp_x.cpp │ │ │ │ ├── Se2_Dx_log_this.cpp │ │ │ │ ├── Se2_Dx_this_mul_exp_x_at_0.cpp │ │ │ │ ├── Se3_Dx_exp_x.cpp │ │ │ │ ├── Se3_Dx_log_this.cpp │ │ │ │ ├── Se3_Dx_this_mul_exp_x_at_0.cpp │ │ │ │ ├── So2_Dx_exp_x.cpp │ │ │ │ ├── So2_Dx_log_exp_x_times_this_at_0.cpp │ │ │ │ ├── So2_Dx_log_this.cpp │ │ │ │ ├── So2_Dx_this_mul_exp_x_at_0.cpp │ │ │ │ ├── So3_Dx_exp_x.cpp │ │ │ │ ├── So3_Dx_log_exp_x_times_this_at_0.cpp │ │ │ │ ├── So3_Dx_log_this.cpp │ │ │ │ └── So3_Dx_this_mul_exp_x_at_0.cpp │ │ │ ├── run_tests.sh │ │ │ └── sophus/ │ │ │ ├── __init__.py │ │ │ ├── complex.py │ │ │ ├── cse_codegen.py │ │ │ ├── dual_quaternion.py │ │ │ ├── matrix.py │ │ │ ├── quaternion.py │ │ │ ├── se2.py │ │ │ ├── se3.py │ │ │ ├── so2.py │ │ │ └── so3.py │ │ └── test/ │ │ ├── CMakeLists.txt │ │ ├── ceres/ │ │ │ ├── CMakeLists.txt │ │ │ ├── test_ceres_rxso2.cpp │ │ │ ├── test_ceres_rxso3.cpp │ │ │ ├── test_ceres_se2.cpp │ │ │ ├── test_ceres_se3.cpp │ │ │ ├── test_ceres_sim2.cpp │ │ │ ├── test_ceres_sim3.cpp │ │ │ ├── test_ceres_so2.cpp │ │ │ ├── test_ceres_so3.cpp │ │ │ └── tests.hpp │ │ └── core/ │ │ ├── CMakeLists.txt │ │ ├── test_cartesian2.cpp │ │ ├── test_cartesian3.cpp │ │ ├── test_common.cpp │ │ ├── test_geometry.cpp │ │ ├── test_rxso2.cpp │ │ ├── test_rxso3.cpp │ │ ├── test_se2.cpp │ │ ├── test_se3.cpp │ │ ├── test_sim2.cpp │ │ ├── test_sim3.cpp │ │ ├── test_so2.cpp │ │ ├── test_so3.cpp │ │ ├── test_velocities.cpp │ │ └── tests.hpp │ └── tessil-src/ │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── appveyor.yml │ ├── cmake/ │ │ └── tsl-robin-mapConfig.cmake.in │ ├── doxygen.conf │ ├── include/ │ │ └── tsl/ │ │ ├── robin_growth_policy.h │ │ ├── robin_hash.h │ │ ├── robin_map.h │ │ └── robin_set.h │ ├── tests/ │ │ ├── CMakeLists.txt │ │ ├── custom_allocator_tests.cpp │ │ ├── main.cpp │ │ ├── policy_tests.cpp │ │ ├── robin_map_tests.cpp │ │ ├── robin_set_tests.cpp │ │ └── utils.h │ └── tsl-robin-map.natvis ├── config/ │ ├── avia.yaml │ ├── horizon.yaml │ ├── mid360.yaml │ ├── ouster.yaml │ ├── pandar.yaml │ ├── robosense.yaml │ └── velodyne.yaml ├── include/ │ ├── color.h │ ├── common_lib.h │ ├── ikd-Tree/ │ │ ├── README.md │ │ ├── ikd_Tree.cpp │ │ └── ikd_Tree.h │ ├── matplotlibcpp.h │ ├── scope_timer.hpp │ └── so3_math.h ├── launch/ │ ├── hesai_pandarXT.launch │ ├── livox_avia.launch │ ├── livox_horizon.launch │ ├── livox_mid360.launch │ ├── ouster.launch │ ├── robosense.launch │ └── velodyne.launch ├── msg/ │ ├── Pose6D.msg │ └── States.msg ├── package.xml ├── rviz_cfg/ │ ├── .gitignore │ ├── fast_lo.rviz │ └── spinning.rviz └── src/ ├── IMU_Processing.hpp ├── laserMapping.cpp ├── preprocess.cpp └── preprocess.h ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(i2ekf_lo) SET(CMAKE_BUILD_TYPE "Release") ADD_COMPILE_OPTIONS(-std=c++14 ) ADD_COMPILE_OPTIONS(-std=c++14 ) set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") message("Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) include(ProcessorCount) ProcessorCount(N) message("Processer number: ${N}") if(N GREATER 4) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=3) message("core for MP: 3") elseif(N GREATER 3) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=2) message("core for MP: 2") else() add_definitions(-DMP_PROC_NUM=1) endif() else() add_definitions(-DMP_PROC_NUM=1) endif() add_subdirectory(Thirdparty/Sophus) add_subdirectory(Thirdparty/tessil-src) find_package(OpenMP QUIET) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") find_package(PythonLibs REQUIRED) find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs sensor_msgs roscpp rospy std_msgs pcl_ros tf livox_ros_driver message_generation eigen_conversions ) find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) find_package(Ceres REQUIRED) message(Eigen: ${EIGEN3_INCLUDE_DIR}) include_directories( ../../devel/include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} include) add_message_files( FILES Pose6D.msg States.msg ) generate_messages( DEPENDENCIES geometry_msgs ) catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime DEPENDS EIGEN3 PCL INCLUDE_DIRS ) add_executable(i2ekf_lo_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) add_dependencies(i2ekf_lo_mapping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(i2ekf_lo_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${CERES_LIBRARIES} Sophus::Sophus tsl::robin_map) target_include_directories(i2ekf_lo_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) ================================================ FILE: LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 2, June 1991 Copyright (C) 1989, 1991 Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. This General Public License applies to most of the Free Software Foundation's software and to any other program whose authors commit to using it. (Some other Free Software Foundation software is covered by the GNU Lesser General Public License instead.) You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for this service if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs; and that you know you can do these things. To protect your rights, we need to make restrictions that forbid anyone to deny you these rights or to ask you to surrender the rights. These restrictions translate to certain responsibilities for you if you distribute copies of the software, or if you modify it. For example, if you distribute copies of such a program, whether gratis or for a fee, you must give the recipients all the rights that you have. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. We protect your rights with two steps: (1) copyright the software, and (2) offer you this license which gives you legal permission to copy, distribute and/or modify the software. Also, for each author's protection and ours, we want to make certain that everyone understands that there is no warranty for this free software. If the software is modified by someone else and passed on, we want its recipients to know that what they have is not the original, so that any problems introduced by others will not reflect on the original authors' reputations. Finally, any free program is threatened constantly by software patents. We wish to avoid the danger that redistributors of a free program will individually obtain patent licenses, in effect making the program proprietary. To prevent this, we have made it clear that any patent must be licensed for everyone's free use or not licensed at all. The precise terms and conditions for copying, distribution and modification follow. GNU GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License applies to any program or other work which contains a notice placed by the copyright holder saying it may be distributed under the terms of this General Public License. The "Program", below, refers to any such program or work, and a "work based on the Program" means either the Program or any derivative work under copyright law: that is to say, a work containing the Program or a portion of it, either verbatim or with modifications and/or translated into another language. (Hereinafter, translation is included without limitation in the term "modification".) Each licensee is addressed as "you". Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running the Program is not restricted, and the output from the Program is covered only if its contents constitute a work based on the Program (independent of having been made by running the Program). Whether that is true depends on what the Program does. 1. You may copy and distribute verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and give any other recipients of the Program a copy of this License along with the Program. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. 2. You may modify your copy or copies of the Program or any portion of it, thus forming a work based on the Program, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: a) You must cause the modified files to carry prominent notices stating that you changed the files and the date of any change. b) You must cause any work that you distribute or publish, that in whole or in part contains or is derived from the Program or any part thereof, to be licensed as a whole at no charge to all third parties under the terms of this License. c) If the modified program normally reads commands interactively when run, you must cause it, when started running for such interactive use in the most ordinary way, to print or display an announcement including an appropriate copyright notice and a notice that there is no warranty (or else, saying that you provide a warranty) and that users may redistribute the program under these conditions, and telling the user how to view a copy of this License. (Exception: if the Program itself is interactive but does not normally print such an announcement, your work based on the Program is not required to print an announcement.) These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Program, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Program, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Program. In addition, mere aggregation of another work not based on the Program with the Program (or with a work based on the Program) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. 3. You may copy and distribute the Program (or a work based on it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you also do one of the following: a) Accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, b) Accompany it with a written offer, valid for at least three years, to give any third party, for a charge no more than your cost of physically performing source distribution, a complete machine-readable copy of the corresponding source code, to be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, c) Accompany it with the information you received as to the offer to distribute corresponding source code. (This alternative is allowed only for noncommercial distribution and only if you received the program in object code or executable form with such an offer, in accord with Subsection b above.) The source code for a work means the preferred form of the work for making modifications to it. For an executable work, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the executable. However, as a special exception, the source code distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. If distribution of executable or object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place counts as distribution of the source code, even though third parties are not compelled to copy the source along with the object code. 4. You may not copy, modify, sublicense, or distribute the Program except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense or distribute the Program is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. 5. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Program or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Program (or any work based on the Program), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Program or works based on it. 6. Each time you redistribute the Program (or any work based on the Program), the recipient automatically receives a license from the original licensor to copy, distribute or modify the Program subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties to this License. 7. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Program at all. For example, if a patent license would not permit royalty-free redistribution of the Program by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Program. If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply and the section as a whole is intended to apply in other circumstances. It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system, which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. 8. If the distribution and/or use of the Program is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Program under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. 9. The Free Software Foundation may publish revised and/or new versions of the General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of this License, you may choose any version ever published by the Free Software Foundation. 10. If you wish to incorporate parts of the Program into other free programs whose distribution conditions are different, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. NO WARRANTY 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively convey the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. Also add information on how to contact you by electronic and paper mail. If the program is interactive, make it output a short notice like this when it starts in an interactive mode: Gnomovision version 69, Copyright (C) year name of author Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, the commands you use may be called something other than `show w' and `show c'; they could even be mouse-clicks or menu items--whatever suits your program. You should also get your employer (if you work as a programmer) or your school, if any, to sign a "copyright disclaimer" for the program, if necessary. Here is a sample; alter the names: Yoyodyne, Inc., hereby disclaims all copyright interest in the program `Gnomovision' (which makes passes at compilers) written by James Hacker. , 1 April 1989 Ty Coon, President of Vice This General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. ================================================ FILE: README.md ================================================

I2EKF-LO

A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry
IROS 2024 Oral
Pipeline Image
### Introduction LiDAR odometry is a pivotal technology in the fields of autonomous driving and autonomous mobile robotics. However, most of the current works focuse on nonlinear optimization methods, and there are still many challenges in using the traditional Iterative Extended Kalman Filter (IEKF) framework to tackle the problem: IEKF only iterates over the observation equation, relying on a rough estimate of the initial state, which is insufficient to fully eliminate motion distortion in the input point cloud; the system process noise is difficult to be determined during state estimation of the complex motions; and the varying motion models across different sensor carriers. To address these issues, we propose the Dual-Iteration Extended Kalman Filter (I2EKF) and the LiDAR odometry based on I2EKF (I2EKF-LO). This approach not only iterates over the observation equation but also leverages state updates to iteratively mitigate motion distortion in LiDAR point clouds. Moreover, it dynamically adjusts process noise based on the confidence level of prior predictions during state estimation and establishes motion models for different sensor carriers to achieve accurate and efficient state estimation. Comprehensive experiments demonstrate that I2EKF-LO achieves outstanding levels of accuracy and computational efficiency in the realm of LiDAR odometry. **Developers**: The codes of this repo are contributed by [Wenlu Yu (于文录)](https://github.com/YWL0720), [Jie Xu (徐杰)](https://github.com/jiejie567), [Chengwei Zhao (赵成伟)](https://github.com/chengwei0427) ### News * **[30/06/2024]**: I2EKF-LO is accepted to IROS 2024. * **[01/07/2024]**: We are currently working on organizing and refining the complete code. The full version will be released soon. * **[02/07/2024]**: Updated the video link and submitted the paper to arxiv. ### Related Paper Related paper available on arxiv: [I2EKF-LO: A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry](https://arxiv.org/abs/2407.02190) ### Related Video:
## 1. Prerequisites ### 1.1 **Ubuntu** and **ROS** Ubuntu >= 18.04. ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) ### 1.2. **PCL && Eigen** PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). ### 1.3. **livox_ros_driver** Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). ## 2. Build Clone the repository and catkin_make: ``` cd ~/catkin_ws/src git clone https://github.com/YWL0720/I2EKF-LO cd .. catkin_make -j source devel/setup.bash ``` ## 3. Directly run ```bash cd ~/$I2EKF_LO_ROS_DIR$ source devel/setup.bash roslaunch i2ekf_lo xxx.launch ``` ## 4. Rosbag Example Download our test bags here: [HIT-TIB Datasets](https://drive.google.com/drive/folders/1L5cX9uyAiZei17oq7ELyd8WyIRReHq8u?usp=drive_link). ## 5. Acknowledgments Thanks for [Fast-LIO2](https://github.com/hku-mars/FAST_LIO) (Fast Direct LiDAR-inertial Odometry) and [LI-INIT](https://github.com/hku-mars/LiDAR_IMU_Init)(Robust Real-time LiDAR-inertial Initialization). ## 6. License The source code is released under [GPLv2](http://www.gnu.org/licenses/) license. ## 7. TODO(s) - [ ] Updated video link - [ ] Upload a preprint of our paper ================================================ FILE: Thirdparty/Sophus/.clang-format ================================================ Language: Cpp # BasedOnStyle: Google AccessModifierOffset: -1 AlignAfterOpenBracket: Align AlignConsecutiveAssignments: false AlignConsecutiveDeclarations: false AlignEscapedNewlinesLeft: true AlignOperands: true AlignTrailingComments: true AllowAllParametersOfDeclarationOnNextLine: true AllowShortBlocksOnASingleLine: false AllowShortCaseLabelsOnASingleLine: false AllowShortFunctionsOnASingleLine: All AllowShortIfStatementsOnASingleLine: true AllowShortLoopsOnASingleLine: true AlwaysBreakAfterDefinitionReturnType: None AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: true AlwaysBreakTemplateDeclarations: true BinPackArguments: true BinPackParameters: true BraceWrapping: AfterClass: false AfterControlStatement: false AfterEnum: false AfterFunction: false AfterNamespace: false AfterObjCDeclaration: false AfterStruct: false AfterUnion: false BeforeCatch: false BeforeElse: false IndentBraces: false BreakBeforeBinaryOperators: None BreakBeforeBraces: Attach BreakBeforeTernaryOperators: true BreakConstructorInitializersBeforeComma: false ColumnLimit: 80 CommentPragmas: '^ IWYU pragma:' ConstructorInitializerAllOnOneLineOrOnePerLine: true ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 4 Cpp11BracedListStyle: true DerivePointerAlignment: true DisableFormat: false ExperimentalAutoDetectBinPacking: false ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] IncludeCategories: - Regex: '^<.*\.h>' Priority: 1 - Regex: '^<.*' Priority: 2 - Regex: '.*' Priority: 3 IndentCaseLabels: true IndentWidth: 2 IndentWrappedFunctionNames: false KeepEmptyLinesAtTheStartOfBlocks: false MacroBlockBegin: '' MacroBlockEnd: '' MaxEmptyLinesToKeep: 1 NamespaceIndentation: None ObjCBlockIndentWidth: 2 ObjCSpaceAfterProperty: false ObjCSpaceBeforeProtocolList: false PenaltyBreakBeforeFirstCallParameter: 1 PenaltyBreakComment: 300 PenaltyBreakFirstLessLess: 120 PenaltyBreakString: 1000 PenaltyExcessCharacter: 1000000 PenaltyReturnTypeOnItsOwnLine: 200 PointerAlignment: Left ReflowComments: true SortIncludes: true SpaceAfterCStyleCast: false SpaceBeforeAssignmentOperators: true SpaceBeforeParens: ControlStatements SpaceInEmptyParentheses: false SpacesBeforeTrailingComments: 2 SpacesInAngles: false SpacesInContainerLiterals: true SpacesInCStyleCastParentheses: false SpacesInParentheses: false SpacesInSquareBrackets: false Standard: Auto TabWidth: 8 UseTab: Never ================================================ FILE: Thirdparty/Sophus/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.4) project(Sophus VERSION 1.22.10) include(CMakePackageConfigHelpers) include(GNUInstallDirs) # Determine if sophus is built as a subproject (using add_subdirectory) # or if it is the master project. if (NOT DEFINED SOPHUS_MASTER_PROJECT) set(SOPHUS_MASTER_PROJECT OFF) if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) set(SOPHUS_MASTER_PROJECT ON) message(STATUS "CMake version: ${CMAKE_VERSION}") endif () endif () option(SOPHUS_INSTALL "Generate the install target." ${SOPHUS_MASTER_PROJECT}) option(SOPHUS_USE_BASIC_LOGGING "Use basic logging (in ensure and test macros)" OFF) if(SOPHUS_MASTER_PROJECT) # Release by default # Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() set(CMAKE_CXX_STANDARD 14) # Set compiler specific settings (FixMe: Should not cmake do this for us automatically?) IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") SET(CMAKE_CXX_FLAGS_RELEASE "-O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics") ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") SET(CMAKE_CXX_FLAGS_RELEASE "-O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++14 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0") SET(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage") SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage") ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$") ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP") ENDIF() # Add local path for finding packages, set the local version first list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules") endif() if(SOPHUS_USE_BASIC_LOGGING) set (CMAKE_DISABLE_FIND_PACKAGE_fmt ON) endif() # Find public dependencies if targets are not yet defined. (Targets might be for example # defined by a parent project including Sophus via `add_subdirectory`.) if(NOT TARGET Eigen3::Eigen) find_package(Eigen3 3.3.0 REQUIRED) endif() if(NOT TARGET fmt::fmt) find_package(fmt) endif() # Define interface library target add_library(sophus INTERFACE) add_library (Sophus::Sophus ALIAS sophus) set(SOPHUS_HEADER_FILES sophus/average.hpp sophus/cartesian.hpp sophus/ceres_local_parameterization.hpp sophus/ceres_manifold.hpp sophus/ceres_typetraits.hpp sophus/common.hpp sophus/geometry.hpp sophus/interpolate.hpp sophus/interpolate_details.hpp sophus/num_diff.hpp sophus/rotation_matrix.hpp sophus/rxso2.hpp sophus/rxso3.hpp sophus/se2.hpp sophus/se3.hpp sophus/sim2.hpp sophus/sim3.hpp sophus/sim_details.hpp sophus/so2.hpp sophus/so3.hpp sophus/spline.hpp sophus/types.hpp sophus/velocities.hpp ) set(SOPHUS_OTHER_FILES sophus/test_macros.hpp sophus/example_ensure_handler.cpp ) if(MSVC) # Define common math constants if we compile with MSVC target_compile_definitions (sophus INTERFACE _USE_MATH_DEFINES) endif (MSVC) # Add Eigen interface dependency, depending on available cmake info if(TARGET Eigen3::Eigen) target_link_libraries(sophus INTERFACE Eigen3::Eigen) set(Eigen3_DEPENDENCY "find_dependency (Eigen3 ${Eigen3_VERSION})") else() target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR}) endif() if(SOPHUS_USE_BASIC_LOGGING OR NOT TARGET fmt::fmt) # NOTE fmt_FOUND does not seem to be defined even though the package config # was found. target_compile_definitions(sophus INTERFACE SOPHUS_USE_BASIC_LOGGING=1) message(STATUS "Turning basic logging ON") else() target_link_libraries(sophus INTERFACE fmt::fmt) set(fmt_DEPENDENCY "find_dependency (fmt ${fmt_VERSION})") message(STATUS "Turning basic logging OFF") endif() # Associate target with include directory target_include_directories(sophus INTERFACE "$" "$" ) # Declare all used C++14 features target_compile_features (sophus INTERFACE cxx_auto_type cxx_decltype cxx_nullptr cxx_right_angle_brackets cxx_variadic_macros cxx_variadic_templates ) # Add sources as custom target so that they are shown in IDE's add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES} ${SOPHUS_HEADER_FILES}) # Create 'test' make target using ctest option(BUILD_SOPHUS_TESTS "Build tests." ON) if(BUILD_SOPHUS_TESTS) enable_testing() add_subdirectory(test) endif() # Create examples make targets using ctest option(BUILD_SOPHUS_EXAMPLES "Build examples." ON) if(BUILD_SOPHUS_EXAMPLES) add_subdirectory(examples) endif() # Build python sophus bindings option(BUILD_PYTHON_BINDINGS "Build python sophus bindings." OFF) if(BUILD_PYTHON_BINDINGS) include(FetchContent) FetchContent_Declare( pybind11 GIT_REPOSITORY https://github.com/pybind/pybind11.git GIT_TAG master ) FetchContent_MakeAvailable(pybind11) add_subdirectory(${pybind11_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}/pybind) pybind11_add_module(sophus_pybind ${CMAKE_CURRENT_SOURCE_DIR}/sophus_pybind/bindings.cpp) target_link_libraries(sophus_pybind PUBLIC sophus) endif(BUILD_PYTHON_BINDINGS) if(SOPHUS_INSTALL) # Export package for use from the build tree set(SOPHUS_CMAKE_EXPORT_DIR ${CMAKE_INSTALL_DATADIR}/sophus/cmake) set_target_properties(sophus PROPERTIES EXPORT_NAME Sophus) install(TARGETS sophus EXPORT SophusTargets) install(EXPORT SophusTargets NAMESPACE Sophus:: DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} ) export(TARGETS sophus NAMESPACE Sophus:: FILE SophusTargets.cmake) export(PACKAGE Sophus) configure_package_config_file( SophusConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake INSTALL_DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} NO_CHECK_REQUIRED_COMPONENTS_MACRO ) # Remove architecture dependence. Sophus is a header-only library. set(TEMP_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P}) unset(CMAKE_SIZEOF_VOID_P) # Write version to file write_basic_package_version_file ( SophusConfigVersion.cmake VERSION ${PROJECT_VERSION} COMPATIBILITY SameMajorVersion ) # Recover architecture dependence set(CMAKE_SIZEOF_VOID_P ${TEMP_SIZEOF_VOID_P}) # Install cmake targets install( FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} ) # Install header files install( FILES ${SOPHUS_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus ) endif() ================================================ FILE: Thirdparty/Sophus/LICENSE.txt ================================================ Copyright 2011-2017 Hauke Strasdat 2012-2017 Steven Lovegrove Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: Thirdparty/Sophus/README.rst ================================================ |GithubCICpp|_ windows: |AppVeyor|_ |GithubCISympy|_ |ci_cov|_ Sophus ====== Overview -------- This is a c++ implementation of Lie groups commonly used for 2d and 3d geometric problems (i.e. for Computer Vision or Robotics applications). Among others, this package includes the special orthogonal groups SO(2) and SO(3) to present rotations in 2d and 3d as well as the special Euclidean group SE(2) and SE(3) to represent rigid body transformations (i.e. rotations and translations) in 2d and 3d. API documentation: https://strasdat.github.io/Sophus/ Cross platform support ---------------------- Sophus compiles with clang and gcc on Linux and OS X as well as msvc on Windows. The specific compiler and operating system versions which are supported are the ones which are used in the Continuous Integration (CI): See GitHubCI_ and AppVeyor_ for details. However, it should work (with no to minor modification) on many other modern configurations as long they support c++14, CMake, Eigen 3.3.X and (optionally) fmt. The fmt dependency can be eliminated by passing "-DUSE_BASIC_LOGGING=ON" to cmake when configuring Sophus. .. _GitHubCI: https://github.com/strasdat/Sophus/actions .. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true .. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master .. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master .. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master .. |GithubCICpp| image:: https://github.com/strasdat/Sophus/actions/workflows/main.yml/badge.svg?branch=master .. _GithubCICpp: https://github.com/strasdat/Sophus/actions/workflows/main.yml?query=branch%3Amaster .. |GithubCISympy| image:: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml/badge.svg?branch=master .. _GithubCISympy: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml?query=branch%3Amaster ================================================ FILE: Thirdparty/Sophus/Sophus.code-workspace ================================================ { "folders": [ { "path": "." } ], "settings": {} } ================================================ FILE: Thirdparty/Sophus/SophusConfig.cmake.in ================================================ @PACKAGE_INIT@ include (CMakeFindDependencyMacro) @Eigen3_DEPENDENCY@ @fmt_DEPENDENCY@ include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake") ================================================ FILE: Thirdparty/Sophus/appveyor.yml ================================================ branches: only: - master os: Visual Studio 2015 clone_folder: c:\projects\sophus platform: x64 configuration: Debug build: project: c:\projects\sophus\build\Sophus.sln install: - ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul - git clone https://github.com/fmtlib/fmt.git - cd fmt - git checkout 5.3.0 - mkdir build - cd build - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug .. - cmake --build . - cmake --build . --target install - cd ../.. before_build: - cd c:\projects\sophus - mkdir build - cd build - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3.4 .. after_build: - ctest --output-on-failure ================================================ FILE: Thirdparty/Sophus/cmake_modules/FindEigen3.cmake ================================================ # - Try to find Eigen3 lib # # This module supports requiring a minimum version, e.g. you can do # find_package(Eigen3 3.1.2) # to require version 3.1.2 or newer of Eigen3. # # Once done this will define # # EIGEN3_FOUND - system has eigen lib with correct version # EIGEN3_INCLUDE_DIR - the eigen include directory # EIGEN3_VERSION - eigen version # # and the following imported target: # # Eigen3::Eigen - The header-only Eigen library # # This module reads hints about search locations from # the following environment variables: # # EIGEN3_ROOT # EIGEN3_ROOT_DIR # Copyright (c) 2006, 2007 Montel Laurent, # Copyright (c) 2008, 2009 Gael Guennebaud, # Copyright (c) 2009 Benoit Jacob # Redistribution and use is allowed according to the terms of the 2-clause BSD license. if(NOT Eigen3_FIND_VERSION) if(NOT Eigen3_FIND_VERSION_MAJOR) set(Eigen3_FIND_VERSION_MAJOR 2) endif() if(NOT Eigen3_FIND_VERSION_MINOR) set(Eigen3_FIND_VERSION_MINOR 91) endif() if(NOT Eigen3_FIND_VERSION_PATCH) set(Eigen3_FIND_VERSION_PATCH 0) endif() set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") endif() macro(_eigen3_check_version) file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) set(EIGEN3_VERSION_OK FALSE) else() set(EIGEN3_VERSION_OK TRUE) endif() if(NOT EIGEN3_VERSION_OK) message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " "but at least version ${Eigen3_FIND_VERSION} is required") endif() endmacro() if (EIGEN3_INCLUDE_DIR) # in cache already _eigen3_check_version() set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) set(Eigen3_FOUND ${EIGEN3_VERSION_OK}) else () # search first if an Eigen3Config.cmake is available in the system, # if successful this would set EIGEN3_INCLUDE_DIR and the rest of # the script will work as usual find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) if(NOT EIGEN3_INCLUDE_DIR) find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library HINTS ENV EIGEN3_ROOT ENV EIGEN3_ROOT_DIR PATHS ${CMAKE_INSTALL_PREFIX}/include ${KDE4_INCLUDE_DIR} PATH_SUFFIXES eigen3 eigen ) endif() if(EIGEN3_INCLUDE_DIR) _eigen3_check_version() endif() include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) mark_as_advanced(EIGEN3_INCLUDE_DIR) endif() if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen) add_library(Eigen3::Eigen INTERFACE IMPORTED) set_target_properties(Eigen3::Eigen PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}") endif() ================================================ FILE: Thirdparty/Sophus/doxyfile ================================================ DOXYFILE_ENCODING = UTF-8 PROJECT_NAME = "Sophus" INPUT = sophus EXTRACT_ALL = YES ENABLE_PREPROCESSING = YES MACRO_EXPANSION = YES WARN_AS_ERROR = YES EXPAND_ONLY_PREDEF = NO SKIP_FUNCTION_MACROS = NO AUTOLINK_SUPPORT = YES MULTILINE_CPP_IS_BRIEF = YES MARKDOWN_SUPPORT = YES INLINE_INHERITED_MEMB = NO EXCLUDE_SYMBOLS = Eigen::internal Sophus::details Sophus::interp_details Sophus::experimental GENERATE_LATEX = NO STRIP_CODE_COMMENTS = NO GENERATE_XML = YES GENERATE_HTML = NO XML_OUTPUT = xml-dir XML_PROGRAMLISTING = NO CASE_SENSE_NAMES = NO HIDE_UNDOC_RELATIONS = YES EXTRACT_ALL = YES ================================================ FILE: Thirdparty/Sophus/doxyrest-config.lua ================================================ FRAME_DIR_LIST = { "doxyrest_b/doxyrest/frame/cfamily", "doxyrest_b/doxyrest/frame/common" } FRAME_FILE = "index.rst.in" INPUT_FILE = "xml-dir/index.xml" OUTPUT_FILE = "rst-dir/index.rst" INTRO_FILE = "page_index.rst" SORT_GROUPS_BY = "title" GLOBAL_AUX_COMPOUND_ID = "group_global" LANGUAGE = cpp VERBATIM_TO_CODE_BLOCK = "cpp" ESCAPE_ASTERISKS = true ESCAPE_PIPES = true ESCAPE_TRAILING_UNDERSCORES = true PROTECTION_FILTER = "protected" EXCLUDE_EMPTY_DEFINES = true EXCLUDE_DEFAULT_CONSTRUCTORS = false EXCLUDE_DESTRUCTORS = false EXCLUDE_PRIMITIVE_TYPEDEFS = true SHOW_DIRECT_DESCENDANTS = true TYPEDEF_TO_USING = true ML_PARAM_LIST_LENGTH_THRESHOLD = 80 ================================================ FILE: Thirdparty/Sophus/examples/CMakeLists.txt ================================================ # Tests to run SET( EXAMPLE_SOURCES HelloSO3) FOREACH(example_src ${EXAMPLE_SOURCES}) ADD_EXECUTABLE( ${example_src} ${example_src}.cpp) TARGET_LINK_LIBRARIES( ${example_src} sophus) ENDFOREACH(example_src) ================================================ FILE: Thirdparty/Sophus/examples/HelloSO3.cpp ================================================ #include #include "sophus/geometry.hpp" int main() { // The following demonstrates the group multiplication of rotation matrices // Create rotation matrices from rotations around the x and y and z axes: const double kPi = Sophus::Constants::pi(); Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4); Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6); Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3); std::cout << "The rotation matrices are" << std::endl; std::cout << "R1:\n" << R1.matrix() << std::endl; std::cout << "R2:\n" << R2.matrix() << std::endl; std::cout << "R3:\n" << R3.matrix() << std::endl; std::cout << "Their product R1*R2*R3:\n" << (R1 * R2 * R3).matrix() << std::endl; std::cout << std::endl; // Rotation matrices can act on vectors Eigen::Vector3d x; x << 0.0, 0.0, 1.0; std::cout << "Rotation matrices can act on vectors" << std::endl; std::cout << "x\n" << x << std::endl; std::cout << "R2*x\n" << R2 * x << std::endl; std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl; std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl; std::cout << std::endl; // SO(3) are internally represented as unit quaternions. std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl; std::cout << "R1 in unit quaternion form:\n" << R1.unit_quaternion().coeffs() << std::endl; // Note that the order of coefficients of Eigen's quaternion class is // (imag0, imag1, imag2, real) std::cout << std::endl; } ================================================ FILE: Thirdparty/Sophus/generate_stubs.py ================================================ import subprocess subprocess.run( "pybind11-stubgen sophus_pybind -o sophus_pybind-stubs/", shell=True, check=True, ) subprocess.run("touch sophus_pybind-stubs/py.typed", shell=True, check=True) ================================================ FILE: Thirdparty/Sophus/make_docs.sh ================================================ doxygen doxyfile doxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua sphinx-build -b html rst-dir html-dir ================================================ FILE: Thirdparty/Sophus/package.xml ================================================ sophus 1.22.10 C++ implementation of Lie Groups using Eigen. https://github.com/strasdat/sophus https://github.com/strasdat/sophus/issues Daniel Stonier Hauke Strasdat MIT cmake eigen eigen cmake ================================================ FILE: Thirdparty/Sophus/rst-dir/conf.py ================================================ # Configuration file for the Sphinx documentation builder. # # This file only contains a selection of the most common options. For a full # list see the documentation: # http://www.sphinx-doc.org/en/master/config # -- Path setup -------------------------------------------------------------- # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. # import os import sys sys.path.insert(0, os.path.abspath('../sympy')) sys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx')) extensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc'] # -- Project information ----------------------------------------------------- project = 'Sophus' copyright = '2019, Hauke Strasdat' author = 'Hauke Strasdat' # Tell sphinx what the primary language being documented is. primary_domain = 'cpp' # Tell sphinx what the pygments highlight language should be. highlight_language = 'cpp' # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. exclude_patterns = [] # -- Options for HTML output ------------------------------------------------- # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # html_theme = "sphinx_rtd_theme" # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] ================================================ FILE: Thirdparty/Sophus/rst-dir/page_index.rst ================================================ Sophus - Lie groups for 2d/3d Geometry ======================================= .. toctree:: :maxdepth: 2 :caption: Contents: GitHub Page pysophus ================================================ FILE: Thirdparty/Sophus/rst-dir/pysophus.rst ================================================ Python API ========== .. automodule:: sophus.matrix :members: .. automodule:: sophus.complex :members: .. automodule:: sophus.quaternion :members: .. automodule:: sophus.so2 :members: .. automodule:: sophus.so3 :members: .. automodule:: sophus.se2 :members: .. automodule:: sophus.se3 :members: ================================================ FILE: Thirdparty/Sophus/run_format.sh ================================================ find . -type d \( -path ./sympy -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o \( -iname "*.hpp" -o -iname "*.cpp" \) -print | xargs clang-format -i ================================================ FILE: Thirdparty/Sophus/scripts/install_docs_deps.sh ================================================ #!/bin/bash set -x # echo on set -e # exit on error sudo apt-get -qq update sudo apt-get install doxygen liblua5.3-dev ragel pip3 install 'sphinx==2.0.1' pip3 install sphinx_rtd_theme pip3 install sympy git clone https://github.com/vovkos/doxyrest_b cd doxyrest_b git reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958 git submodule update --init mkdir build cd build cmake .. cmake --build . cd ../.. ================================================ FILE: Thirdparty/Sophus/scripts/install_linux_deps.sh ================================================ #!/bin/bash set -x # echo on set -e # exit on error cmake --version sudo apt-get -qq update sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libceres-dev ccache wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2 tar xvf eigen-3.3.4.tar.bz2 mkdir build-eigen cd build-eigen cmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT sudo make install cd .. git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver cd ceres-solver git reset --hard b0aef211db734379319c19c030e734d6e23436b0 mkdir build cd build ccache -s cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. make -j8 sudo make install cd ../.. ================================================ FILE: Thirdparty/Sophus/scripts/install_linux_fmt_deps.sh ================================================ #!/bin/bash set -x # echo on set -e # exit on error git clone https://github.com/fmtlib/fmt.git cd fmt git checkout 5.3.0 mkdir build cd build cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. make -j8 sudo make install cd ../.. ================================================ FILE: Thirdparty/Sophus/scripts/install_osx_deps.sh ================================================ #!/bin/bash set -x # echo on set -e # exit on error brew update brew install fmt brew install ccache # Build a specific version of ceres-solver instead of one shipped over brew curl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb patch < #include "cartesian.hpp" #include "common.hpp" #include "rxso2.hpp" #include "rxso3.hpp" #include "se2.hpp" #include "se3.hpp" #include "sim2.hpp" #include "sim3.hpp" #include "so2.hpp" #include "so3.hpp" namespace Sophus { /// Calculates mean iteratively. /// /// Returns ``nullopt`` if it does not converge. /// template optional iterativeMean( SequenceContainer const& foo_Ts_bar, int max_num_iterations) { size_t N = foo_Ts_bar.size(); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); using Group = typename SequenceContainer::value_type; using Scalar = typename Group::Scalar; using Tangent = typename Group::Tangent; // This implements the algorithm in the beginning of Sec. 4.2 in // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. Group foo_T_average = foo_Ts_bar.front(); Scalar w = Scalar(1. / N); for (int i = 0; i < max_num_iterations; ++i) { Tangent average; setToZero(average); for (Group const& foo_T_bar : foo_Ts_bar) { average += w * (foo_T_average.inverse() * foo_T_bar).log(); } Group foo_T_newaverage = foo_T_average * Group::exp(average); if (squaredNorm( (foo_T_newaverage.inverse() * foo_T_average).log()) < Constants::epsilon()) { return foo_T_newaverage; } foo_T_average = foo_T_newaverage; } // LCOV_EXCL_START return nullopt; // LCOV_EXCL_STOP } #ifdef DOXYGEN_SHOULD_SKIP_THIS /// Mean implementation for any Lie group. template optional average( SequenceContainer const& foo_Ts_bar); #else // Mean implementation for Cartesian. template enable_if_t >::value, optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); Sophus::Vector average; average.setZero(); for (Cartesian const& foo_T_bar : foo_Ts_bar) { average += foo_T_bar.params(); } return Cartesian(average / Scalar(N)); } // Mean implementation for SO(2). template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { // This implements rotational part of Proposition 12 from Sec. 6.2 of // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); SO2 foo_T_average = foo_Ts_bar.front(); Scalar w = Scalar(1. / N); Scalar average(0); for (SO2 const& foo_T_bar : foo_Ts_bar) { average += w * (foo_T_average.inverse() * foo_T_bar).log(); } return foo_T_average * SO2::exp(average); } // Mean implementation for RxSO(2). template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); RxSO2 foo_T_average = foo_Ts_bar.front(); Scalar w = Scalar(1. / N); Vector2 average(Scalar(0), Scalar(0)); for (RxSO2 const& foo_T_bar : foo_Ts_bar) { average += w * (foo_T_average.inverse() * foo_T_bar).log(); } return foo_T_average * RxSO2::exp(average); } namespace details { template void getQuaternion(T const&); template Eigen::Quaternion getUnitQuaternion(SO3 const& R) { return R.unit_quaternion(); } template Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { return sR.so3().unit_quaternion(); } template Eigen::Quaternion averageUnitQuaternion( SequenceContainer const& foo_Ts_bar) { // This: http://stackoverflow.com/a/27410865/1221742 size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); Eigen::Matrix Q(4, N); int i = 0; Scalar w = Scalar(1. / N); for (auto const& foo_T_bar : foo_Ts_bar) { Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); ++i; } Eigen::Matrix QQt = Q * Q.transpose(); // TODO: Figure out why we can't use SelfAdjointEigenSolver here. Eigen::EigenSolver > es(QQt); std::complex max_eigenvalue = es.eigenvalues()[0]; Eigen::Matrix, 4, 1> max_eigenvector = es.eigenvectors().col(0); for (int i = 1; i < 4; i++) { if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { max_eigenvalue = es.eigenvalues()[i]; max_eigenvector = es.eigenvectors().col(i); } } Eigen::Quaternion quat; quat.coeffs() << // max_eigenvector[0].real(), // max_eigenvector[1].real(), // max_eigenvector[2].real(), // max_eigenvector[3].real(); return quat; } } // namespace details // Mean implementation for SO(3). // // TODO: Detect degenerated cases and return nullopt. template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { return SO3(details::averageUnitQuaternion(foo_Ts_bar)); } // Mean implementation for R x SO(3). template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); Scalar scale_sum = Scalar(0); using std::exp; using std::log; for (RxSO3 const& foo_T_bar : foo_Ts_bar) { scale_sum += log(foo_T_bar.scale()); } return RxSO3(exp(scale_sum / Scalar(N)), SO3(details::averageUnitQuaternion(foo_Ts_bar))); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { // TODO: Implement Proposition 12 from Sec. 6.2 of // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. return iterativeMean(foo_Ts_bar, max_num_iterations); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } #endif // DOXYGEN_SHOULD_SKIP_THIS } // namespace Sophus ================================================ FILE: Thirdparty/Sophus/sophus/cartesian.hpp ================================================ /// @file /// Cartesian - Euclidean vector space as Lie group #pragma once #include namespace Sophus { template class Cartesian; template using Cartesian2 = Cartesian; template using Cartesian3 = Cartesian; using Cartesian2d = Cartesian2; using Cartesian3d = Cartesian3; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { using Scalar = Scalar_; using ParamsType = Sophus::Vector; }; template struct traits, Options>> : traits> { using Scalar = Scalar_; using ParamsType = Map, Options>; }; template struct traits const, Options>> : traits const> { using Scalar = Scalar_; using ParamsType = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// Cartesian base type - implements Cartesian class but is storage agnostic. /// /// Euclidean vector space as Lie group. /// /// Lie groups can be seen as a generalization over the Euclidean vector /// space R^M. Here a N-dimensional vector ``p`` is represented as a // (M+1) x (M+1) homogeneous matrix: /// /// | I p | /// | o 1 | /// /// On the other hand, Cartesian(M) can be seen as a special case of SE(M) /// with identity rotation, and hence represents pure translation. /// /// The purpose of this class is two-fold: /// - for educational purpose, to highlight how Lie groups generalize over /// Euclidean vector spaces. /// - to be used in templated/generic algorithms (such as Sophus::Spline) /// which are implemented against the Lie group interface. /// /// Obviously, Cartesian(M) can just be represented as a M-tuple. /// /// Cartesian is not compact, but a commutative group. For vector additions it /// holds `a+b = b+a`. /// /// See Cartesian class for more details below. /// template class CartesianBase { public: using Scalar = typename Eigen::internal::traits::Scalar; using ParamsType = typename Eigen::internal::traits::ParamsType; /// Degrees of freedom of manifold, equals to number of Cartesian coordinates. static int constexpr DoF = M; /// Number of internal parameters used, also M. static int constexpr num_parameters = M; /// Group transformations are (M+1)x(M+1) matrices. static int constexpr N = M + 1; static int constexpr Dim = M; using Transformation = Sophus::Matrix; using Point = Sophus::Vector; using HomogeneousPoint = Sophus::Vector; using Line = ParametrizedLine; using Hyperplane = Eigen::Hyperplane; using Tangent = Sophus::Vector; using Adjoint = Matrix; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with Cartesian operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using CartesianSum = Cartesian, M>; template using PointProduct = Sophus::Vector, M>; template using HomogeneousPointProduct = Sophus::Vector, N>; /// Adjoint transformation /// /// Always identity of commutative groups. SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC Cartesian cast() const { return Cartesian(params().template cast()); } /// Returns derivative of this * exp(x) wrt x at x=0. /// SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() const { Sophus::Matrix m; m.setIdentity(); return m; } /// Returns derivative of log(this^{-1} * x) by x at x=this. /// SOPHUS_FUNC Matrix Dx_log_this_inv_by_x_at_this() const { Matrix m; m.setIdentity(); return m; } /// Returns group inverse. /// /// The additive inverse. /// SOPHUS_FUNC Cartesian inverse() const { return Cartesian(-params()); } /// Logarithmic map /// /// For Euclidean vector space, just the identity. Or to be more precise /// it just extracts the significant M-vector from the NxN matrix. /// SOPHUS_FUNC Tangent log() const { return params(); } /// Returns 4x4 matrix representation of the instance. /// /// It has the following form: /// /// | I p | /// | o 1 | /// SOPHUS_FUNC Transformation matrix() const { Sophus::Matrix matrix; matrix.setIdentity(); matrix.col(M).template head() = params(); return matrix; } /// Group multiplication, are vector additions. /// template SOPHUS_FUNC CartesianBase& operator=( CartesianBase const& other) { params() = other.params(); return *this; } /// Group multiplication, are vector additions. /// template SOPHUS_FUNC CartesianSum operator*( CartesianBase const& other) const { return CartesianSum(params() + other.params()); } /// Group action on points, again just vector addition. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return PointProduct(params() + p); } /// Group action on homogeneous points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rp = *this * p.template head(); HomogeneousPointProduct r; r << rp, p(M); return r; } /// Group action on lines. /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), l.direction()); } /// Group action on planes. /// SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { return Hyperplane(p.normal(), p.offset() - params().dot(p.normal())); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this Cartesian's Scalar /// type. /// template >::value>::type> SOPHUS_FUNC CartesianBase& operator*=( CartesianBase const& other) { *static_cast(this) = *this * other; return *this; } /// Mutator of params vector. /// SOPHUS_FUNC ParamsType& params() { return static_cast(this)->params(); } /// Accessor of params vector /// SOPHUS_FUNC ParamsType const& params() const { return static_cast(this)->params(); } }; /// Cartesian using default storage; derived from CartesianBase. template class Cartesian : public CartesianBase, M> { using Base = CartesianBase, M>; public: static int constexpr DoF = Base::DoF; static int constexpr num_parameters = Base::num_parameters; static int constexpr N = Base::N; static int constexpr Dim = Base::Dim; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using ParamsMember = Sophus::Vector; using Base::operator=; /// Define copy-assignment operator explicitly. The definition of /// implicit copy assignment operator is deprecated in presence of a /// user-declared copy constructor (-Wdeprecated-copy in clang >= 13). SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes to zero vector. /// SOPHUS_FUNC Cartesian() { params_.setZero(); } /// Copy constructor /// SOPHUS_FUNC Cartesian(Cartesian const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC Cartesian(CartesianBase const& other) : params_(other.params()) { static_assert(std::is_same::value, "must be same Scalar type"); } /// Accepts either M-vector or (M+1)x(M+1) matrices. /// template explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase const& m) { static_assert( std::is_same::Scalar, Scalar>::value, ""); if (m.rows() == DoF && m.cols() == 1) { // trick so this compiles params_ = m.template block(0, 0); } else if (m.rows() == N && m.cols() == N) { params_ = m.template block(0, M); } else { SOPHUS_ENSURE(false, "{} {}", m.rows(), m.cols()); } } /// This provides unsafe read/write access to internal data. /// SOPHUS_FUNC Scalar* data() { return params_.data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { return params_.data(); } /// Returns derivative of exp(x) wrt. x. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_at_0() { Sophus::Matrix m; m.setIdentity(); return m; } /// Returns derivative of exp(x) wrt. x_i at x=0. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( Tangent const&) { return Dx_exp_x_at_0(); } /// Returns derivative of exp(x) * p wrt. x_i at x=0. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_times_point_at_0( Point const&) { Sophus::Matrix J; J.setIdentity(); return J; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Mutator of params vector /// SOPHUS_FUNC ParamsMember& params() { return params_; } /// Accessor of params vector /// SOPHUS_FUNC ParamsMember const& params() const { return params_; } /// Returns the ith infinitesimal generators of Cartesian(M). /// /// The infinitesimal generators for e.g. the 3-dimensional case: /// /// ``` /// | 0 0 0 1 | /// G_0 = | 0 0 0 0 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_1 = | 0 0 0 1 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_2 = | 0 0 0 0 | /// | 0 0 0 1 | /// | 0 0 0 0 | /// ``` /// /// Precondition: ``i`` must be in [0, M-1]. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= M, "i should be in range [0,M-1]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// Group exponential /// /// For Euclidean vector space, just the identity. Or to be more precise /// it just constructs the (M+1xM+1) homogeneous matrix representation // from the M-vector. /// SOPHUS_FUNC static Cartesian exp(Tangent const& a) { return Cartesian(a); } /// hat-operator /// /// Formally, the hat()-operator of Cartesian(M) is defined as /// /// ``hat(.): R^M -> R^{M+1xM+1}, hat(a) = sum_i a_i * G_i`` /// (for i=0,...,M-1) /// /// with ``G_i`` being the ith infinitesimal generator of Cartesian(M). /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; Omega.setZero(); Omega.col(M).template head() = a.template head(); return Omega; } /// Lie bracket /// /// Always 0 for commutative groups. SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { return Tangent::Zero(); } /// Draws uniform samples in the range [-1, 1] per coordinates. /// template static Cartesian sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); Vector v; for (int i = 0; i < M; ++i) { v[i] = uniform(generator); } return Cartesian(v); } /// vee-operator /// /// This is the inverse of the hat()-operator, see above. /// SOPHUS_FUNC static Tangent vee(Transformation const& m) { return m.col(M).template head(); } protected: ParamsMember params_; }; } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``Cartesian``; derived from /// CartesianBase. /// /// Allows us to wrap Cartesian objects around POD array. template class Map, Options> : public Sophus::CartesianBase, Options>, M> { public: using Base = Sophus::CartesianBase, Options>, M>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Base::operator=; using Base::operator*=; using Base::operator*; SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {} /// Mutator of params vector /// SOPHUS_FUNC Map>& params() { return params_; } /// Accessor of params vector /// SOPHUS_FUNC Map> const& params() const { return params_; } protected: Map, Options> params_; }; /// Specialization of Eigen::Map for ``Cartesian const``; derived from /// CartesianBase. /// /// Allows us to wrap Cartesian objects around POD array. template class Map const, Options> : public Sophus::CartesianBase< Map const, Options>, M> { public: using Base = Sophus::CartesianBase const, Options>, M>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {} /// Accessor of params vector /// SOPHUS_FUNC Map const, Options> const& params() const { return params_; } protected: Map const, Options> const params_; }; } // namespace Eigen ================================================ FILE: Thirdparty/Sophus/sophus/ceres_local_parameterization.hpp ================================================ #pragma once #include #include namespace Sophus { /// Templated local parameterization for LieGroup [with implemented /// LieGroup::Dx_this_mul_exp_x_at_0() ] template