├── .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-stubs │ │ ├── py.typed │ │ └── sophus_pybind.pyi │ ├── sophus_pybind │ │ ├── README │ │ ├── SE3PyBind.h │ │ ├── SO3PyBind.h │ │ ├── SophusPyBind.h │ │ ├── bindings.cpp │ │ ├── examples │ │ │ └── sophus_quickstart_tutorial.ipynb │ │ └── tests │ │ │ └── sophusPybindTests.py │ ├── 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 ├── image ├── overview.png └── video_cover.png ├── 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 /.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YWL0720/I2EKF-LO/8d2158cda30ed9b261e668b9ab6f75f5ab424624/.gitignore -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(i2ekf_lo) 3 | 4 | SET(CMAKE_BUILD_TYPE "Release") 5 | 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 9 | 10 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 13 | set(CMAKE_CXX_STANDARD 14) 14 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 15 | set(CMAKE_CXX_EXTENSIONS OFF) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 17 | 18 | message("Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") 19 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 20 | include(ProcessorCount) 21 | ProcessorCount(N) 22 | message("Processer number: ${N}") 23 | if(N GREATER 4) 24 | add_definitions(-DMP_EN) 25 | add_definitions(-DMP_PROC_NUM=3) 26 | message("core for MP: 3") 27 | elseif(N GREATER 3) 28 | add_definitions(-DMP_EN) 29 | add_definitions(-DMP_PROC_NUM=2) 30 | message("core for MP: 2") 31 | else() 32 | add_definitions(-DMP_PROC_NUM=1) 33 | endif() 34 | else() 35 | add_definitions(-DMP_PROC_NUM=1) 36 | endif() 37 | 38 | add_subdirectory(Thirdparty/Sophus) 39 | add_subdirectory(Thirdparty/tessil-src) 40 | 41 | find_package(OpenMP QUIET) 42 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 43 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 44 | 45 | find_package(PythonLibs REQUIRED) 46 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") 47 | 48 | find_package(catkin REQUIRED COMPONENTS 49 | geometry_msgs 50 | nav_msgs 51 | sensor_msgs 52 | roscpp 53 | rospy 54 | std_msgs 55 | pcl_ros 56 | tf 57 | livox_ros_driver 58 | message_generation 59 | eigen_conversions 60 | ) 61 | 62 | find_package(Eigen3 REQUIRED) 63 | find_package(PCL 1.8 REQUIRED) 64 | find_package(Ceres REQUIRED) 65 | 66 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 67 | 68 | include_directories( 69 | ../../devel/include 70 | ${catkin_INCLUDE_DIRS} 71 | ${EIGEN3_INCLUDE_DIR} 72 | ${PCL_INCLUDE_DIRS} 73 | ${PYTHON_INCLUDE_DIRS} 74 | include) 75 | 76 | add_message_files( 77 | FILES 78 | Pose6D.msg 79 | States.msg 80 | ) 81 | 82 | generate_messages( 83 | DEPENDENCIES 84 | geometry_msgs 85 | ) 86 | 87 | catkin_package( 88 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 89 | DEPENDS EIGEN3 PCL 90 | INCLUDE_DIRS 91 | ) 92 | 93 | add_executable(i2ekf_lo_mapping 94 | src/laserMapping.cpp 95 | include/ikd-Tree/ikd_Tree.cpp 96 | src/preprocess.cpp) 97 | 98 | add_dependencies(i2ekf_lo_mapping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 99 | 100 | target_link_libraries(i2ekf_lo_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${CERES_LIBRARIES} Sophus::Sophus tsl::robin_map) 101 | target_include_directories(i2ekf_lo_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

I2EKF-LO

3 | A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry 4 |
5 | IROS 2024 Oral 6 |
7 | Pipeline Image 8 |
9 |
10 | 11 | ### Introduction 12 | 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. 13 | 14 | **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) 15 | 16 | ### News 17 | * **[30/06/2024]**: I2EKF-LO is accepted to IROS 2024. 18 | * **[01/07/2024]**: We are currently working on organizing and refining the complete code. The full version will be released soon. 19 | * **[02/07/2024]**: Updated the video link and submitted the paper to arxiv. 20 | 21 | ### Related Paper 22 | 23 | Related paper available on arxiv: [I2EKF-LO: A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry](https://arxiv.org/abs/2407.02190) 24 | 25 | ### Related Video: 26 | 27 |
28 | 29 | 30 |
31 | 32 | ## 1. Prerequisites 33 | 34 | ### 1.1 **Ubuntu** and **ROS** 35 | 36 | Ubuntu >= 18.04. 37 | 38 | ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) 39 | 40 | ### 1.2. **PCL && Eigen** 41 | 42 | PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). 43 | 44 | Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). 45 | 46 | ### 1.3. **livox_ros_driver** 47 | 48 | Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). 49 | 50 | 51 | ## 2. Build 52 | 53 | Clone the repository and catkin_make: 54 | 55 | ``` 56 | cd ~/catkin_ws/src 57 | git clone https://github.com/YWL0720/I2EKF-LO 58 | cd .. 59 | catkin_make -j 60 | source devel/setup.bash 61 | ``` 62 | 63 | ## 3. Directly run 64 | 65 | ```bash 66 | cd ~/$I2EKF_LO_ROS_DIR$ 67 | source devel/setup.bash 68 | roslaunch i2ekf_lo xxx.launch 69 | ``` 70 | ## 4. Rosbag Example 71 | Download our test bags here: [HIT-TIB Datasets](https://drive.google.com/drive/folders/1L5cX9uyAiZei17oq7ELyd8WyIRReHq8u?usp=drive_link). 72 | 73 | ## 5. Acknowledgments 74 | 75 | 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). 76 | 77 | ## 6. License 78 | 79 | The source code is released under [GPLv2](http://www.gnu.org/licenses/) license. 80 | 81 | ## 7. TODO(s) 82 | - [ ] Updated video link 83 | - [ ] Upload a preprint of our paper 84 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | # BasedOnStyle: Google 3 | AccessModifierOffset: -1 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignEscapedNewlinesLeft: true 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: true 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: true 21 | BinPackParameters: true 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | ColumnLimit: 80 39 | CommentPragmas: '^ IWYU pragma:' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 41 | ConstructorInitializerIndentWidth: 4 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: true 45 | DisableFormat: false 46 | ExperimentalAutoDetectBinPacking: false 47 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 48 | IncludeCategories: 49 | - Regex: '^<.*\.h>' 50 | Priority: 1 51 | - Regex: '^<.*' 52 | Priority: 2 53 | - Regex: '.*' 54 | Priority: 3 55 | IndentCaseLabels: true 56 | IndentWidth: 2 57 | IndentWrappedFunctionNames: false 58 | KeepEmptyLinesAtTheStartOfBlocks: false 59 | MacroBlockBegin: '' 60 | MacroBlockEnd: '' 61 | MaxEmptyLinesToKeep: 1 62 | NamespaceIndentation: None 63 | ObjCBlockIndentWidth: 2 64 | ObjCSpaceAfterProperty: false 65 | ObjCSpaceBeforeProtocolList: false 66 | PenaltyBreakBeforeFirstCallParameter: 1 67 | PenaltyBreakComment: 300 68 | PenaltyBreakFirstLessLess: 120 69 | PenaltyBreakString: 1000 70 | PenaltyExcessCharacter: 1000000 71 | PenaltyReturnTypeOnItsOwnLine: 200 72 | PointerAlignment: Left 73 | ReflowComments: true 74 | SortIncludes: true 75 | SpaceAfterCStyleCast: false 76 | SpaceBeforeAssignmentOperators: true 77 | SpaceBeforeParens: ControlStatements 78 | SpaceInEmptyParentheses: false 79 | SpacesBeforeTrailingComments: 2 80 | SpacesInAngles: false 81 | SpacesInContainerLiterals: true 82 | SpacesInCStyleCastParentheses: false 83 | SpacesInParentheses: false 84 | SpacesInSquareBrackets: false 85 | Standard: Auto 86 | TabWidth: 8 87 | UseTab: Never 88 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2011-2017 Hauke Strasdat 2 | 2012-2017 Steven Lovegrove 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to 6 | deal in the Software without restriction, including without limitation the 7 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 8 | sell copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 20 | IN THE SOFTWARE. -------------------------------------------------------------------------------- /Thirdparty/Sophus/README.rst: -------------------------------------------------------------------------------- 1 | |GithubCICpp|_ windows: |AppVeyor|_ |GithubCISympy|_ |ci_cov|_ 2 | 3 | 4 | Sophus 5 | ====== 6 | 7 | Overview 8 | -------- 9 | 10 | This is a c++ implementation of Lie groups commonly used for 2d and 3d 11 | geometric problems (i.e. for Computer Vision or Robotics applications). 12 | Among others, this package includes the special orthogonal groups SO(2) and 13 | SO(3) to present rotations in 2d and 3d as well as the special Euclidean group 14 | SE(2) and SE(3) to represent rigid body transformations (i.e. rotations and 15 | translations) in 2d and 3d. 16 | 17 | API documentation: https://strasdat.github.io/Sophus/ 18 | 19 | Cross platform support 20 | ---------------------- 21 | 22 | Sophus compiles with clang and gcc on Linux and OS X as well as msvc on Windows. 23 | The specific compiler and operating system versions which are supported are 24 | the ones which are used in the Continuous Integration (CI): See GitHubCI_ and 25 | AppVeyor_ for details. 26 | 27 | However, it should work (with no to minor modification) on many other 28 | modern configurations as long they support c++14, CMake, Eigen 3.3.X and 29 | (optionally) fmt. The fmt dependency can be eliminated by passing 30 | "-DUSE_BASIC_LOGGING=ON" to cmake when configuring Sophus. 31 | 32 | .. _GitHubCI: https://github.com/strasdat/Sophus/actions 33 | 34 | .. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true 35 | .. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master 36 | 37 | .. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master 38 | .. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master 39 | 40 | .. |GithubCICpp| image:: https://github.com/strasdat/Sophus/actions/workflows/main.yml/badge.svg?branch=master 41 | .. _GithubCICpp: https://github.com/strasdat/Sophus/actions/workflows/main.yml?query=branch%3Amaster 42 | 43 | .. |GithubCISympy| image:: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml/badge.svg?branch=master 44 | .. _GithubCISympy: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml?query=branch%3Amaster 45 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/Sophus.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | } 6 | ], 7 | "settings": {} 8 | } -------------------------------------------------------------------------------- /Thirdparty/Sophus/SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include (CMakeFindDependencyMacro) 4 | 5 | @Eigen3_DEPENDENCY@ 6 | @fmt_DEPENDENCY@ 7 | 8 | include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake") 9 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/appveyor.yml: -------------------------------------------------------------------------------- 1 | branches: 2 | only: 3 | - master 4 | 5 | os: Visual Studio 2015 6 | 7 | clone_folder: c:\projects\sophus 8 | 9 | platform: x64 10 | configuration: Debug 11 | 12 | build: 13 | project: c:\projects\sophus\build\Sophus.sln 14 | 15 | install: 16 | - ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip 17 | - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul 18 | - git clone https://github.com/fmtlib/fmt.git 19 | - cd fmt 20 | - git checkout 5.3.0 21 | - mkdir build 22 | - cd build 23 | - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug .. 24 | - cmake --build . 25 | - cmake --build . --target install 26 | - cd ../.. 27 | 28 | before_build: 29 | - cd c:\projects\sophus 30 | - mkdir build 31 | - cd build 32 | - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3.4 .. 33 | 34 | after_build: 35 | - ctest --output-on-failure 36 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | # 13 | # and the following imported target: 14 | # 15 | # Eigen3::Eigen - The header-only Eigen library 16 | # 17 | # This module reads hints about search locations from 18 | # the following environment variables: 19 | # 20 | # EIGEN3_ROOT 21 | # EIGEN3_ROOT_DIR 22 | 23 | # Copyright (c) 2006, 2007 Montel Laurent, 24 | # Copyright (c) 2008, 2009 Gael Guennebaud, 25 | # Copyright (c) 2009 Benoit Jacob 26 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 27 | 28 | if(NOT Eigen3_FIND_VERSION) 29 | if(NOT Eigen3_FIND_VERSION_MAJOR) 30 | set(Eigen3_FIND_VERSION_MAJOR 2) 31 | endif() 32 | if(NOT Eigen3_FIND_VERSION_MINOR) 33 | set(Eigen3_FIND_VERSION_MINOR 91) 34 | endif() 35 | if(NOT Eigen3_FIND_VERSION_PATCH) 36 | set(Eigen3_FIND_VERSION_PATCH 0) 37 | endif() 38 | 39 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 40 | endif() 41 | 42 | macro(_eigen3_check_version) 43 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 44 | 45 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 46 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 47 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 48 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 49 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 50 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 51 | 52 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 53 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 54 | set(EIGEN3_VERSION_OK FALSE) 55 | else() 56 | set(EIGEN3_VERSION_OK TRUE) 57 | endif() 58 | 59 | if(NOT EIGEN3_VERSION_OK) 60 | 61 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 62 | "but at least version ${Eigen3_FIND_VERSION} is required") 63 | endif() 64 | endmacro() 65 | 66 | if (EIGEN3_INCLUDE_DIR) 67 | 68 | # in cache already 69 | _eigen3_check_version() 70 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 71 | set(Eigen3_FOUND ${EIGEN3_VERSION_OK}) 72 | 73 | else () 74 | 75 | # search first if an Eigen3Config.cmake is available in the system, 76 | # if successful this would set EIGEN3_INCLUDE_DIR and the rest of 77 | # the script will work as usual 78 | find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) 79 | 80 | if(NOT EIGEN3_INCLUDE_DIR) 81 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 82 | HINTS 83 | ENV EIGEN3_ROOT 84 | ENV EIGEN3_ROOT_DIR 85 | PATHS 86 | ${CMAKE_INSTALL_PREFIX}/include 87 | ${KDE4_INCLUDE_DIR} 88 | PATH_SUFFIXES eigen3 eigen 89 | ) 90 | endif() 91 | 92 | if(EIGEN3_INCLUDE_DIR) 93 | _eigen3_check_version() 94 | endif() 95 | 96 | include(FindPackageHandleStandardArgs) 97 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 98 | 99 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 100 | 101 | endif() 102 | 103 | if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen) 104 | add_library(Eigen3::Eigen INTERFACE IMPORTED) 105 | set_target_properties(Eigen3::Eigen PROPERTIES 106 | INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}") 107 | endif() 108 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/doxyfile: -------------------------------------------------------------------------------- 1 | DOXYFILE_ENCODING = UTF-8 2 | PROJECT_NAME = "Sophus" 3 | INPUT = sophus 4 | EXTRACT_ALL = YES 5 | ENABLE_PREPROCESSING = YES 6 | MACRO_EXPANSION = YES 7 | WARN_AS_ERROR = YES 8 | EXPAND_ONLY_PREDEF = NO 9 | SKIP_FUNCTION_MACROS = NO 10 | AUTOLINK_SUPPORT = YES 11 | MULTILINE_CPP_IS_BRIEF = YES 12 | MARKDOWN_SUPPORT = YES 13 | INLINE_INHERITED_MEMB = NO 14 | EXCLUDE_SYMBOLS = Eigen::internal Sophus::details Sophus::interp_details Sophus::experimental 15 | GENERATE_LATEX = NO 16 | STRIP_CODE_COMMENTS = NO 17 | 18 | GENERATE_XML = YES 19 | GENERATE_HTML = NO 20 | XML_OUTPUT = xml-dir 21 | XML_PROGRAMLISTING = NO 22 | CASE_SENSE_NAMES = NO 23 | HIDE_UNDOC_RELATIONS = YES 24 | EXTRACT_ALL = YES -------------------------------------------------------------------------------- /Thirdparty/Sophus/doxyrest-config.lua: -------------------------------------------------------------------------------- 1 | FRAME_DIR_LIST = { "doxyrest_b/doxyrest/frame/cfamily", "doxyrest_b/doxyrest/frame/common" } 2 | FRAME_FILE = "index.rst.in" 3 | INPUT_FILE = "xml-dir/index.xml" 4 | OUTPUT_FILE = "rst-dir/index.rst" 5 | INTRO_FILE = "page_index.rst" 6 | SORT_GROUPS_BY = "title" 7 | GLOBAL_AUX_COMPOUND_ID = "group_global" 8 | LANGUAGE = cpp 9 | VERBATIM_TO_CODE_BLOCK = "cpp" 10 | ESCAPE_ASTERISKS = true 11 | ESCAPE_PIPES = true 12 | ESCAPE_TRAILING_UNDERSCORES = true 13 | PROTECTION_FILTER = "protected" 14 | EXCLUDE_EMPTY_DEFINES = true 15 | EXCLUDE_DEFAULT_CONSTRUCTORS = false 16 | EXCLUDE_DESTRUCTORS = false 17 | EXCLUDE_PRIMITIVE_TYPEDEFS = true 18 | SHOW_DIRECT_DESCENDANTS = true 19 | TYPEDEF_TO_USING = true 20 | ML_PARAM_LIST_LENGTH_THRESHOLD = 80 -------------------------------------------------------------------------------- /Thirdparty/Sophus/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Tests to run 2 | SET( EXAMPLE_SOURCES HelloSO3) 3 | 4 | FOREACH(example_src ${EXAMPLE_SOURCES}) 5 | ADD_EXECUTABLE( ${example_src} ${example_src}.cpp) 6 | TARGET_LINK_LIBRARIES( ${example_src} sophus) 7 | ENDFOREACH(example_src) 8 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/examples/HelloSO3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "sophus/geometry.hpp" 3 | 4 | int main() { 5 | // The following demonstrates the group multiplication of rotation matrices 6 | 7 | // Create rotation matrices from rotations around the x and y and z axes: 8 | const double kPi = Sophus::Constants::pi(); 9 | Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4); 10 | Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6); 11 | Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3); 12 | 13 | std::cout << "The rotation matrices are" << std::endl; 14 | std::cout << "R1:\n" << R1.matrix() << std::endl; 15 | std::cout << "R2:\n" << R2.matrix() << std::endl; 16 | std::cout << "R3:\n" << R3.matrix() << std::endl; 17 | std::cout << "Their product R1*R2*R3:\n" 18 | << (R1 * R2 * R3).matrix() << std::endl; 19 | std::cout << std::endl; 20 | 21 | // Rotation matrices can act on vectors 22 | Eigen::Vector3d x; 23 | x << 0.0, 0.0, 1.0; 24 | std::cout << "Rotation matrices can act on vectors" << std::endl; 25 | std::cout << "x\n" << x << std::endl; 26 | std::cout << "R2*x\n" << R2 * x << std::endl; 27 | std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl; 28 | std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl; 29 | std::cout << std::endl; 30 | 31 | // SO(3) are internally represented as unit quaternions. 32 | std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl; 33 | std::cout << "R1 in unit quaternion form:\n" 34 | << R1.unit_quaternion().coeffs() << std::endl; 35 | // Note that the order of coefficients of Eigen's quaternion class is 36 | // (imag0, imag1, imag2, real) 37 | std::cout << std::endl; 38 | } -------------------------------------------------------------------------------- /Thirdparty/Sophus/generate_stubs.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | 3 | subprocess.run( 4 | "pybind11-stubgen sophus_pybind -o sophus_pybind-stubs/", 5 | shell=True, 6 | check=True, 7 | ) 8 | 9 | subprocess.run("touch sophus_pybind-stubs/py.typed", shell=True, check=True) 10 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/make_docs.sh: -------------------------------------------------------------------------------- 1 | doxygen doxyfile 2 | doxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua 3 | sphinx-build -b html rst-dir html-dir -------------------------------------------------------------------------------- /Thirdparty/Sophus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sophus 4 | 1.22.10 5 | 6 | C++ implementation of Lie Groups using Eigen. 7 | 8 | https://github.com/strasdat/sophus 9 | https://github.com/strasdat/sophus/issues 10 | Daniel Stonier 11 | Hauke Strasdat 12 | MIT 13 | 14 | cmake 15 | 16 | eigen 17 | eigen 18 | 19 | 20 | cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/rst-dir/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # http://www.sphinx-doc.org/en/master/config 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | import os 14 | import sys 15 | sys.path.insert(0, os.path.abspath('../sympy')) 16 | 17 | 18 | sys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx')) 19 | extensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc'] 20 | 21 | # -- Project information ----------------------------------------------------- 22 | 23 | project = 'Sophus' 24 | copyright = '2019, Hauke Strasdat' 25 | author = 'Hauke Strasdat' 26 | 27 | 28 | # Tell sphinx what the primary language being documented is. 29 | primary_domain = 'cpp' 30 | 31 | # Tell sphinx what the pygments highlight language should be. 32 | highlight_language = 'cpp' 33 | 34 | 35 | # Add any paths that contain templates here, relative to this directory. 36 | templates_path = ['_templates'] 37 | 38 | # List of patterns, relative to source directory, that match files and 39 | # directories to ignore when looking for source files. 40 | # This pattern also affects html_static_path and html_extra_path. 41 | exclude_patterns = [] 42 | 43 | 44 | # -- Options for HTML output ------------------------------------------------- 45 | 46 | # The theme to use for HTML and HTML Help pages. See the documentation for 47 | # a list of builtin themes. 48 | # 49 | html_theme = "sphinx_rtd_theme" 50 | 51 | # Add any paths that contain custom static files (such as style sheets) here, 52 | # relative to this directory. They are copied after the builtin static files, 53 | # so a file named "default.css" will overwrite the builtin "default.css". 54 | html_static_path = ['_static'] 55 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/rst-dir/page_index.rst: -------------------------------------------------------------------------------- 1 | Sophus - Lie groups for 2d/3d Geometry 2 | ======================================= 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | :caption: Contents: 7 | 8 | GitHub Page 9 | pysophus -------------------------------------------------------------------------------- /Thirdparty/Sophus/rst-dir/pysophus.rst: -------------------------------------------------------------------------------- 1 | Python API 2 | ========== 3 | 4 | .. automodule:: sophus.matrix 5 | :members: 6 | 7 | .. automodule:: sophus.complex 8 | :members: 9 | 10 | .. automodule:: sophus.quaternion 11 | :members: 12 | 13 | .. automodule:: sophus.so2 14 | :members: 15 | 16 | .. automodule:: sophus.so3 17 | :members: 18 | 19 | .. automodule:: sophus.se2 20 | :members: 21 | 22 | .. automodule:: sophus.se3 23 | :members: -------------------------------------------------------------------------------- /Thirdparty/Sophus/run_format.sh: -------------------------------------------------------------------------------- 1 | find . -type d \( -path ./sympy -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o \( -iname "*.hpp" -o -iname "*.cpp" \) -print | xargs clang-format -i 2 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/scripts/install_docs_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | sudo apt-get -qq update 7 | sudo apt-get install doxygen liblua5.3-dev ragel 8 | pip3 install 'sphinx==2.0.1' 9 | pip3 install sphinx_rtd_theme 10 | pip3 install sympy 11 | 12 | git clone https://github.com/vovkos/doxyrest_b 13 | cd doxyrest_b 14 | git reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958 15 | git submodule update --init 16 | mkdir build 17 | cd build 18 | cmake .. 19 | cmake --build . 20 | 21 | cd ../.. 22 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/scripts/install_linux_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | cmake --version 7 | 8 | sudo apt-get -qq update 9 | sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libceres-dev ccache 10 | wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2 11 | tar xvf eigen-3.3.4.tar.bz2 12 | mkdir build-eigen 13 | cd build-eigen 14 | cmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT 15 | sudo make install 16 | cd .. 17 | 18 | git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver 19 | cd ceres-solver 20 | git reset --hard b0aef211db734379319c19c030e734d6e23436b0 21 | mkdir build 22 | cd build 23 | ccache -s 24 | cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. 25 | make -j8 26 | sudo make install 27 | cd ../.. 28 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/scripts/install_linux_fmt_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | git clone https://github.com/fmtlib/fmt.git 7 | cd fmt 8 | git checkout 5.3.0 9 | mkdir build 10 | cd build 11 | cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. 12 | make -j8 13 | sudo make install 14 | cd ../.. 15 | -------------------------------------------------------------------------------- /Thirdparty/Sophus/scripts/install_osx_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | brew update 7 | brew install fmt 8 | brew install ccache 9 | 10 | # Build a specific version of ceres-solver instead of one shipped over brew 11 | curl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb 12 | patch < 4 | 5 | #include 6 | 7 | namespace Sophus { 8 | 9 | /// Templated local parameterization for LieGroup [with implemented 10 | /// LieGroup::Dx_this_mul_exp_x_at_0() ] 11 | template