├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── build_submodules.sh ├── config └── simu.yaml ├── data ├── bag │ └── simu_bag.bag ├── imu_intrinsic_gt.txt ├── lidar_intrinsic.png └── lidar_intrinsic_gt.txt ├── depend_pack.rosinstall ├── include ├── basalt │ ├── spline │ │ ├── calib_bias.hpp │ │ ├── ceres_local_param.hpp │ │ ├── ceres_spline_helper.h │ │ ├── ceres_spline_helper_jet.h │ │ ├── rd_spline.h │ │ ├── se3_spline.h │ │ ├── so3_spline.h │ │ ├── spline_common.h │ │ └── spline_segment.h │ └── utils │ │ ├── assert.h │ │ ├── eigen_utils.hpp │ │ └── sophus_utils.hpp ├── calib │ ├── calib_helper.h │ ├── calib_tool.h │ ├── inertial_initializer.h │ ├── io │ │ ├── dataset_reader.h │ │ └── segment_dataset.h │ ├── lidar_localization.h │ ├── lidar_ndt_odometry.h │ ├── ndt_registration.h │ ├── scan_undistortion.h │ └── surfel_association.h ├── factor │ └── auto_diff │ │ ├── imu_factor.h │ │ ├── lidar_feature_factor.h │ │ └── motion_factor.h ├── sensor_data │ ├── calibration.h │ ├── cloud_type.h │ ├── imu_data.h │ ├── lidar_feature.h │ ├── lidar_ouster.h │ ├── lidar_rs_16.h │ ├── lidar_vlp_16.h │ ├── lidar_vlp_points.h │ └── scaled_misaligned_imu.h ├── trajectory │ ├── se3_trajectory.h │ ├── trajectory_estimator.h │ ├── trajectory_estimator_options.h │ └── trajectory_viewer.h └── utils │ ├── ceres_callbacks.h │ ├── eigen_utils.hpp │ ├── map_evaluation_utils.h │ ├── math_utils.h │ ├── tic_toc.h │ └── yaml_utils.h ├── launch ├── li-calib.rviz └── li_calib.launch ├── msg ├── feature_cloud.msg ├── imu_array.msg └── pose_array.msg ├── package.xml ├── script ├── PlotCollection.py ├── liso_plots.py ├── plot_lidar_intrinsic_data.py └── plot_node.py └── src ├── app ├── li_calib_node.cpp └── map_evaluation_tools.cpp ├── calib ├── calib_helper.cpp ├── inertial_initializer.cpp ├── lidar_localization.cpp ├── lidar_ndt_odometry.cpp ├── scan_undistortion.cpp └── surfel_association.cpp └── trajectory ├── se3_trajectory.cpp ├── trajectory_estimator.cpp └── trajectory_viewer.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | build 3 | thirdparty/build-* 4 | 5 | # Compiled python. 6 | *.pyc 7 | *.pdf 8 | *.pcd 9 | 10 | .vscode/* 11 | 12 | # QtCreator 13 | CMakeLists.txt.user 14 | 15 | simu_bag/ 16 | 17 | launch/.vscode/ 18 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/Sophus"] 2 | path = thirdparty/Sophus 3 | url = https://github.com/strasdat/Sophus.git 4 | [submodule "thirdparty/eigen"] 5 | path = thirdparty/eigen 6 | url = https://gitlab.com/libeigen/eigen.git 7 | [submodule "thirdparty/ceres-solver"] 8 | path = thirdparty/ceres-solver 9 | url = https://github.com/ceres-solver/ceres-solver.git 10 | [submodule "thirdparty/Pangolin"] 11 | path = thirdparty/Pangolin 12 | url = https://github.com/stevenlovegrove/Pangolin.git 13 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oa_licalib) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | #SET(CXX_MARCH native) 8 | #set(CMAKE_CXX_FLAGS "-ftemplate-backtrace-limit=0 -march=${CXX_MARCH} -O3 -DBASALT_USE_CHOLMOD -DBASALT_DISABLE_ASSERTS -DNDEBUG") 9 | #set(CMAKE_CXX_FLAGS " -march=${CXX_MARCH} -O3") 10 | set(CMAKE_CXX_STANDARD 17) 11 | set(CMAKE_BUILD_TYPE "RELEASE") 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -msse4.2") 13 | #add_definitions(-O3 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 14 | 15 | find_package(OpenMP) 16 | if (OPENMP_FOUND) 17 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 18 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 19 | endif() 20 | 21 | 22 | ## Find catkin macros and libraries 23 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 24 | ## is used, also find other catkin packages 25 | # set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/" ${CMAKE_MODULE_PATH}) 26 | #set(EIGEN_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/eigen") 27 | set(CERES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/build-ceres-slover") 28 | 29 | #find_package(Ceres REQUIRED) 30 | find_package(Eigen3 REQUIRED) 31 | include_directories(${EIGEN3_INCLUDE_DIR}) 32 | set(STD_CXX_FS stdc++fs) 33 | 34 | find_package(Ceres REQUIRED) 35 | include_directories(${CERES_INCLUDE_DIRS}) 36 | 37 | message(STATUS "Found Eigen headers in: ${EIGEN3_INCLUDE_DIR}") 38 | message(STATUS "Found ceres headers in: ${CERES_INCLUDE_DIRS}") 39 | 40 | find_package(catkin REQUIRED COMPONENTS 41 | roscpp 42 | rospy 43 | geometry_msgs 44 | nav_msgs 45 | message_generation 46 | pcl_conversions 47 | pcl_ros 48 | sensor_msgs 49 | std_msgs 50 | tf 51 | eigen_conversions 52 | tf_conversions 53 | ndt_omp 54 | roslib 55 | ) 56 | 57 | find_package(OpenCV REQUIRED) 58 | include_directories(${OpenCV_INCLUDE_DIRS}) 59 | 60 | #find_package(PkgConfig REQUIRED) 61 | #pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 62 | #include_directories(${YAML_CPP_INCLUDEDIR}) 63 | find_package(yaml-cpp REQUIRED) 64 | include_directories(${YAML_INCLUDE_DIRS}) 65 | 66 | set(PANGOLIN_DIR "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/build-pangolin") 67 | find_package(Pangolin REQUIRED) 68 | 69 | ## Generate messages in the 'msg' folder 70 | add_message_files( 71 | FILES 72 | feature_cloud.msg 73 | imu_array.msg 74 | pose_array.msg 75 | ) 76 | 77 | 78 | ## Generate added messages and services with any dependencies listed here 79 | generate_messages( 80 | DEPENDENCIES 81 | std_msgs # Or other packages containing msgs 82 | sensor_msgs 83 | ) 84 | 85 | catkin_package( 86 | INCLUDE_DIRS include 87 | CATKIN_DEPENDS std_msgs sensor_msgs 88 | # LIBRARIES spline_test 89 | # CATKIN_DEPENDS other_catkin_pkg 90 | # DEPENDS system_lib 91 | ) 92 | 93 | ########### 94 | ## Build ## 95 | ########### 96 | 97 | ## Specify additional locations of header files 98 | ## Your package locations should be listed before other locations 99 | include_directories( 100 | include 101 | ${catkin_INCLUDE_DIRS} 102 | ${Pangolin_INCLUDE_DIRS} 103 | ) 104 | 105 | include_directories(thirdparty/Sophus) 106 | 107 | # Set link libraries used by all binaries 108 | list(APPEND thirdparty_libraries 109 | ${Boost_LIBRARIES} 110 | ${catkin_LIBRARIES} 111 | ${PCL_LIBRARIES} 112 | ${OpenCV_LIBS} 113 | ${YAML_CPP_LIBRARIES} 114 | ${Pangolin_LIBRARIES} 115 | ceres 116 | ) 117 | 118 | ################ Library ####################### 119 | add_library(lib_calib 120 | src/trajectory/se3_trajectory.cpp 121 | src/trajectory/trajectory_estimator.cpp 122 | src/trajectory/trajectory_viewer.cpp 123 | src/calib/lidar_localization.cpp 124 | src/calib/lidar_ndt_odometry.cpp 125 | src/calib/surfel_association.cpp 126 | src/calib/inertial_initializer.cpp 127 | src/calib/scan_undistortion.cpp 128 | ) 129 | target_link_libraries(lib_calib ${thirdparty_libraries}) 130 | add_dependencies(lib_calib ${PROJECT_NAME}_generate_messages_cpp) 131 | ################# Node ################### 132 | 133 | add_executable(li_calib_node src/app/li_calib_node.cpp src/calib/calib_helper.cpp) 134 | target_link_libraries(li_calib_node lib_calib ${thirdparty_libraries}) 135 | add_dependencies(li_calib_node ${PROJECT_NAME}_generate_messages_cpp) 136 | 137 | add_executable(map_evaluation_tools src/app/map_evaluation_tools.cpp) 138 | target_link_libraries(map_evaluation_tools ${thirdparty_libraries}) 139 | 140 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OA-LICalib: Observability-Aware Intrinsic and Extrinsic Calibration of LiDAR-IMU Systems 2 | 3 | **OA-LICalib** is a versatile and highly repeatable calibration method for the LiDAR-inertial system within a continuous-time batch-optimization framework, where the intrinsics of both sensors and the spatial-temporal extrinsics between sensors are calibrated comprehensively without explicit hand-crafted targets. To improve efficiency and cope with challenges from degenerate motions, we introduce two dedicated modules to enable observability-aware calibration. Firstly, a data selection policy based on the information-theoretic metric selects informative segments for calibration in unconscious data collection process. Secondly, an observability-aware state update mechanism in the back-end optimization is introduced to update only the identifiable directions of the calibrated parameters by leveraging truncated singular value decomposition. In this way, the proposed method can get accurate calibration results even under degenerate cases where informative enough data segments do not exist. Extensive evaluations by both simulated and real-world experiments are carried out. The results demonstrate the high accuracy and repeatability of the proposed method in common human-made scenarios and various robot platforms. 4 | 5 | ## Prerequisites 6 | 7 | - [ROS](http://wiki.ros.org/ROS/Installation) (tested with Melodic) 8 | 9 | - Others (Sophus, ceres, Pangolin have been included in the gitsubmodule) 10 | 11 | ```shell 12 | sudo apt-get install ros-melodic-velodyne-msgs 13 | sudo apt-get install libpcap-dev 14 | sudo apt-get install ccache 15 | sudo apt-get install libyaml-cpp-dev 16 | ``` 17 | 18 | ## Install 19 | 20 | ``` 21 | # init ROS workspace 22 | mkdir -p ~/catkin_oa_calib/src 23 | cd ~/catkin_oa_calib/src 24 | catkin_init_workspace 25 | 26 | # Clone the source code for the project and build it. 27 | git clone https://github.com/APRIL-ZJU/OA-LICalib.git 28 | 29 | # ndt_omp, ros_rslidar 30 | wstool init 31 | wstool merge OA-LICalib/depend_pack.rosinstall 32 | wstool update 33 | 34 | # thirdparty 35 | cd OA-LICalib 36 | ./build_submodules.sh 37 | 38 | ## build 39 | cd ../.. 40 | catkin_make -DCATKIN_WHITELIST_PACKAGES="ndt_omp" 41 | catkin_make -DCATKIN_WHITELIST_PACKAGES="" 42 | source ./devel/setup.bash 43 | ``` 44 | 45 | ## Intrinsic and Extrinsic Calibration 46 | 47 | ![](./data/lidar_intrinsic.png) 48 | 49 | The intrinsics of an individual laser comprising a multi-beam 3D LiDAR. 50 | 51 | A example to calibrate extrinsics between LiDAR and IMU while simultaneously calibrating intrinsics of both LiDAR and IMU in simulation. You can find simulated data at [`./data/bag/simu_bag.bag`]. 52 | The ground truth of intrinsics are at `[./data/bag]` and of extrinsics are as follows: 53 | 54 | ```yaml 55 | P_LinI [0.30, 0.15, 0.05] meter 56 | euler_LtoI [1.0, 2.0, 5.0] degree 57 | ``` 58 | Check the parameter `path_bag` in the `config/simu.yaml`, **change it to your absolute path**. Then run it! 59 | 60 | ```shell 61 | roslaunch oa_licalib li_calib.launch 62 | ``` 63 | 64 | After completing calibration, run the following script to check the calibration result. 65 | 66 | ```python 67 | cd script 68 | python plot_lidar_intrinsic_data.py 69 | ``` 70 | 71 | ## Observability-Aware Calibration 72 | 73 | The code of observability-awared module and the simulator would be released in the near future. 74 | 75 | ## Credits 76 | 77 | This code was developed by the [APRIL Lab](https://april.zju.edu.cn/) in Zhejiang University. For researchers that leveraged this work, please cite the 78 | following: 79 | 80 | ```txt 81 | @Conference{lv2020targetless, 82 | title={Targetless calibration of lidar-imu system based on continuous-time batch estimation}, 83 | author={Lv, Jiajun and Xu, Jinhong and Hu, Kewei and Liu, Yong and Zuo, Xingxing}, 84 | booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 85 | pages={9968--9975}, 86 | year={2020}, 87 | organization={IEEE} 88 | } 89 | @Journal{lv2022, 90 | title={{OA-LICalib}: Observability-Aware Intrinsic and Extrinsic Calibration of LiDAR-IMU Systems}, 91 | author={Jiajun Lv, Xingxing Zuo, Kewei Hu, Jinhong Xu, Guoquan Huang, and Yong Liu}, 92 | journal={IEEE Transactions on Robotics}, 93 | year={2022}, 94 | publisher={IEEE} 95 | } 96 | ``` 97 | 98 | ## License 99 | 100 | The code is provided under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt). 101 | -------------------------------------------------------------------------------- /build_submodules.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | MYPWD=$(pwd) 4 | 5 | #https://www.cnblogs.com/robinunix/p/11635560.html 6 | # set -x 7 | # set -e 8 | set -x 9 | set -e 10 | 11 | BUILD_TYPE=RelWithDebInfo 12 | 13 | if [ -n "$1" ]; then 14 | BUILD_TYPE=$1 15 | fi 16 | 17 | # https://stackoverflow.com/a/45181694 18 | NUM_CORES=`getconf _NPROCESSORS_ONLN 2>/dev/null || sysctl -n hw.ncpu || echo 1` 19 | 20 | NUM_PARALLEL_BUILDS=$NUM_CORES 21 | 22 | CXX_MARCH=native 23 | 24 | EIGEN_DIR="$MYPWD/thirdparty/eigen" 25 | 26 | COMMON_CMAKE_ARGS=( 27 | -DCMAKE_C_COMPILER_LAUNCHER=ccache 28 | -DCMAKE_CXX_COMPILER_LAUNCHER=ccache 29 | -DCMAKE_BUILD_TYPE=${BUILD_TYPE} 30 | -DCMAKE_EXPORT_NO_PACKAGE_REGISTRY=ON 31 | -DCMAKE_CXX_FLAGS="-march=$CXX_MARCH -O3 -Wno-deprecated-declarations -Wno-null-pointer-arithmetic -Wno-unknown-warning-option -Wno-unused-function" # -Wno-int-in-bool-context 32 | ) 33 | 34 | git submodule sync --recursive 35 | git submodule update --init --recursive 36 | 37 | BUILD_CERES=thirdparty/build-ceres-solver 38 | rm -rf "$BUILD_CERES" 39 | 40 | mkdir -p "$BUILD_CERES" 41 | pushd "$BUILD_CERES" 42 | cmake ../ceres-solver "${COMMON_CMAKE_ARGS[@]}" \ 43 | "-DEIGEN_INCLUDE_DIR_HINTS=$EIGEN_DIR" \ 44 | -DBUILD_EXAMPLES=OFF \ 45 | -DBUILD_TESTING=OFF \ 46 | -DEXPORT_BUILD_DIR=ON \ 47 | -DOPENMP=ON 48 | make -j$NUM_PARALLEL_BUILDS ceres 49 | popd 50 | 51 | 52 | 53 | BUILD_PANGOLIN=thirdparty/build-pangolin 54 | rm -rf "$BUILD_PANGOLIN" 55 | 56 | mkdir -p "$BUILD_PANGOLIN" 57 | pushd "$BUILD_PANGOLIN" 58 | cmake ../Pangolin 59 | make -j$NUM_PARALLEL_BUILDS 60 | popd 61 | -------------------------------------------------------------------------------- /config/simu.yaml: -------------------------------------------------------------------------------- 1 | # bag 2 | topic_lidar: /scan_sim # /velodyne_packets # 3 | topic_imu: /imu_sim 4 | 5 | LidarModel: VLP_16_SIMU 6 | vlp16_ring_case: 0 7 | 8 | scan4map: 15.0 9 | ndtResolution: 0.5 # 0.5 for indoor case and 1.0 for outdoor case 10 | ndt_key_frame_downsample: 0.1 11 | map_downsample_size: 0.25 12 | 13 | knot_distance: 0.02 14 | 15 | use_gui: false 16 | 17 | # 当 segment_num = 1时，即用一段数据标定 18 | segment_num: 1 19 | selected_segment: 20 | - {start_time: 0, end_time: 7, path_bag: /home/ha/catkin_oa_calib/src/OA-LICalib/data/bag/simu_bag.bag} 21 | 22 | 23 | # 平面运动时，旋转外参无法初始化 24 | plane_motion: false 25 | 26 | # optimization weight 27 | gyro_weight: 2.80 28 | accel_weight: 1.85 29 | lidar_weight: 1.0 30 | 31 | lock_accel_bias: false 32 | 33 | opt_timeoffset: false 34 | timeoffset_padding: 0.01 35 | 36 | opt_lidar_intrinsic: true 37 | opt_IMU_intrinsic: true 38 | 39 | iteration_num: 14 40 | 41 | # For CalibParamManager 42 | extrinsic: 43 | Trans: [0.25, 0.13, 0.07] 44 | Trans_prior: [0.25, 0.13, 0.07] 45 | 46 | Rot: [1, 0, 0, 47 | 0, 1, 0, 48 | 0, 0, 1] 49 | 50 | # Trans: [0.3, 0.15, 0.05] 51 | 52 | # gt [1, 2, 5]deg 53 | # Rot: [ 0.995588, -0.0871026, 0.0348995, 54 | # 0.0877492, 0.99599, -0.0174418, 55 | # -0.0332403, 0.0204272, 0.999239] 56 | -------------------------------------------------------------------------------- /data/bag/simu_bag.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/APRIL-ZJU/OA-LICalib/50af68042111c93cf7c1d8ad36e982ec95e125ef/data/bag/simu_bag.bag -------------------------------------------------------------------------------- /data/imu_intrinsic_gt.txt: -------------------------------------------------------------------------------- 1 | Sw0,Sw1,Sw2,Mw0,Mw1,Mw2,Sa0,Sa1,Sa2,Ma0,Ma1,Ma2 2 | 0.997381,0.998849,1.00329,-0.001274527,-0.00279483,0.00254216,0.984871,1.01851,0.979747,0.016071,0.0219687,-0.00825452 3 | -------------------------------------------------------------------------------- /data/lidar_intrinsic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/APRIL-ZJU/OA-LICalib/50af68042111c93cf7c1d8ad36e982ec95e125ef/data/lidar_intrinsic.png -------------------------------------------------------------------------------- /data/lidar_intrinsic_gt.txt: -------------------------------------------------------------------------------- 1 | dist_scale,dist_offset_mm,vert_offset_mm,horiz_offset_mm,vert_degree,delta_horiz_degree 2 | 0.99876,-13.5859,6.93198,-5.34351,15.1579,0.03697 3 | 0.99989,-6.45528,5.73541,-0.10001,12.9819,-0.02545 4 | 1.00234,0.20365,3.99366,8.79403,11.2059,0.00201 5 | 1.00021,5.51681,-0.12562,-5.71735,9.04116,0.02266 6 | 0.99909,13.2454,1.60837,7.50466,6.96988,0.01714 7 | 1.0009,18.0433,-1.84689,10.5796,4.91984,0.00358 8 | 0.99937,-2.62905,0.03405,1.87254,3.09738,-0.01826 9 | 1.00202,-0.71223,-1.63089,3.04209,0.96232,0.00366 10 | 0.99625,-2.52451,-1.82597,6.43972,-1.10433,0.00446 11 | 1.0042,1.15164,8.46108,1.37269,-2.96291,-0.00978 12 | 0.99616,-5.99829,-2.91857,0.28193,-4.98058,-0.00295 13 | 1.00046,-11.1032,-2.75417,5.72985,-6.95121,0.00221 14 | 1.00563,8.47777,1.35921,-1.89459,-9.0207,-0.03814 15 | 0.99797,1.18153,-3.10423,-4.53184,-10.8839,0.0055 16 | 1.00067,-3.26012,3.70095,-2.09108,-13.2417,0.00378 17 | 1.00018,-13.1456,0,0,-15,0 18 | -------------------------------------------------------------------------------- /depend_pack.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ndt_omp 3 | uri: https://github.com/APRIL-ZJU/ndt_omp.git 4 | 5 | - git: 6 | local-name: ros_rslidar 7 | uri: https://github.com/RoboSense-LiDAR/ros_rslidar.git 8 | 9 | -------------------------------------------------------------------------------- /include/basalt/spline/calib_bias.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Definition of static IMU biases used for calibration 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | namespace basalt { 44 | 45 | /// @brief Static calibration for accelerometer. 46 | /// 47 | /// Calibrates axis scaling and misalignment and has 9 parameters \f$ [b_x, 48 | /// b_y, b_z, s_1, s_2, s_3, s_4, s_5, s_6]^T \f$. 49 | /// \f[ 50 | /// a_c = \begin{bmatrix} s_1 + 1 & 0 & 0 \\ s_2 & s_4 + 1 & 0 \\ s_3 & s_5 & 51 | /// s_6 + 1 \\ \end{bmatrix} a_r - \begin{bmatrix} b_x \\ b_y \\ b_z 52 | /// \end{bmatrix} 53 | /// \f] where \f$ a_c \f$ is a calibrated measurement and \f$ a_r \f$ is a 54 | /// raw measurement. When all elements are zero applying calibration results in 55 | /// Identity mapping. 56 | template 57 | class CalibAccelBias { 58 | public: 59 | using Vec3 = Eigen::Matrix; 60 | using Mat33 = Eigen::Matrix; 61 | 62 | /// @brief Default constructor with zero initialization. 63 | inline CalibAccelBias() { accel_bias_full.setZero(); } 64 | 65 | /// @brief Set calibration to random values (used in unit-tests). 66 | inline void setRandom() { 67 | accel_bias_full.setRandom(); 68 | accel_bias_full.template head<3>() /= 10; 69 | accel_bias_full.template tail<6>() /= 100; 70 | } 71 | 72 | /// @brief Return const vector of parameters. 73 | /// See detailed description in \ref CalibAccelBias. 74 | inline const Eigen::Matrix& getParam() const { 75 | return accel_bias_full; 76 | } 77 | 78 | /// @brief Return vector of parameters. See detailed description in \ref 79 | /// CalibAccelBias. 80 | inline Eigen::Matrix& getParam() { return accel_bias_full; } 81 | 82 | /// @brief Increment the calibration vector 83 | /// 84 | /// @param inc increment vector 85 | inline void operator+=(const Eigen::Matrix& inc) { 86 | accel_bias_full += inc; 87 | } 88 | 89 | /// @brief Return bias vector and scale matrix. See detailed description in 90 | /// \ref CalibAccelBias. 91 | inline void getBiasAndScale(Vec3& accel_bias, Mat33& accel_scale) const { 92 | accel_bias = accel_bias_full.template head<3>(); 93 | 94 | accel_scale.setZero(); 95 | accel_scale.col(0) = accel_bias_full.template segment<3>(3); 96 | accel_scale(1, 1) = accel_bias_full(6); 97 | accel_scale(2, 1) = accel_bias_full(7); 98 | accel_scale(2, 2) = accel_bias_full(8); 99 | } 100 | 101 | /// @brief Calibrate the measurement. See detailed description in 102 | /// \ref CalibAccelBias. 103 | /// 104 | /// @param raw_measurement 105 | /// @return calibrated measurement 106 | inline Vec3 getCalibrated(const Vec3& raw_measurement) const { 107 | Vec3 accel_bias; 108 | Mat33 accel_scale; 109 | 110 | getBiasAndScale(accel_bias, accel_scale); 111 | 112 | return (raw_measurement + accel_scale * raw_measurement - accel_bias); 113 | } 114 | 115 | /// @brief Invert calibration (used in unit-tests). 116 | /// 117 | /// @param calibrated_measurement 118 | /// @return raw measurement 119 | inline Vec3 invertCalibration(const Vec3& calibrated_measurement) const { 120 | Vec3 accel_bias; 121 | Mat33 accel_scale; 122 | 123 | getBiasAndScale(accel_bias, accel_scale); 124 | 125 | Mat33 accel_scale_inv = 126 | (Eigen::Matrix3d::Identity() + accel_scale).inverse(); 127 | 128 | return accel_scale_inv * (calibrated_measurement + accel_bias); 129 | } 130 | 131 | private: 132 | Eigen::Matrix accel_bias_full; 133 | }; 134 | 135 | /// @brief Static calibration for gyroscope. 136 | /// 137 | /// Calibrates rotation, axis scaling and misalignment and has 12 parameters \f$ 138 | /// [b_x, b_y, b_z, s_1, s_2, s_3, s_4, s_5, s_6, s_7, s_8, s_9]^T \f$. \f[ 139 | /// \omega_c = \begin{bmatrix} s_1 + 1 & s_4 & s_7 \\ s_2 & s_5 + 1 & s_8 \\ s_3 140 | /// & s_6 & s_9 +1 \\ \end{bmatrix} \omega_r - \begin{bmatrix} b_x \\ b_y 141 | /// \\ b_z \end{bmatrix} \f] where \f$ \omega_c \f$ is a calibrated measurement 142 | /// and \f$ \omega_r \f$ is a raw measurement. When all elements are zero 143 | /// applying calibration results in Identity mapping. 144 | template 145 | class CalibGyroBias { 146 | public: 147 | using Vec3 = Eigen::Matrix; 148 | using Mat33 = Eigen::Matrix; 149 | 150 | /// @brief Default constructor with zero initialization. 151 | inline CalibGyroBias() { gyro_bias_full.setZero(); } 152 | 153 | /// @brief Set calibration to random values (used in unit-tests). 154 | inline void setRandom() { 155 | gyro_bias_full.setRandom(); 156 | gyro_bias_full.template head<3>() /= 10; 157 | gyro_bias_full.template tail<9>() /= 100; 158 | } 159 | 160 | /// @brief Return const vector of parameters. 161 | /// See detailed description in \ref CalibGyroBias. 162 | inline const Eigen::Matrix& getParam() const { 163 | return gyro_bias_full; 164 | } 165 | 166 | /// @brief Return vector of parameters. 167 | /// See detailed description in \ref CalibGyroBias. 168 | inline Eigen::Matrix& getParam() { return gyro_bias_full; } 169 | 170 | /// @brief Increment the calibration vector 171 | /// 172 | /// @param inc increment vector 173 | inline void operator+=(const Eigen::Matrix& inc) { 174 | gyro_bias_full += inc; 175 | } 176 | 177 | /// @brief Return bias vector and scale matrix. See detailed description in 178 | /// \ref CalibGyroBias. 179 | inline void getBiasAndScale(Vec3& gyro_bias, Mat33& gyro_scale) const { 180 | gyro_bias = gyro_bias_full.template head<3>(); 181 | gyro_scale.col(0) = gyro_bias_full.template segment<3>(3); 182 | gyro_scale.col(1) = gyro_bias_full.template segment<3>(6); 183 | gyro_scale.col(2) = gyro_bias_full.template segment<3>(9); 184 | } 185 | 186 | /// @brief Calibrate the measurement. See detailed description in 187 | /// \ref CalibGyroBias. 188 | /// 189 | /// @param raw_measurement 190 | /// @return calibrated measurement 191 | inline Vec3 getCalibrated(const Vec3& raw_measurement) const { 192 | Vec3 gyro_bias; 193 | Mat33 gyro_scale; 194 | 195 | getBiasAndScale(gyro_bias, gyro_scale); 196 | 197 | return (raw_measurement + gyro_scale * raw_measurement - gyro_bias); 198 | } 199 | 200 | /// @brief Invert calibration (used in unit-tests). 201 | /// 202 | /// @param calibrated_measurement 203 | /// @return raw measurement 204 | inline Vec3 invertCalibration(const Vec3& calibrated_measurement) const { 205 | Vec3 gyro_bias; 206 | Mat33 gyro_scale; 207 | 208 | getBiasAndScale(gyro_bias, gyro_scale); 209 | 210 | Mat33 gyro_scale_inv = (Eigen::Matrix3d::Identity() + gyro_scale).inverse(); 211 | 212 | return gyro_scale_inv * (calibrated_measurement + gyro_bias); 213 | } 214 | 215 | private: 216 | Eigen::Matrix gyro_bias_full; 217 | }; 218 | 219 | } // namespace basalt 220 | -------------------------------------------------------------------------------- /include/basalt/spline/ceres_local_param.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Generic local parametrization for Sophus Lie group types to be used with 37 | ceres. 38 | */ 39 | 40 | /** 41 | File adapted from Sophus 42 | 43 | Copyright 2011-2017 Hauke Strasdat 44 | 2012-2017 Steven Lovegrove 45 | 46 | Permission is hereby granted, free of charge, to any person obtaining a copy 47 | of this software and associated documentation files (the "Software"), to 48 | deal in the Software without restriction, including without limitation the 49 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 50 | sell copies of the Software, and to permit persons to whom the Software is 51 | furnished to do so, subject to the following conditions: 52 | 53 | The above copyright notice and this permission notice shall be included in 54 | all copies or substantial portions of the Software. 55 | 56 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 57 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 58 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 59 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 60 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 61 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 62 | IN THE SOFTWARE. 63 | */ 64 | 65 | #pragma once 66 | 67 | #include 68 | #include 69 | 70 | namespace basalt { 71 | 72 | /// @brief Local parametrization for ceres that can be used with Sophus Lie 73 | /// group implementations. 74 | template 75 | class LieLocalParameterization : public ceres::LocalParameterization { 76 | public: 77 | virtual ~LieLocalParameterization() {} 78 | 79 | using Tangentd = typename Groupd::Tangent; 80 | 81 | /// @brief plus operation for Ceres 82 | /// 83 | /// T * exp(x) 84 | /// 85 | virtual bool Plus(double const* T_raw, double const* delta_raw, 86 | double* T_plus_delta_raw) const { 87 | Eigen::Map const T(T_raw); 88 | Eigen::Map const delta(delta_raw); 89 | Eigen::Map T_plus_delta(T_plus_delta_raw); 90 | T_plus_delta = T * Groupd::exp(delta); 91 | return true; 92 | } 93 | 94 | ///@brief Jacobian of plus operation for Ceres 95 | /// 96 | /// Dx T * exp(x) with x=0 97 | /// 98 | virtual bool ComputeJacobian(double const* T_raw, 99 | double* jacobian_raw) const { 100 | Eigen::Map T(T_raw); 101 | Eigen::Map> 103 | jacobian(jacobian_raw); 104 | jacobian = T.Dx_this_mul_exp_x_at_0().transpose(); 105 | return true; 106 | } 107 | 108 | ///@brief Global size 109 | virtual int GlobalSize() const { return Groupd::num_parameters; } 110 | 111 | ///@brief Local size 112 | virtual int LocalSize() const { return Groupd::DoF; } 113 | }; 114 | 115 | } // namespace basalt 116 | -------------------------------------------------------------------------------- /include/basalt/spline/ceres_spline_helper.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Helper for implementing Lie group and Euclidean b-splines in ceres. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | 44 | namespace basalt { 45 | 46 | /// @brief Helper for implementing Lie group and Euclidean b-splines in ceres of 47 | /// order N 48 | /// 49 | /// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details. 50 | template 51 | struct CeresSplineHelper { 52 | static constexpr int N = _N; // Order of the spline. 53 | static constexpr int DEG = _N - 1; // Degree of the spline. 54 | 55 | using MatN = Eigen::Matrix; 56 | using VecN = Eigen::Matrix; 57 | 58 | static const MatN blending_matrix_; 59 | static const MatN cumulative_blending_matrix_; 60 | static const MatN base_coefficients_; 61 | 62 | /// @brief Vector of derivatives of time polynomial. 63 | /// 64 | /// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots & 65 | /// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first 66 | /// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1) 67 | /// t^{N-2}\end{bmatrix} \f$. 68 | /// @param Derivative derivative to evaluate 69 | /// @param[out] res_const vector to store the result 70 | /// @param[in] t 71 | template 72 | static inline void baseCoeffsWithTime( 73 | const Eigen::MatrixBase& res_const, double t) { 74 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N); 75 | Eigen::MatrixBase& res = 76 | const_cast&>(res_const); 77 | 78 | res.setZero(); 79 | 80 | if (Derivative < N) { 81 | res[Derivative] = base_coefficients_(Derivative, Derivative); 82 | 83 | double _t = t; 84 | for (int j = Derivative + 1; j < N; j++) { 85 | res[j] = base_coefficients_(Derivative, j) * _t; 86 | _t = _t * t; 87 | } 88 | } 89 | } 90 | 91 | /// @brief Evaluate Lie group cummulative B-spline and time derivatives. 92 | /// 93 | /// @param[in] sKnots array of pointers of the spline knots. The size of each 94 | /// knot should be GroupT::num_parameters: 4 for SO(3) and 7 for SE(3). 95 | /// @param[in] u normalized time to compute value of the spline 96 | /// @param[in] inv_dt inverse of the time spacing in seconds between spline 97 | /// knots 98 | /// @param[out] transform_out if not nullptr return the value of the spline 99 | /// @param[out] vel_out if not nullptr velocity (first time derivative) in the 100 | /// body frame 101 | /// @param[out] accel_out if not nullptr acceleration (second time derivative) 102 | /// in the body frame 103 | template class GroupT> 104 | static inline void evaluate_lie( 105 | T const* const* sKnots, const double u, const double inv_dt, 106 | GroupT* transform_out = nullptr, 107 | typename GroupT::Tangent* vel_out = nullptr, 108 | typename GroupT::Tangent* accel_out = nullptr, 109 | typename GroupT::Tangent* jerk_out = nullptr) { 110 | using Group = GroupT; 111 | using Tangent = typename GroupT::Tangent; 112 | using Adjoint = typename GroupT::Adjoint; 113 | 114 | VecN p, coeff, dcoeff, ddcoeff, dddcoeff; 115 | 116 | CeresSplineHelper::template baseCoeffsWithTime<0>(p, u); 117 | coeff = CeresSplineHelper::cumulative_blending_matrix_ * p; 118 | 119 | if (vel_out || accel_out || jerk_out) { 120 | CeresSplineHelper::template baseCoeffsWithTime<1>(p, u); 121 | dcoeff = inv_dt * CeresSplineHelper::cumulative_blending_matrix_ * p; 122 | 123 | if (accel_out || jerk_out) { 124 | CeresSplineHelper::template baseCoeffsWithTime<2>(p, u); 125 | ddcoeff = inv_dt * inv_dt * 126 | CeresSplineHelper::cumulative_blending_matrix_ * p; 127 | 128 | if (jerk_out) { 129 | CeresSplineHelper::template baseCoeffsWithTime<3>(p, u); 130 | dddcoeff = inv_dt * inv_dt * inv_dt * 131 | CeresSplineHelper::cumulative_blending_matrix_ * p; 132 | } 133 | } 134 | } 135 | 136 | if (transform_out) { 137 | Eigen::Map const p00(sKnots[0]); 138 | *transform_out = p00; 139 | } 140 | 141 | Tangent rot_vel, rot_accel, rot_jerk; 142 | 143 | if (vel_out || accel_out || jerk_out) rot_vel.setZero(); 144 | if (accel_out || jerk_out) rot_accel.setZero(); 145 | if (jerk_out) rot_jerk.setZero(); 146 | 147 | for (int i = 0; i < DEG; i++) { 148 | Eigen::Map const p0(sKnots[i]); 149 | Eigen::Map const p1(sKnots[i + 1]); 150 | 151 | Group r01 = p0.inverse() * p1; 152 | Tangent delta = r01.log(); 153 | 154 | Group exp_kdelta = Group::exp(delta * coeff[i + 1]); 155 | 156 | if (transform_out) (*transform_out) *= exp_kdelta; 157 | 158 | if (vel_out || accel_out || jerk_out) { 159 | Adjoint A = exp_kdelta.inverse().Adj(); 160 | 161 | rot_vel = A * rot_vel; 162 | Tangent rot_vel_current = delta * dcoeff[i + 1]; 163 | rot_vel += rot_vel_current; 164 | 165 | if (accel_out || jerk_out) { 166 | rot_accel = A * rot_accel; 167 | Tangent accel_lie_bracket = 168 | Group::lieBracket(rot_vel, rot_vel_current); 169 | rot_accel += ddcoeff[i + 1] * delta + accel_lie_bracket; 170 | 171 | if (jerk_out) { 172 | rot_jerk = A * rot_jerk; 173 | rot_jerk += dddcoeff[i + 1] * delta + 174 | Group::lieBracket(ddcoeff[i + 1] * rot_vel + 175 | 2 * dcoeff[i + 1] * rot_accel - 176 | dcoeff[i + 1] * accel_lie_bracket, 177 | delta); 178 | } 179 | } 180 | } 181 | } 182 | 183 | if (vel_out) *vel_out = rot_vel; 184 | if (accel_out) *accel_out = rot_accel; 185 | if (jerk_out) *jerk_out = rot_jerk; 186 | } 187 | 188 | /// @brief Evaluate Euclidean B-spline or time derivatives. 189 | /// 190 | /// @param[in] sKnots array of pointers of the spline knots. The size of each 191 | /// knot should be DIM. 192 | /// @param[in] u normalized time to compute value of the spline 193 | /// @param[in] inv_dt inverse of the time spacing in seconds between spline 194 | /// knots 195 | /// @param[out] vec_out if DERIV=0 returns value of the spline, otherwise 196 | /// corresponding derivative. 197 | template 198 | static inline void evaluate(T const* const* sKnots, const double u, 199 | const double inv_dt, 200 | Eigen::Matrix* vec_out) { 201 | if (!vec_out) return; 202 | 203 | using VecD = Eigen::Matrix; 204 | 205 | VecN p, coeff; 206 | 207 | CeresSplineHelper::template baseCoeffsWithTime(p, u); 208 | coeff = 209 | std::pow(inv_dt, DERIV) * CeresSplineHelper::blending_matrix_ * p; 210 | 211 | vec_out->setZero(); 212 | 213 | for (int i = 0; i < N; i++) { 214 | Eigen::Map const p(sKnots[i]); 215 | 216 | (*vec_out) += coeff[i] * p; 217 | } 218 | } 219 | }; 220 | 221 | template 222 | const typename CeresSplineHelper<_N>::MatN 223 | CeresSplineHelper<_N>::base_coefficients_ = 224 | basalt::computeBaseCoefficients<_N, double>(); 225 | 226 | template 227 | const typename CeresSplineHelper<_N>::MatN 228 | CeresSplineHelper<_N>::blending_matrix_ = 229 | basalt::computeBlendingMatrix<_N, double, false>(); 230 | 231 | template 232 | const typename CeresSplineHelper<_N>::MatN 233 | CeresSplineHelper<_N>::cumulative_blending_matrix_ = 234 | basalt::computeBlendingMatrix<_N, double, true>(); 235 | 236 | } // namespace basalt 237 | -------------------------------------------------------------------------------- /include/basalt/spline/ceres_spline_helper_jet.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Helper for implementing Lie group and Euclidean b-splines in ceres. 37 | Based on Jet type 38 | */ 39 | 40 | #pragma once 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | namespace basalt { 47 | 48 | /// @brief Helper for implementing Lie group and Euclidean b-splines in ceres of 49 | /// order N 50 | /// 51 | /// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details. 52 | template 53 | struct CeresSplineHelperJet { 54 | static constexpr int N = _N; // Order of the spline. 55 | static constexpr int DEG = _N - 1; // Degree of the spline. 56 | 57 | using MatN = Eigen::Matrix; 58 | using VecN = Eigen::Matrix; 59 | 60 | static const MatN blending_matrix_; 61 | static const MatN cumulative_blending_matrix_; 62 | static const MatN base_coefficients_; 63 | 64 | /// @brief Vector of derivatives of time polynomial. 65 | /// 66 | /// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots & 67 | /// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first 68 | /// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1) 69 | /// t^{N-2}\end{bmatrix} \f$. 70 | /// @param Derivative derivative to evaluate 71 | /// @param[out] res_const vector to store the result 72 | /// @param[in] t 73 | template 74 | static inline void baseCoeffsWithTime( 75 | const Eigen::MatrixBase& res_const, T t) { 76 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N); 77 | Eigen::MatrixBase& res = 78 | const_cast&>(res_const); 79 | 80 | res.setZero(); 81 | 82 | if (Derivative < N) { 83 | res[Derivative] = base_coefficients_(Derivative, Derivative); 84 | 85 | T _t = t; 86 | for (int j = Derivative + 1; j < N; j++) { 87 | res[j] = base_coefficients_(Derivative, j) * _t; 88 | _t = _t * t; 89 | } 90 | } 91 | } 92 | 93 | /// @brief Evaluate Lie group cummulative B-spline and time derivatives. 94 | /// 95 | /// @param[in] sKnots array of pointers of the spline knots. The size of each 96 | /// knot should be GroupT::num_parameters: 4 for SO(3) and 7 for SE(3). 97 | /// @param[in] u normalized time to compute value of the spline 98 | /// @param[in] inv_dt inverse of the time spacing in seconds between spline 99 | /// knots 100 | /// @param[out] transform_out if not nullptr return the value of the spline 101 | /// @param[out] vel_out if not nullptr velocity (first time derivative) in the 102 | /// body frame 103 | /// @param[out] accel_out if not nullptr acceleration (second time derivative) 104 | /// in the body frame 105 | template