├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── data ├── C_T_L.txt ├── C_T_L_concatanated_2022_01_28_15_06_51.txt ├── C_T_L_concatanated_2022_01_28_15_11_20.txt ├── C_T_L_concatanated_2022_01_28_15_15_06.txt ├── C_T_L_concatanated_2022_01_28_15_20_21.txt ├── C_T_L_concatanated_2022_01_28_15_30_47.txt ├── C_T_L_final.txt ├── C_T_L_final_0.txt ├── C_T_L_final_1.txt ├── C_T_L_final_2.txt ├── C_T_L_final_3_kalibr.txt ├── I_T_C_final.txt ├── I_T_C_init.txt ├── I_T_C_kalibr.txt ├── I_T_L_final.txt ├── I_T_L_init.txt ├── cam_imu_results │ ├── I_T_C_2022_01_28_15_06_51.txt │ ├── I_T_C_2022_01_28_15_11_20.txt │ ├── I_T_C_2022_01_28_15_15_06.txt │ ├── I_T_C_2022_01_28_15_20_21.txt │ ├── I_T_C_2022_01_28_15_30_47.txt │ ├── mocalResidual │ │ ├── I_T_C_2022_01_28_15_06_51.txt │ │ ├── I_T_C_2022_01_28_15_11_20.txt │ │ ├── I_T_C_2022_01_28_15_15_06.txt │ │ ├── I_T_C_2022_01_28_15_20_21.txt │ │ └── I_T_C_2022_01_28_15_30_47.txt │ └── repErrResidual │ │ ├── I_T_C_2022_01_28_15_06_51.txt │ │ ├── I_T_C_2022_01_28_15_11_20.txt │ │ ├── I_T_C_2022_01_28_15_15_06.txt │ │ ├── I_T_C_2022_01_28_15_20_21.txt │ │ └── I_T_C_2022_01_28_15_30_47.txt ├── camera_imu_calib_dt.csv ├── camera_imu_calib_extrinsics.csv ├── camera_imu_lidar_results │ ├── C_T_L_final_2022_01_28_15_06_51.txt │ ├── C_T_L_final_2022_01_28_15_11_20.txt │ ├── C_T_L_final_2022_01_28_15_15_06.txt │ ├── C_T_L_final_2022_01_28_15_20_21.txt │ ├── C_T_L_final_2022_01_28_15_30_47.txt │ ├── I_T_C_final_2022_01_28_15_06_51.txt │ ├── I_T_C_final_2022_01_28_15_11_20.txt │ ├── I_T_C_final_2022_01_28_15_15_06.txt │ ├── I_T_C_final_2022_01_28_15_20_21.txt │ ├── I_T_C_final_2022_01_28_15_30_47.txt │ ├── I_T_L_final_2022_01_28_15_06_51.txt │ ├── I_T_L_final_2022_01_28_15_11_20.txt │ ├── I_T_L_final_2022_01_28_15_15_06.txt │ ├── I_T_L_final_2022_01_28_15_20_21.txt │ └── I_T_L_final_2022_01_28_15_30_47.txt ├── compareITC.asv ├── compareITC.m ├── compareWithKalibr.m ├── daisyChainTF.m ├── differencebwIndividualAndJointCalibration.m ├── displayAllITC.m ├── imu_bias.csv ├── imu_trajectory.csv ├── imu_velocity.csv ├── kalibrResults.m ├── kalibr_results │ ├── I_T_C_kalibr_2022_01_28_15_06_51.txt │ ├── I_T_C_kalibr_2022_01_28_15_11_20.txt │ ├── I_T_C_kalibr_2022_01_28_15_15_06.txt │ ├── I_T_C_kalibr_2022_01_28_15_20_21.txt │ └── I_T_C_kalibr_2022_01_28_15_30_47.txt ├── lidar_imu_calib_dt.csv ├── lidar_imu_calib_extrinsics.csv ├── lidar_imu_results │ ├── I_T_L_2022_01_28_15_06_51.txt │ ├── I_T_L_2022_01_28_15_11_20.txt │ ├── I_T_L_2022_01_28_15_15_06.txt │ ├── I_T_L_2022_01_28_15_20_21.txt │ └── I_T_L_2022_01_28_15_30_47.txt ├── lo_trajectory.csv ├── plotBias.m ├── plotCalib_camera_imu.m ├── plotCalib_lidar_imu.m ├── plotResidual_cameraimulidar.m ├── plotTrajectory.m ├── plotVelocity.m ├── pylon_calib.yaml ├── residual_cameraimulidar.csv └── resultsfromcameraimucalibration.m ├── launch ├── camera_imu_lidar_calibration.launch ├── camera_imu_lidar_calibration_bagcreater.launch ├── ros_calib_init.launch └── ros_ndt_lodom.launch ├── package.xml └── src ├── core ├── calibManager.cpp ├── calibManager.h └── calibManagerOptions.h ├── init ├── InertialInitializer.cpp └── InertialInitializer.h ├── relpose └── relativePose.h ├── ros_camera_imu_lidar_calibration_bag_generator.cpp ├── ros_camera_imu_lidar_calibration_test.cpp ├── state ├── Propagator.cpp ├── Propagator.h ├── State.cpp ├── State.h ├── StateHelper.cpp ├── StateHelper.h └── StateOptions.h ├── track ├── cameraPoseTracking.cpp ├── cameraPoseTracking.h ├── lidarOdometry.cpp ├── lidarOdometry.h ├── lidarPlaneDetector.cpp └── lidarPlaneDetector.h ├── types ├── IMU.h ├── Pose.h ├── Quat.h ├── Type.h └── Vec.h ├── update ├── UpdaterCameraLidarConstraint.cpp ├── UpdaterCameraLidarConstraint.h ├── UpdaterCameraTracking.cpp ├── UpdaterCameraTracking.h ├── UpdaterLidarOdometry.cpp ├── UpdaterLidarOdometry.h └── UpdaterOptions.h └── utils ├── color.h ├── eigen_utils.h ├── math_utils.h ├── parse_ros.h ├── pcl_utils.h └── quat_ops.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | *.idea/ 35 | *cmake-build-debug 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | # Project name 3 | project(camera_imu_lidar_calibration) 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_BUILD_TYPE "RELEASE") 6 | #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native ") 7 | #set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O1") 8 | find_package(OpenMP) 9 | if (OPENMP_FOUND) 10 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 11 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 12 | endif() 13 | 14 | # Find catkin (the ROS build system) 15 | find_package(catkin QUIET COMPONENTS 16 | roscpp 17 | rosbag 18 | std_msgs 19 | geometry_msgs 20 | sensor_msgs 21 | nav_msgs 22 | ndt_omp 23 | tf 24 | imuPacket 25 | pcl_ros 26 | pcl_conversions 27 | cv_bridge 28 | ) 29 | 30 | # Include libraries 31 | find_package(Eigen3 REQUIRED) 32 | find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) 33 | find_package(OpenCV REQUIRED) 34 | 35 | #link_directories(${PCL_LIBRARY_DIRS}) 36 | #add_definitions(${PCL_DEFINITIONS}) 37 | 38 | # display message to user 39 | message(STATUS "EIGEN VERSION: " ${EIGEN3_VERSION}) 40 | message(STATUS "BOOST VERSION: " ${Boost_VERSION}) 41 | 42 | # Describe catkin project 43 | if (catkin_FOUND) 44 | add_definitions(-DROS_AVAILABLE=1) 45 | catkin_package( 46 | CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs imuPacket 47 | INCLUDE_DIRS src 48 | LIBRARIES camera_imu_lidar_calibration_lib 49 | ) 50 | else() 51 | message(WARNING "CATKIN NOT FOUND BUILDING WITHOUT ROS!") 52 | endif() 53 | 54 | # Include our header files 55 | include_directories( 56 | src 57 | ${EIGEN3_INCLUDE_DIR} 58 | ${Boost_INCLUDE_DIRS} 59 | ${catkin_INCLUDE_DIRS} 60 | ${OpenCV_INCLUDE_DIRS} 61 | ) 62 | 63 | # Set link libraries used by all binaries 64 | list(APPEND thirdparty_libraries 65 | ${Boost_LIBRARIES} 66 | ${catkin_LIBRARIES} 67 | ${PCL_LIBRARIES} 68 | ${OpenCV_LIBRARIES} 69 | # gtsam 70 | ) 71 | 72 | ################################################## 73 | # Make the core library 74 | ################################################## 75 | add_library(camera_imu_lidar_calibration_lib SHARED 76 | src/init/InertialInitializer.cpp 77 | src/state/State.cpp 78 | src/state/StateHelper.cpp 79 | src/state/Propagator.cpp 80 | src/core/calibManager.cpp 81 | src/update/UpdaterLidarOdometry.cpp 82 | src/update/UpdaterCameraTracking.cpp 83 | src/track/lidarOdometry.cpp 84 | src/track/cameraPoseTracking.cpp 85 | src/track/lidarPlaneDetector.cpp 86 | src/update/UpdaterCameraLidarConstraint.cpp 87 | ) 88 | target_link_libraries(camera_imu_lidar_calibration_lib ${thirdparty_libraries}) 89 | target_include_directories(camera_imu_lidar_calibration_lib PUBLIC src) 90 | 91 | ################################################## 92 | # Adding different executables 93 | ################################################## 94 | add_executable(ros_test_camera_imu_lidar_calibration_node 95 | src/ros_camera_imu_lidar_calibration_test.cpp) 96 | target_link_libraries(ros_test_camera_imu_lidar_calibration_node 97 | camera_imu_lidar_calibration_lib ${catkin_LIBRARIES}) 98 | 99 | add_executable(ros_camera_imu_lidar_calibration_bag_generator_node src/ros_camera_imu_lidar_calibration_bag_generator.cpp) 100 | target_link_libraries(ros_camera_imu_lidar_calibration_bag_generator_node ${catkin_LIBRARIES}) 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2023, Unmanned Systems Lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Tool for Joint Calibration of a LiDAR-IMU-Camera Systems. 2 | More details in [Extrinsic Calibration of LiDAR, IMU and Camera](https://arxiv.org/abs/2205.08701) 3 | 4 | # Instructions 5 | Coming soon. 6 | -------------------------------------------------------------------------------- /data/C_T_L.txt: -------------------------------------------------------------------------------- 1 | 0.0273398108155789,-0.999573534460788,0.0103053164135603,-0.259648686004306 2 | 0.00880184506910196,-0.0100679968216903,-0.999909856339434,-0.00138601834141441 3 | 0.999588295623842,0.0274280495471715,0.00852279219155126,-0.0619990845298129 4 | 0,0,0,1 5 | -------------------------------------------------------------------------------- /data/C_T_L_concatanated_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.041229 -0.99914 0.0043375 -0.25717 2 | 0.0013503 -0.0042855 -0.99999 -0.020953 3 | 0.99915 0.041235 0.0011725 -0.058183 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/C_T_L_concatanated_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.047263 -0.99888 0.00019485 -0.25645 2 | 0.013561 0.00044668 -0.99991 -0.011408 3 | 0.99879 0.047261 0.013567 -0.062981 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/C_T_L_concatanated_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.024669 -0.99969 0.002452 -0.25561 2 | 0.015724 -0.0020644 -0.99987 0.0015189 3 | 0.99957 0.024705 0.015668 -0.06577 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/C_T_L_concatanated_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.02734 -0.99957 0.010305 -0.25965 2 | 0.0088018 -0.010068 -0.99991 -0.001386 3 | 0.99959 0.027428 0.0085228 -0.061999 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/C_T_L_concatanated_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.018645 -0.99983 0.00059461 -0.25241 2 | 0.010712 -0.00039487 -0.99994 -0.0043508 3 | 0.99977 0.01865 0.010703 -0.067418 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/C_T_L_final.txt: -------------------------------------------------------------------------------- 1 | 0.0263394 -0.999653 -0.000827361 -0.225819 2 | -0.0173262 0.000371004 -0.99985 0.0134575 3 | 0.999503 0.0263498 -0.0173104 -0.0684869 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/C_T_L_final_0.txt: -------------------------------------------------------------------------------- 1 | 0.0355114 -0.998989 0.0275562 -0.266546 2 | 0.0727538 -0.0249162 -0.997039 -0.02758 3 | 0.996718 0.0374111 0.0717955 -0.0187939 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/C_T_L_final_1.txt: -------------------------------------------------------------------------------- 1 | 0.0230028 -0.999579 0.0177096 -0.264768 2 | -0.0132962 -0.0180186 -0.999749 0.00660157 3 | 0.999647 0.0227615 -0.013705 -0.0460839 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/C_T_L_final_2.txt: -------------------------------------------------------------------------------- 1 | 0.02734 -0.99957 0.010305 -0.25965 2 | 0.0088018 -0.010068 -0.99991 -0.001386 3 | 0.99959 0.027428 0.0085228 -0.061999 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/C_T_L_final_3_kalibr.txt: -------------------------------------------------------------------------------- 1 | 0.030226 -0.99949 0.010487 -0.26211 2 | 0.0093919 -0.010207 -0.9999 -0.0067636 3 | 0.9995 0.030321 0.0090785 -0.064625 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/I_T_C_final.txt: -------------------------------------------------------------------------------- 1 | -0.0225949 -0.00538159 0.99973 0.0237562 2 | 0.999628 0.0151729 0.0226742 0.0747855 3 | -0.0152909 0.99987 0.00503676 -0.106496 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/I_T_C_init.txt: -------------------------------------------------------------------------------- 1 | 0 0 1 0 2 | 1 0 0 0 3 | 0 1 0 0 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/I_T_C_kalibr.txt: -------------------------------------------------------------------------------- 1 | 0.016495 0.014597 0.99976 0.072727 2 | 0.99968 0.01883 -0.016768 0.12542 3 | -0.01907 0.99972 -0.014282 -0.075599 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/I_T_L_final.txt: -------------------------------------------------------------------------------- 1 | 0.998731 0.0489277 -0.0119063 -0.0396823 2 | 0.0487297 -0.998678 -0.0163902 -0.152299 3 | -0.0126925 0.0157892 -0.999795 -0.0899327 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/I_T_L_init.txt: -------------------------------------------------------------------------------- 1 | 0.99985 0.0114737 0.0129375 0 2 | 0.0113827 -0.99991 0.00707979 0 3 | 0.0130176 -0.00693146 -0.999891 0 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/cam_imu_results/I_T_C_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.0183719 0.0162367 0.999699 0.0725803 2 | 0.999652 0.0186064 -0.0186733 0.124892 3 | -0.018904 0.999695 -0.0158892 -0.0776702 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/I_T_C_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.0161057 0.0142213 0.999769 0.0721124 2 | 0.999705 0.0179344 -0.0163598 0.12704 3 | -0.0181629 0.999738 -0.0139283 -0.0804582 4 | 0 0 0 1 5 | 6 | -------------------------------------------------------------------------------- /data/cam_imu_results/I_T_C_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.017914 0.0143767 0.999736 0.0714918 2 | 0.999673 0.0179709 -0.0181713 0.125221 3 | -0.0182274 0.999735 -0.0140501 -0.0810866 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/I_T_C_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.0150982 0.0141827 0.999785 0.0697867 2 | 0.999711 0.018481 -0.0153593 0.123535 3 | -0.0186948 0.999729 -0.0138996 -0.0799106 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/I_T_C_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.0162378 0.0176152 0.999713 0.0679443 2 | 0.999689 0.0186145 -0.0165654 0.125258 3 | -0.0189009 0.999672 -0.0173075 -0.07712 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.000564479 0.0203764 0.999792 0.059607 2 | 0.999666 0.0258281 -0.0010908 0.130148 3 | -0.025845 0.999459 -0.020355 -0.0776416 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.00131312 -0.0152583 0.999883 0.0639834 2 | 0.999847 0.0174673 -0.00104652 0.131455 3 | -0.0174493 0.999731 0.0152789 -0.076399 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.00895815 -0.0146913 0.999852 0.0559762 2 | 0.999717 0.0221709 -0.00863118 0.12814 3 | -0.0220409 0.999646 0.0148858 -0.0785592 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | -0.0142247 -0.00627098 0.999879 0.0559944 2 | 0.999641 0.0226128 0.0143631 0.126143 3 | -0.0227001 0.999725 0.00594706 -0.0805601 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.00671307 0.00496814 0.999965 0.0593054 2 | 0.999777 0.0199868 -0.00681111 0.128446 3 | -0.0200199 0.999788 -0.00483286 -0.0769654 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.0183719 0.0162367 0.999699 0.0725803 2 | 0.999652 0.0186064 -0.0186733 0.124892 3 | -0.018904 0.999695 -0.0158892 -0.0776702 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.0161057 0.0142213 0.999769 0.0721124 2 | 0.999705 0.0179344 -0.0163598 0.12704 3 | -0.0181629 0.999738 -0.0139283 -0.0804582 4 | 0 0 0 1 5 | 6 | -------------------------------------------------------------------------------- /data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.017914 0.0143767 0.999736 0.0714918 2 | 0.999673 0.0179709 -0.0181713 0.125221 3 | -0.0182274 0.999735 -0.0140501 -0.0810866 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.0150982 0.0141827 0.999785 0.0697867 2 | 0.999711 0.018481 -0.0153593 0.123535 3 | -0.0186948 0.999729 -0.0138996 -0.0799106 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.0162378 0.0176152 0.999713 0.0679443 2 | 0.999689 0.0186145 -0.0165654 0.125258 3 | -0.0189009 0.999672 -0.0173075 -0.07712 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/camera_imu_calib_dt.csv: -------------------------------------------------------------------------------- 1 | 0, 3.13209 2 | 0, 5.24764e-155 3 | 0, 3.59485e-156 4 | 0, 4.51344e-156 5 | 0, 0.117811 6 | 0, 2.05251e-155 7 | 0, 2.62701e-155 8 | 0, 4.82191e-20 9 | 0, 2.62701e-155 10 | 0, 0 11 | 0, 2.16013e-155 12 | 0, 1.95438e-156 13 | 0, -nan 14 | 0, 2.62701e-155 15 | 0, 0 16 | 0, 4.97844e-156 17 | 0, 0 18 | 0, 1.38846e-84 19 | 0, 0 20 | 0, 1.52643e-113 21 | 0, 0.16681 22 | 0, 1.84534e-89 23 | 0, 0.181336 24 | 0, 2.25287e-106 25 | 0, 1.64332e-156 26 | 0, 4.40645e-161 27 | 0, 0.00218803 28 | 0, 4.40645e-161 29 | 0, -nan 30 | 0, 1.1357e+124 31 | 0, -nan 32 | 0, 1.1357e+124 33 | 0, 6.14747e+116 34 | 0, -nan 35 | 0, 6.14747e+116 36 | 0, -nan 37 | 0, 6.39179e+116 38 | 0, 6.39179e+116 39 | 0, 1.13569e+124 40 | 0, 6.39179e+116 41 | 0, 1.13569e+124 42 | 0, 3499.94 43 | 0, 4.43996e-161 44 | 0, 3499.94 45 | 0, 4.43996e-161 46 | 0, -nan 47 | 0, 4.52264e-161 48 | 0, -nan 49 | 0, 2.10293e+131 50 | 0, -nan 51 | 0, -nan 52 | 0, -nan 53 | 0, 5.26091e+133 54 | 0, -nan 55 | 0, -nan 56 | 0, -nan 57 | 0, -nan 58 | 0, 5.06723e+75 59 | 0, -nan 60 | 0, 2.41845e+85 61 | 0, 6.30053e+145 62 | 0, -nan 63 | 0, -nan 64 | 0, -nan 65 | 0, -nan 66 | 0, -nan 67 | 0, 4.61992e-161 68 | 0, -nan 69 | 0, 4.61992e-161 70 | 0, -nan 71 | 0, -nan 72 | 0, -nan 73 | 0, -nan 74 | 0, 3448.37 75 | 0, 6.40934e-104 76 | 0, -nan 77 | 0, 1.49062e-113 78 | 0, 2856.03 79 | 0, -nan 80 | 0, 1.49062e-113 81 | 0, -nan 82 | 0, 0 83 | 0, -nan 84 | 0, -nan 85 | 0, 0 86 | 0, 93.4792 87 | 0, -nan 88 | 0, -nan 89 | 0, -nan 90 | 0, -nan 91 | 0, 0.310032 92 | 0, 0.310032 93 | 0, 0.310032 94 | 0, 2.33073e-156 95 | 0, -nan 96 | 0, 3.24002e-155 97 | 0, 6.40576e-104 98 | 0, 0 99 | 0, 2.70514 100 | 0, 0.00398423 101 | 0, 0.00369266 102 | 0, 4.46706e-155 103 | 0, 8.019 104 | 0, 0.17145 105 | 0, 8.019 106 | 0, 2.06858e-155 107 | 0, 2.16013e-155 108 | 0, 2.16013e-155 109 | 0, -nan 110 | 0, 430.366 111 | 0, 0 112 | 0, -nan 113 | 0, 0.685303 114 | 0, 0 115 | 0, 0 116 | 0, 0 117 | 0, 0 118 | 0, 9.02913e-160 119 | 0, 2.16013e-155 120 | 0, -nan 121 | 0, -nan 122 | 0, 3.54291e-155 123 | 0, 2.68998e-156 124 | 0, 3.54291e-155 125 | 0, -nan 126 | 0, -nan 127 | 0, 4.3895e-156 128 | 0, -nan 129 | 0, 1.4712e-156 130 | 0, 2.74078e-156 131 | 0, 4.43584e+121 132 | 0, 1.99734e-156 133 | 0, 4.43584e+121 134 | 0, 4.21691e-156 135 | 0, -nan 136 | 0, -nan 137 | 0, -nan 138 | 0, -nan 139 | 0, 0.835454 140 | 0, -nan 141 | 0, -nan 142 | 0, 2.73e-123 143 | 0, 2.09503 144 | 0, 3.07982e-156 145 | 0, 2.09503 146 | 0, 2672.84 147 | 0, 2.71832 148 | 0, 2.71832 149 | 0, 2.27199e-156 150 | 0, 40.8938 151 | 0, -nan 152 | 0, -nan 153 | 0, -nan 154 | 0, 3.3535e-155 155 | 0, 0.31069 156 | 0, 0.31069 157 | 0, 0.31069 158 | 0, 4.28705 159 | 0, 0.0132837 160 | 0, -nan 161 | 0, 0.0132837 162 | 0, 0 163 | 0, 1.36877 164 | 0, 4.97205e-156 165 | 0, 1.36877 166 | 0, 3597.09 167 | 0, 0.00935469 168 | 0, -nan 169 | 0, -nan 170 | 0, 0 171 | 0, 2.4408e+114 172 | 0, -nan 173 | 0, -nan 174 | 0, 0 175 | 0, 4.31638e-156 176 | 0, 4.88812e-156 177 | 0, 4.88812e-156 178 | 0, 4.44109e-155 179 | 0, 1.21872e-96 180 | 0, -nan 181 | 0, 5.39433e-116 182 | 0, 395905 183 | 0, 3.31861e-156 184 | 0, 0 185 | 0, 4.13818e-156 186 | 0, 0 187 | 0, 0 188 | 0, 0 189 | 0, 0 190 | 0, 0 191 | 0, 1.50902e-159 192 | 0, 1.50902e-159 193 | 0, 1.50902e-159 194 | 0, 0.13826 195 | 0, 2.6652 196 | 0, 2.6652 197 | 0, -nan 198 | 0, 161.453 199 | 0, 495.35 200 | 0, -nan 201 | 0, -nan 202 | 0, 0.768926 203 | 0, -nan 204 | 0, 0.768926 205 | 0, -nan 206 | 0, 0 207 | 0, 3.7074e-155 208 | 0, -nan 209 | 0, 3.7074e-155 210 | 0, 0.0129709 211 | 0, 2.04437e-89 212 | 0, -nan 213 | 0, 1.38095e-113 214 | 0, -nan 215 | 0, -nan 216 | 0, 1.71635e-101 217 | 0, 1.04067e-160 218 | 0, 0.0620883 219 | 0, 3.00214e-94 220 | 0, 0.0620883 221 | 0, 4.29727e-99 222 | 0, -nan 223 | 0, 3.27788e-07 224 | 0, 1.14902e-96 225 | 0, -nan 226 | 0, 0.00249241 227 | 0, 5.53058e-29 228 | 0, 0.536338 229 | 0, 5.67498e-29 230 | 0, 0 231 | 0, 0.0425672 232 | 0, 0 233 | 0, 2.80203e-156 234 | 0, 0 235 | 0, 0 236 | 0, 0.000553946 237 | 0, 0.000751799 238 | 0, 0 239 | 0, 1.21887e-159 240 | 0, 1.21887e-159 241 | 0, 1.21887e-159 242 | 0, 1.99191e-155 243 | 0, 5.50153e+133 244 | 0, 0.0012139 245 | 0, 3.06152e+126 246 | 0, 28.2955 247 | 0, 0.396269 248 | 0, 0.00146978 249 | 0, 0.00160021 250 | 0, 0 251 | 0, 3.63302e-156 252 | 0, 1.15695e+153 253 | 0, 3.63302e-156 254 | 0, 4.50285e-155 255 | 0, -nan 256 | 0, -nan 257 | 0, 3.80729e-09 258 | 0, 1.44637e-159 259 | 0, -nan 260 | 0, -nan 261 | 0, 0.479503 262 | 0, -nan 263 | 0, 2.46928e-155 264 | 0, 0.00233585 265 | 0, 4.92488e-156 266 | 0, 1.4681e-155 267 | 0, 15.0618 268 | 0, 15.0618 269 | 0, 153.799 270 | 0, 265.676 271 | 0, 0.00253632 272 | 0, 1.9113 273 | 0, 3.97619e-161 274 | 0, 0.0519135 275 | 0, -nan 276 | 0, 0.00276333 277 | 0, -nan 278 | 0, 1.44977e-155 279 | 0, 4.2439e-128 280 | 0, 0 281 | 0, 3.00214e-94 282 | 0, 4.24699e-156 283 | 0, 4.68365e-155 284 | 0, 4.24699e-156 285 | 0, 4.68365e-155 286 | 0, -nan 287 | 0, 0.0218397 288 | 0, 0.00255371 289 | 0, 0 290 | 0, 0 291 | 0, 0 292 | 0, 0 293 | 0, 0 294 | 0, 3.66376e-155 295 | 0, 0.00241294 296 | 0, 3.66376e-155 297 | 0, 4.31342e+121 298 | 0, -nan 299 | 0, 4.31342e+121 300 | 0, -nan 301 | 0, 4.31342e+121 302 | 0, 0 303 | 0, 0.00223066 304 | 0, 0.00218737 305 | 0, 0.00213121 306 | 0, 1.28521e+20 307 | 0, 0 308 | 0, 0.00212545 309 | 0, 1.48337 310 | 0, -nan 311 | 0, 0 312 | 0, 0.00210041 313 | 0, 0.00206836 314 | 0, 1.48337 315 | 0, 0.00213347 316 | 0, -nan 317 | 0, 0.00207134 318 | 0, 4.44552e-162 319 | 0, -nan 320 | 0, -nan 321 | 0, -nan 322 | 0, 3.82471 323 | 0, 2.63513 324 | 0, 3.82471 325 | 0, 2.63513 326 | 0, 0.000487355 327 | 0, 0.740287 328 | 0, 4.44552e-162 329 | 0, 0.740287 330 | 0, 1.84611e-155 331 | 0, 0.00141745 332 | 0, 2.16013e-155 333 | 0, 1.84611e-155 334 | 0, 1.61747e-155 335 | 0, 2.81913 336 | 0, 2.81913 337 | 0, 2.81913 338 | 0, -nan 339 | 0, 2.16013e-155 340 | 0, 2.16013e-155 341 | 0, 2.16013e-155 342 | 0, 3.28793e-155 343 | 0, 2.16013e-155 344 | 0, 2.16013e-155 345 | -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/C_T_L_final_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.0226477 -0.99965 0.0136892 -0.248979 2 | -0.016269 -0.0140594 -0.999769 0.0128314 3 | 0.999611 0.0224197 -0.0165817 -0.0468335 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/C_T_L_final_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.0236338 -0.99963 0.0134638 -0.252289 2 | -0.0157642 -0.0138385 -0.99978 0.00347648 3 | 0.999596 0.0234164 -0.0160855 -0.0428084 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/C_T_L_final_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.0249281 -0.999512 0.018847 -0.256699 2 | -0.0140178 -0.0192005 -0.999717 0.00998013 3 | 0.999591 0.0246569 -0.0144896 -0.0450325 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/C_T_L_final_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.0230028 -0.999579 0.0177096 -0.264768 2 | -0.0132962 -0.0180186 -0.999749 0.00660157 3 | 0.999647 0.0227615 -0.013705 -0.0460839 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/C_T_L_final_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.0262541 -0.999334 0.0253265 -0.268136 2 | -0.0154025 -0.0257366 -0.99955 0.00804218 3 | 0.999537 0.0258522 -0.0160679 -0.0431316 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.0163756 0.0134655 0.999775 0.0731824 2 | 0.999699 0.0180352 -0.0166173 0.124265 3 | -0.0182549 0.999747 -0.0131661 -0.0795565 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.0132359 0.0109717 0.999852 0.0727609 2 | 0.999756 0.0175213 -0.0134269 0.126116 3 | -0.017666 0.999786 -0.0107371 -0.0817803 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.0137025 0.0144591 0.999802 0.0709992 2 | 0.999741 0.0179747 -0.0139616 0.122921 3 | -0.018173 0.999734 -0.014209 -0.0779432 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.0134897 0.0125963 0.99983 0.0695715 2 | 0.999733 0.0185704 -0.0137224 0.122777 3 | -0.0187401 0.999748 -0.0123425 -0.0805534 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.012231 0.0140161 0.999827 0.0685884 2 | 0.999742 0.018974 -0.0124959 0.123 3 | -0.0191458 0.999722 -0.0137804 -0.0795164 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_L_final_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.999538 0.00585547 -0.0298162 0.0224549 2 | 0.00573661 -0.999975 -0.00407035 -0.12363 3 | -0.0298393 0.00389743 -0.999547 -0.0615666 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_L_final_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.999589 0.0100301 -0.0268742 0.0266578 2 | 0.00993037 -0.999943 -0.003841 -0.125475 3 | -0.0269112 0.00357255 -0.999631 -0.073388 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_L_final_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.999532 0.0106785 -0.0286834 0.0226025 2 | 0.0107138 -0.999942 0.00107485 -0.132903 3 | -0.0286703 -0.00138166 -0.999588 -0.0626609 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_L_final_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.99962 0.00904669 -0.026057 0.020007 2 | 0.00903222 -0.999959 -0.000672805 -0.141166 3 | -0.026062 0.000437197 -0.99966 -0.068423 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/camera_imu_lidar_results/I_T_L_final_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.999469 0.0132641 -0.0297652 0.0222975 2 | 0.0134649 -0.999888 0.00655528 -0.144375 3 | -0.0296749 -0.00695258 -0.999535 -0.0657484 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/compareITC.asv: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_06_51.txt'); 5 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_11_20.txt'); 6 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_15_06.txt'); 7 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_20_21.txt'); 8 | I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_30_47.txt'); 9 | 10 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt'); 11 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt'); 12 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt'); 13 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt'); 14 | I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt'); 15 | 16 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt'); 17 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt'); 18 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt'); 19 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt'); 20 | I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt'); 21 | 22 | T_err_mocal = I_T_C_kalibr*inv(I_T_C_mocal); 23 | R_err_mocal = T_err_mocal(1:3, 1:3); 24 | t_err_mocal = T_err_mocal(1:3, 4)'; 25 | t_err_mocal_norm = norm(t_err_mocal)*100; 26 | eul_err_mocal = rotm2eul(R_err_mocal)*180/pi; 27 | eul_err_mocal_norm = norm(eul_err_mocal); 28 | t_eul_mocal_norm = [t_err_mocal_norm, eul_err_mocal_norm] 29 | 30 | T_err_repErr = I_T_C_kalibr*inv(I_T_C_repErr); 31 | R_err_repErr = T_err_repErr(1:3, 1:3); 32 | t_err_repErr = T_err_repErr(1:3, 4)'; 33 | t_err_repErr_norm = norm(t_err_repErr)*100; 34 | eul_err_repErr = rotm2eul(R_err_repErr)*180/pi; 35 | eul_err_repErr_norm = norm(eul_err_repErr); 36 | t_eul_repErr_norm = [t_err_repErr_norm, eul_err_repErr_norm] 37 | 38 | T_err_mocal_repErr = I_T_C_mocal*inv(I_T_C_repErr); 39 | R_err_mocal_repErr = T_err_mocal_repErr(1:3, 1:3); 40 | t_err_mocal_repErr = T_err_mocal_repErr(1:3, 4)'; 41 | t_err_mocal_repErr_norm = norm(t_err_mocal_repErr)*100; 42 | eul_err_mocal_repErr = rotm2eul(R_err_mocal_repErr)*180/pi; 43 | eul_err_mocal_repErr_norm = norm(eul_err_mocal_repErr); 44 | t_eul_repErr_norm = [t_err_repErr_t_err_mocal_repErr_normnorm, eul_err_repErr_norm] 45 | 46 | -------------------------------------------------------------------------------- /data/compareITC.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_06_51.txt'); 5 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_11_20.txt'); 6 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_15_06.txt'); 7 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_20_21.txt'); 8 | I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_30_47.txt'); 9 | 10 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt'); 11 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt'); 12 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt'); 13 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt'); 14 | I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt'); 15 | 16 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt'); 17 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt'); 18 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt'); 19 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt'); 20 | I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt'); 21 | 22 | T_err_mocal = I_T_C_kalibr*inv(I_T_C_mocal); 23 | R_err_mocal = T_err_mocal(1:3, 1:3); 24 | t_err_mocal = T_err_mocal(1:3, 4)'; 25 | t_err_mocal_norm = norm(t_err_mocal)*100; 26 | eul_err_mocal = rotm2eul(R_err_mocal)*180/pi; 27 | eul_err_mocal_norm = norm(eul_err_mocal); 28 | t_eul_mocal_norm = [t_err_mocal_norm, eul_err_mocal_norm] 29 | 30 | T_err_repErr = I_T_C_kalibr*inv(I_T_C_repErr); 31 | R_err_repErr = T_err_repErr(1:3, 1:3); 32 | t_err_repErr = T_err_repErr(1:3, 4)'; 33 | t_err_repErr_norm = norm(t_err_repErr)*100; 34 | eul_err_repErr = rotm2eul(R_err_repErr)*180/pi; 35 | eul_err_repErr_norm = norm(eul_err_repErr); 36 | t_eul_repErr_norm = [t_err_repErr_norm, eul_err_repErr_norm] 37 | 38 | T_err_mocal_repErr = I_T_C_mocal*inv(I_T_C_repErr); 39 | R_err_mocal_repErr = T_err_mocal_repErr(1:3, 1:3); 40 | t_err_mocal_repErr = T_err_mocal_repErr(1:3, 4)'; 41 | t_err_mocal_repErr_norm = norm(t_err_mocal_repErr)*100; 42 | eul_err_mocal_repErr = rotm2eul(R_err_mocal_repErr)*180/pi; 43 | eul_err_mocal_repErr_norm = norm(eul_err_mocal_repErr); 44 | t_eul_repErr_norm = [t_err_repErr_t_err_mocal_repErr_normnorm, eul_err_repErr_norm] 45 | 46 | -------------------------------------------------------------------------------- /data/compareWithKalibr.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt'); 5 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt'); 6 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_06_51.txt'); 7 | 8 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt'); 9 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt'); 10 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_11_20.txt'); 11 | 12 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt'); 13 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt'); 14 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_15_06.txt'); 15 | 16 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt'); 17 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt'); 18 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_20_21.txt'); 19 | 20 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt'); 21 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt'); 22 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_30_47.txt'); 23 | 24 | I_T_C_err_mocal = I_T_C_kalibr*inv(I_T_C_mocal); 25 | I_T_C_err_repErr = I_T_C_kalibr*inv(I_T_C_repErr); 26 | 27 | I_R_C_kalibr = I_T_C_kalibr(1:3, 1:3); 28 | euler_IRC_kalibr = rotm2eul(I_R_C_kalibr)*180/pi; 29 | I_t_C_kalibr = I_T_C_kalibr(1:3, 4)'; 30 | euler_t_kalibr = [euler_IRC_kalibr, I_t_C_kalibr*100] 31 | 32 | I_R_C_mocal = I_T_C_mocal(1:3, 1:3); 33 | euler_IRC_mocal = rotm2eul(I_R_C_mocal)*180/pi; 34 | I_t_C_mocal = I_T_C_mocal(1:3, 4)'; 35 | euler_t_mocal = [euler_IRC_mocal, I_t_C_mocal*100] 36 | 37 | I_R_C_repErr = I_T_C_repErr(1:3, 1:3); 38 | euler_IRC_repErr = rotm2eul(I_R_C_repErr)*180/pi; 39 | I_t_C_repErr = I_T_C_repErr(1:3, 4)'; 40 | euler_t_repErr = [euler_IRC_repErr, I_t_C_repErr*100] 41 | 42 | I_R_C_err_mocal = I_T_C_err_mocal(1:3, 1:3); 43 | euler_IRC_err_mocal = rotm2eul(I_R_C_err_mocal)*180/pi; 44 | I_t_C_err_mocal = I_T_C_err_mocal(1:3, 4)'; 45 | euler_t_err_mocal = [euler_IRC_err_mocal, I_t_C_err_mocal*100] 46 | 47 | I_R_C_err_repErr = I_T_C_err_repErr(1:3, 1:3); 48 | euler_IRC_err_repErr = rotm2eul(I_R_C_err_repErr)*180/pi; 49 | I_t_C_err_repErr = I_T_C_err_repErr(1:3, 4)'; 50 | euler_t_err_repErr = [euler_IRC_err_repErr, I_t_C_err_repErr*100] -------------------------------------------------------------------------------- /data/daisyChainTF.m: -------------------------------------------------------------------------------- 1 | 2 | %I_T_C = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt'); 3 | %I_T_C = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt'); 4 | %I_T_C = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt'); 5 | %I_T_C = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt'); 6 | I_T_C = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt'); 7 | 8 | %I_T_L = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/lidar_imu_results/I_T_L_2022_01_28_15_06_51.txt'); 9 | %I_T_L = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/lidar_imu_results/I_T_L_2022_01_28_15_11_20.txt'); 10 | %I_T_L = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/lidar_imu_results/I_T_L_2022_01_28_15_15_06.txt'); 11 | %I_T_L = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/lidar_imu_results/I_T_L_2022_01_28_15_20_21.txt'); 12 | I_T_L = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/lidar_imu_results/I_T_L_2022_01_28_15_30_47.txt'); 13 | 14 | C_T_L = inv(I_T_C)*I_T_L; 15 | dlmwrite('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/C_T_L_concatanated.txt', ... 16 | C_T_L, 'delimiter',' ','newline','pc'); 17 | 18 | -------------------------------------------------------------------------------- /data/displayAllITC.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | % I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt'); 5 | % I_T_C_ic = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt'); 6 | % I_T_C_jc = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_06_51.txt'); 7 | 8 | % I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt'); 9 | % I_T_C_ic = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt'); 10 | % I_T_C_jc = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_11_20.txt'); 11 | 12 | % I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt'); 13 | % I_T_C_ic = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt'); 14 | % I_T_C_jc = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_15_06.txt'); 15 | 16 | I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt'); 17 | I_T_C_ic = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt'); 18 | I_T_C_jc = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_20_21.txt'); 19 | 20 | % I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt'); 21 | % I_T_C_ic = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt'); 22 | % I_T_C_jc = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/camera_imu_lidar_results/I_T_C_final_2022_01_28_15_30_47.txt'); 23 | 24 | I_R_C_kalibr = I_T_C_kalibr(1:3, 1:3); 25 | euler_IRC_kalibr = rotm2eul(I_R_C_kalibr)*180/pi; 26 | I_t_C_kalibr = I_T_C_kalibr(1:3, 4)'; 27 | 28 | I_R_C_ic = I_T_C_ic(1:3, 1:3); 29 | euler_IRC_ic = rotm2eul(I_R_C_ic)*180/pi; 30 | I_t_C_ic = I_T_C_ic(1:3, 4)'; 31 | 32 | I_R_C_jc = I_T_C_jc(1:3, 1:3); 33 | euler_IRC_jc = rotm2eul(I_R_C_jc)*180/pi; 34 | I_t_C_jc = I_T_C_jc(1:3, 4)'; 35 | 36 | I_T_C_diff = I_T_C_kalibr*inv(I_T_C_jc); 37 | I_R_C_diff = I_T_C_diff(1:3, 1:3); 38 | euler_IRC_diff = rotm2eul(I_R_C_diff)*180/pi; 39 | I_t_C_diff = I_T_C_diff(1:3, 4)'; 40 | 41 | euler_t_kalibr = [euler_IRC_kalibr, I_t_C_kalibr*100] 42 | %euler_t_ic = [euler_IRC_ic, I_t_C_ic] 43 | euler_t_jc = [euler_IRC_jc, I_t_C_jc*100] 44 | euler_t_diff = [euler_IRC_diff, I_t_C_diff*100] -------------------------------------------------------------------------------- /data/kalibrResults.m: -------------------------------------------------------------------------------- 1 | I_T_C_kalibr_2022_01_28_15_20_21 = [0.01649452, 0.01459704, 0.9997574, 0.07272748; 2 | 0.99968208, 0.01883008, -0.0167682, 0.12541803; 3 | -0.01907028, 0.99971614, -0.01428181, -0.07559903; 4 | 0, 0, 0, 1]; 5 | 6 | I_T_C_kalibr_2022_01_28_15_11_20 = [ 0.01770378, 0.01469552, 0.99973527, 0.07331534; 7 | 0.99966975, 0.01836761, -0.01797262, 0.12664824; 8 | -0.01862686, 0.9997233, -0.01436549, -0.07536455; 9 | 0, 0, 0, 1]; 10 | I_T_C_kalibr_2022_01_28_15_15_06 = [ 0.01708571, 0.0142516, 0.99975245, 0.07334434; 11 | 0.99967944, 0.01844132, -0.01734735, 0.12591802; 12 | -0.01868398, 0.99972837, -0.01393195, -0.07538584; 13 | 0, 0, 0, 1 ]; 14 | 15 | I_T_C_kalibr_2022_01_28_15_06_51 = [ 0.01798445, 0.01477116, 0.99972915, 0.07328248; 16 | 0.99965978, 0.01862689, -0.01825842, 0.12584637; 17 | -0.01889154, 0.99971739, -0.01443114, -0.07470323; 18 | 0, 0, 0, 1]; 19 | 20 | I_T_C_kalibr_2022_01_28_15_30_47 = [ 0.01648643, 0.01482037, 0.99975425, 0.073027; 21 | 0.9996874, 0.01855249, -0.01676035, 0.12555912; 22 | -0.01879633, 0.99971804, -0.01450987, -0.0751377; 23 | 0, 0, 0, 1]; 24 | 25 | dlmwrite('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt', ... 26 | I_T_C_kalibr_2022_01_28_15_20_21, 'delimiter',' ','newline','pc'); 27 | 28 | dlmwrite('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt', ... 29 | I_T_C_kalibr_2022_01_28_15_11_20, 'delimiter',' ','newline','pc'); 30 | 31 | dlmwrite('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt', ... 32 | I_T_C_kalibr_2022_01_28_15_15_06, 'delimiter',' ','newline','pc'); 33 | 34 | dlmwrite('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt', ... 35 | I_T_C_kalibr_2022_01_28_15_06_51, 'delimiter',' ','newline','pc'); 36 | 37 | dlmwrite('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt', ... 38 | I_T_C_kalibr_2022_01_28_15_30_47, 'delimiter',' ','newline','pc'); -------------------------------------------------------------------------------- /data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.017984 0.014771 0.99973 0.073282 2 | 0.99966 0.018627 -0.018258 0.12585 3 | -0.018892 0.99972 -0.014431 -0.074703 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.017704 0.014696 0.99974 0.073315 2 | 0.99967 0.018368 -0.017973 0.12665 3 | -0.018627 0.99972 -0.014365 -0.075365 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.017086 0.014252 0.99975 0.073344 2 | 0.99968 0.018441 -0.017347 0.12592 3 | -0.018684 0.99973 -0.013932 -0.075386 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.016495 0.014597 0.99976 0.072727 2 | 0.99968 0.01883 -0.016768 0.12542 3 | -0.01907 0.99972 -0.014282 -0.075599 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.016486 0.01482 0.99975 0.073027 2 | 0.99969 0.018552 -0.01676 0.12556 3 | -0.018796 0.99972 -0.01451 -0.075138 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/lidar_imu_calib_dt.csv: -------------------------------------------------------------------------------- 1 | 0, 3.13209 2 | 0, 5.24764e-155 3 | 0, 3.59485e-156 4 | 0, 4.51344e-156 5 | 0, 0.117811 6 | 0, 2.05251e-155 7 | 0, 2.62701e-155 8 | 0, 4.82191e-20 9 | 0, 2.62701e-155 10 | 0, 0 11 | 0, 2.16013e-155 12 | 0, 1.95438e-156 13 | 0, -nan 14 | 0, 2.62701e-155 15 | 0, 0 16 | 0, 4.97844e-156 17 | 0, 0 18 | 0, 1.38846e-84 19 | 0, 0 20 | 0, 1.52643e-113 21 | 0, 0.16681 22 | 0, 1.84534e-89 23 | 0, 0.181336 24 | 0, 2.25287e-106 25 | 0, 1.64332e-156 26 | 0, 4.40645e-161 27 | 0, 0.00218803 28 | 0, 4.40645e-161 29 | 0, -nan 30 | 0, 1.1357e+124 31 | 0, -nan 32 | 0, 1.1357e+124 33 | 0, 6.14747e+116 34 | 0, -nan 35 | 0, 6.14747e+116 36 | 0, -nan 37 | 0, 6.39179e+116 38 | 0, 6.39179e+116 39 | 0, 1.13569e+124 40 | 0, 6.39179e+116 41 | 0, 1.13569e+124 42 | 0, 3499.94 43 | 0, 4.43996e-161 44 | 0, 3499.94 45 | 0, 4.43996e-161 46 | 0, -nan 47 | 0, 4.52264e-161 48 | 0, -nan 49 | 0, 2.10293e+131 50 | 0, -nan 51 | 0, -nan 52 | 0, -nan 53 | 0, 5.26091e+133 54 | 0, -nan 55 | 0, -nan 56 | 0, -nan 57 | 0, -nan 58 | 0, 5.06723e+75 59 | 0, -nan 60 | 0, 2.41845e+85 61 | 0, 6.30053e+145 62 | 0, -nan 63 | 0, -nan 64 | 0, -nan 65 | 0, -nan 66 | 0, -nan 67 | 0, 4.61992e-161 68 | 0, -nan 69 | 0, 4.61992e-161 70 | 0, -nan 71 | 0, -nan 72 | 0, -nan 73 | 0, -nan 74 | 0, 3448.37 75 | 0, 6.40934e-104 76 | 0, -nan 77 | 0, 1.49062e-113 78 | 0, 2856.03 79 | 0, -nan 80 | 0, 1.49062e-113 81 | 0, -nan 82 | 0, 0 83 | 0, -nan 84 | 0, -nan 85 | 0, 0 86 | 0, 93.4792 87 | 0, -nan 88 | 0, -nan 89 | 0, -nan 90 | 0, -nan 91 | 0, 0.310032 92 | 0, 0.310032 93 | 0, 0.310032 94 | 0, 2.33073e-156 95 | 0, -nan 96 | 0, 3.24002e-155 97 | 0, 6.40576e-104 98 | 0, 0 99 | 0, 2.70514 100 | 0, 0.00398423 101 | 0, 0.00369266 102 | 0, 4.46706e-155 103 | 0, 8.019 104 | 0, 0.17145 105 | 0, 8.019 106 | 0, 2.06858e-155 107 | 0, 2.16013e-155 108 | 0, 2.16013e-155 109 | 0, -nan 110 | 0, 430.366 111 | 0, 0 112 | 0, -nan 113 | 0, 0.685303 114 | 0, 0 115 | 0, 0 116 | 0, 0 117 | 0, 0 118 | 0, 9.02913e-160 119 | 0, 2.16013e-155 120 | 0, -nan 121 | 0, -nan 122 | 0, 3.54291e-155 123 | 0, 2.68998e-156 124 | 0, 3.54291e-155 125 | 0, -nan 126 | 0, -nan 127 | 0, 4.3895e-156 128 | 0, -nan 129 | 0, 1.4712e-156 130 | 0, 2.74078e-156 131 | 0, 4.43584e+121 132 | 0, 1.99734e-156 133 | 0, 4.43584e+121 134 | 0, 4.21691e-156 135 | 0, -nan 136 | 0, -nan 137 | 0, -nan 138 | 0, -nan 139 | 0, 0.835454 140 | 0, -nan 141 | 0, -nan 142 | 0, 2.73e-123 143 | 0, 2.09503 144 | 0, 3.07982e-156 145 | 0, 2.09503 146 | 0, 2672.84 147 | 0, 2.71832 148 | 0, 2.71832 149 | 0, 2.27199e-156 150 | 0, 40.8938 151 | 0, -nan 152 | 0, -nan 153 | 0, -nan 154 | 0, 3.3535e-155 155 | 0, 0.31069 156 | 0, 0.31069 157 | 0, 0.31069 158 | 0, 4.28705 159 | 0, 0.0132837 160 | 0, -nan 161 | 0, 0.0132837 162 | 0, 0 163 | 0, 1.36877 164 | 0, 4.97205e-156 165 | 0, 1.36877 166 | 0, 3597.09 167 | 0, 0.00935469 168 | 0, -nan 169 | 0, -nan 170 | 0, 0 171 | 0, 2.4408e+114 172 | 0, -nan 173 | 0, -nan 174 | 0, 0 175 | 0, 4.31638e-156 176 | 0, 4.88812e-156 177 | 0, 4.88812e-156 178 | 0, 4.44109e-155 179 | 0, 1.21872e-96 180 | 0, -nan 181 | 0, 5.39433e-116 182 | 0, 395905 183 | 0, 3.31861e-156 184 | 0, 0 185 | 0, 4.13818e-156 186 | 0, 0 187 | 0, 0 188 | 0, 0 189 | 0, 0 190 | 0, 0 191 | 0, 1.50902e-159 192 | 0, 1.50902e-159 193 | 0, 1.50902e-159 194 | 0, 0.13826 195 | 0, 2.6652 196 | 0, 2.6652 197 | 0, -nan 198 | 0, 161.453 199 | 0, 495.35 200 | 0, -nan 201 | 0, -nan 202 | 0, 0.768926 203 | 0, -nan 204 | 0, 0.768926 205 | 0, -nan 206 | 0, 0 207 | 0, 3.7074e-155 208 | 0, -nan 209 | 0, 3.7074e-155 210 | 0, 0.0129709 211 | 0, 2.04437e-89 212 | 0, -nan 213 | 0, 1.38095e-113 214 | 0, -nan 215 | 0, -nan 216 | 0, 1.71635e-101 217 | 0, 1.04067e-160 218 | 0, 0.0620883 219 | 0, 3.00214e-94 220 | 0, 0.0620883 221 | 0, 4.29727e-99 222 | 0, -nan 223 | 0, 3.27788e-07 224 | 0, 1.14902e-96 225 | 0, -nan 226 | 0, 0.00249241 227 | 0, 5.53058e-29 228 | 0, 0.536338 229 | 0, 5.67498e-29 230 | 0, 0 231 | 0, 0.0425672 232 | 0, 0 233 | 0, 2.80203e-156 234 | 0, 0 235 | 0, 0 236 | 0, 0.000553946 237 | 0, 0.000751799 238 | 0, 0 239 | 0, 1.21887e-159 240 | 0, 1.21887e-159 241 | 0, 1.21887e-159 242 | 0, 1.99191e-155 243 | 0, 5.50153e+133 244 | 0, 0.0012139 245 | 0, 3.06152e+126 246 | 0, 28.2955 247 | 0, 0.396269 248 | 0, 0.00146978 249 | 0, 0.00160021 250 | 0, 0 251 | 0, 3.63302e-156 252 | 0, 1.15695e+153 253 | 0, 3.63302e-156 254 | 0, 4.50285e-155 255 | 0, -nan 256 | 0, -nan 257 | 0, 3.80729e-09 258 | 0, 1.44637e-159 259 | 0, -nan 260 | 0, -nan 261 | 0, 0.479503 262 | 0, -nan 263 | 0, 2.46928e-155 264 | 0, 0.00233585 265 | 0, 4.92488e-156 266 | 0, 1.4681e-155 267 | 0, 15.0618 268 | 0, 15.0618 269 | 0, 153.799 270 | 0, 265.676 271 | 0, 0.00253632 272 | 0, 1.9113 273 | 0, 3.97619e-161 274 | 0, 0.0519135 275 | 0, -nan 276 | 0, 0.00276333 277 | 0, -nan 278 | 0, 1.44977e-155 279 | 0, 4.2439e-128 280 | 0, 0 281 | 0, 3.00214e-94 282 | 0, 4.24699e-156 283 | 0, 4.68365e-155 284 | 0, 4.24699e-156 285 | 0, 4.68365e-155 286 | 0, -nan 287 | 0, 0.0218397 288 | 0, 0.00255371 289 | 0, 0 290 | 0, 0 291 | 0, 0 292 | 0, 0 293 | 0, 0 294 | 0, 3.66376e-155 295 | 0, 0.00241294 296 | 0, 3.66376e-155 297 | 0, 4.31342e+121 298 | 0, -nan 299 | 0, 4.31342e+121 300 | 0, -nan 301 | 0, 4.31342e+121 302 | 0, 0 303 | 0, 0.00223066 304 | 0, 0.00218737 305 | 0, 0.00213121 306 | 0, 1.28521e+20 307 | 0, 0 308 | 0, 0.00212545 309 | 0, 1.48337 310 | 0, -nan 311 | 0, 0 312 | 0, 0.00210041 313 | 0, 0.00206836 314 | 0, 1.48337 315 | 0, 0.00213347 316 | 0, -nan 317 | 0, 0.00207134 318 | 0, 4.44552e-162 319 | 0, -nan 320 | 0, -nan 321 | 0, -nan 322 | 0, 3.82471 323 | 0, 2.63513 324 | 0, 3.82471 325 | 0, 2.63513 326 | 0, 0.000487355 327 | 0, 0.740287 328 | 0, 4.44552e-162 329 | 0, 0.740287 330 | 0, 1.84611e-155 331 | 0, 0.00141745 332 | 0, 2.16013e-155 333 | 0, 1.84611e-155 334 | 0, 1.61747e-155 335 | 0, 2.81913 336 | 0, 2.81913 337 | 0, 2.81913 338 | 0, -nan 339 | 0, 2.16013e-155 340 | 0, 2.16013e-155 341 | 0, 2.16013e-155 342 | 0, 3.28793e-155 343 | 0, 2.16013e-155 344 | 0, 2.16013e-155 345 | -------------------------------------------------------------------------------- /data/lidar_imu_results/I_T_L_2022_01_28_15_06_51.txt: -------------------------------------------------------------------------------- 1 | 0.999628 0.0227966 -0.0149847 0.00934938 2 | 0.0225827 -0.999643 -0.0142921 -0.131493 3 | -0.0153052 0.0139484 -0.999786 -0.0928312 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/lidar_imu_results/I_T_L_2022_01_28_15_11_20.txt: -------------------------------------------------------------------------------- 1 | 0.999514 0.0311686 -0.000652867 0.00485358 2 | 0.0311518 -0.999353 -0.0179599 -0.128511 3 | -0.00121223 0.0179309 -0.999838 -0.0863284 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/lidar_imu_results/I_T_L_2022_01_28_15_15_06.txt: -------------------------------------------------------------------------------- 1 | 0.999976 0.00675986 0.00133315 0.00118191 2 | 0.00678009 -0.999852 -0.0158022 -0.129086 3 | 0.00122613 0.0158108 -0.999874 -0.0739849 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/lidar_imu_results/I_T_L_2022_01_28_15_20_21.txt: -------------------------------------------------------------------------------- 1 | 0.999911 0.0121876 -0.00550487 0.00386106 2 | 0.0121416 -0.999892 -0.0083079 -0.135112 3 | -0.00560553 0.00824032 -0.99995 -0.0755804 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/lidar_imu_results/I_T_L_2022_01_28_15_30_47.txt: -------------------------------------------------------------------------------- 1 | 0.999973 0.00240252 -0.00690459 -0.00362995 2 | 0.00227654 -0.999832 -0.0181963 -0.126041 3 | -0.00694715 0.0181801 -0.999811 -0.0755317 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /data/lo_trajectory.csv: -------------------------------------------------------------------------------- 1 | 1.6434e+09,0.000258647,0.0110773,0.00241767,0.999936,0.00192266,-0.00146641,0.00218566 2 | 1.6434e+09,-0.000782561,0.0092799,0.00415172,0.999948,-0.00447069,-0.00328291,0.0205515 3 | 1.6434e+09,-0.000449503,0.0103419,0.00301208,0.999942,-0.00435141,-0.00221039,0.0407955 4 | 1.6434e+09,0.00435447,0.0047701,0.0019962,0.999977,-0.00464441,-0.0044082,0.0752542 5 | 1.6434e+09,0.00853187,-0.00434966,0.00217404,0.999952,-0.00883295,-0.00786924,0.113054 6 | 1.6434e+09,0.0106653,-0.012513,0.00275746,0.999861,-0.0107064,-0.00852842,0.145716 7 | 1.6434e+09,0.0117879,-0.0164794,0.00281387,0.999791,-0.0121453,-0.00611364,0.170889 8 | 1.6434e+09,0.0117342,-0.0149951,0.00538456,0.999804,-0.012044,-0.00258396,0.181446 9 | 1.6434e+09,0.00892111,-0.00893327,0.0099943,0.99987,-0.0125334,-0.00118853,0.181342 10 | 1.6434e+09,0.00342925,0.000495849,0.00844668,0.999958,-0.0079971,0.00394152,0.179261 11 | 1.6434e+09,0.000412006,0.00448846,0.00359195,0.999983,-0.00395245,0.00355492,0.174011 12 | 1.6434e+09,0.00371949,0.00512786,-0.000565752,0.99998,-0.00351239,0.00205988,0.177453 13 | 1.6434e+09,0.00631452,0.00657678,-0.00422701,0.999949,0.000366404,-0.00124282,0.178629 14 | 1.6434e+09,0.00497004,0.00786939,-0.0052715,0.999943,-0.000462152,-0.00103443,0.170786 15 | 1.6434e+09,0.00791464,7.38231e-05,-0.00723856,0.999942,-0.0043245,-0.00517577,0.174265 16 | 1.6434e+09,0.00955849,-0.00707611,-0.0106278,0.999873,-0.0027572,-0.0083317,0.174746 17 | 1.6434e+09,-0.00149544,-0.000372878,-0.0137904,0.999904,-0.00345953,-0.0101518,0.171445 18 | 1.6434e+09,-0.0296505,0.00589458,-0.012038,0.99947,-0.00837677,-0.0132754,0.161775 19 | 1.6434e+09,-0.0731367,0.0119966,-0.00988382,0.997201,-0.0116204,-0.0124201,0.149505 20 | 1.6434e+09,-0.121679,0.0146567,-0.00337007,0.992455,-0.0188656,-0.0150732,0.137815 21 | 1.6434e+09,-0.174586,0.0150902,0.0070744,0.984501,-0.0286254,-0.0158461,0.129024 22 | 1.6434e+09,-0.225833,0.0173402,0.0123317,0.973934,-0.0329863,-0.0184464,0.116598 23 | 1.6434e+09,-0.274677,0.018073,0.0165578,0.961224,-0.039122,-0.0245525,0.110653 24 | 1.6434e+09,-0.328131,0.0164313,0.0181541,0.944315,-0.0439596,-0.0282043,0.100764 25 | 1.6434e+09,-0.381581,0.0137057,0.0189559,0.924039,-0.0471078,-0.0342337,0.0865998 26 | 1.6434e+09,-0.43122,0.0127917,0.0197853,0.901939,-0.0506159,-0.0421998,0.0818511 27 | 1.6434e+09,-0.472788,0.0153683,0.0227334,0.880749,-0.0530317,-0.0482133,0.0760396 28 | 1.6434e+09,-0.508905,0.0182062,0.0259093,0.86024,-0.0532526,-0.0567176,0.066959 29 | 1.6434e+09,-0.540502,0.0207322,0.0289232,0.84059,-0.0528331,-0.0636999,0.0660081 30 | 1.6434e+09,-0.561502,0.0225531,0.0328936,0.826514,-0.0556615,-0.0654067,0.061221 31 | 1.6434e+09,-0.568363,0.0242129,0.0378779,0.821549,-0.0566404,-0.0672966,0.0549915 32 | 1.6434e+09,-0.559808,0.0216746,0.0406698,0.82734,-0.0584276,-0.0613664,0.051642 33 | 1.6434e+09,-0.534565,0.015668,0.0426809,0.843904,-0.0592927,-0.0555519,0.0537859 34 | 1.6434e+09,-0.489493,0.00325024,0.0459811,0.870788,-0.0596762,-0.0430807,0.0625551 35 | 1.6434e+09,-0.432733,-0.00939006,0.0446636,0.900366,-0.0577394,-0.0314697,0.0724992 36 | 1.6434e+09,-0.368976,-0.0167511,0.0408804,0.928388,-0.0551234,-0.0161535,0.0852092 37 | 1.6434e+09,-0.301066,-0.0231484,0.0360085,0.952642,-0.0522312,-0.00414679,0.100573 38 | 1.6434e+09,-0.226416,-0.0233238,0.032197,0.973219,-0.049971,0.0065683,0.115941 39 | 1.6434e+09,-0.14374,-0.0185884,0.0262957,0.989091,-0.0458513,0.0117264,0.14082 40 | 1.6434e+09,-0.0641173,-0.0145122,0.0166599,0.997698,-0.038903,0.0141469,0.169508 41 | 1.6434e+09,0.0098389,-0.0160533,0.00642563,0.999802,-0.034246,0.0116858,0.191268 42 | 1.6434e+09,0.0804131,-0.021894,-0.000704489,0.996521,-0.0307015,0.010122,0.20286 43 | 1.6434e+09,0.151632,-0.024759,-0.0052274,0.988113,-0.0297771,0.00222709,0.218456 44 | 1.6434e+09,0.220264,-0.023492,-0.00806158,0.975124,-0.0303253,-0.00835528,0.239555 45 | 1.6434e+09,0.280315,-0.0215668,-0.0113073,0.959599,-0.0289336,-0.0213246,0.256861 46 | 1.6434e+09,0.332272,-0.0220983,-0.0149791,0.942806,-0.0255316,-0.034845,0.268881 47 | 1.6434e+09,0.381192,-0.0226844,-0.016519,0.92407,-0.0251355,-0.0521564,0.278113 48 | 1.6434e+09,0.432314,-0.0177782,-0.0183873,0.90136,-0.0250278,-0.0667464,0.280825 49 | 1.6434e+09,0.474676,-0.0121406,-0.024173,0.879745,-0.02183,-0.0822114,0.278854 50 | 1.6434e+09,0.50687,-0.0108524,-0.0316669,0.861372,-0.0199351,-0.0945306,0.274422 51 | 1.6434e+09,0.531698,-0.0113383,-0.037314,0.846036,-0.0193007,-0.1044,0.272263 52 | 1.6434e+09,0.547738,-0.0125649,-0.0414465,0.835528,-0.0181999,-0.111853,0.269969 53 | 1.6434e+09,0.547909,-0.0124991,-0.042247,0.835377,-0.0179344,-0.107397,0.267987 54 | 1.6434e+09,0.534947,-0.008881,-0.0400055,0.843891,-0.0160839,-0.101779,0.267308 55 | 1.6434e+09,0.512014,-0.005601,-0.0385252,0.858095,-0.0144634,-0.0900608,0.266999 56 | 1.6434e+09,0.472058,-0.00189303,-0.0372495,0.880778,-0.0126243,-0.0765421,0.265453 57 | 1.6434e+09,0.41906,0.00196625,-0.0355252,0.907261,-0.0120357,-0.061835,0.262643 58 | 1.6434e+09,0.360445,0.00735427,-0.0321073,0.932199,-0.0101228,-0.0506374,0.259193 59 | 1.6434e+09,0.296506,0.0138223,-0.029622,0.954471,-0.00907986,-0.0398192,0.254709 60 | 1.6434e+09,0.223703,0.02188,-0.0262833,0.974057,-0.0100546,-0.0276198,0.246226 61 | 1.6434e+09,0.146962,0.028065,-0.0192694,0.988556,-0.0140116,-0.0188905,0.237547 62 | 1.6434e+09,0.0676873,0.0316407,-0.0107478,0.997147,-0.0211325,-0.0106828,0.226002 63 | 1.6434e+09,-0.0102607,0.0377523,-0.000712857,0.999234,-0.0278299,-0.00679881,0.212377 64 | 1.6434e+09,-0.084462,0.0452452,0.00557491,0.995383,-0.033997,-0.00188742,0.195727 65 | 1.6434e+09,-0.15251,0.0602969,0.0124055,0.986383,-0.0410369,3.10448e-05,0.17585 66 | 1.6434e+09,-0.218079,0.0773421,0.0183055,0.972689,-0.0525824,-0.00527747,0.159381 67 | 1.6434e+09,-0.268721,0.0910953,0.0269355,0.958522,-0.05836,-0.0100853,0.141188 68 | 1.6434e+09,-0.310367,0.107145,0.0348354,0.943917,-0.0738866,-0.0170191,0.131219 69 | 1.6434e+09,-0.347544,0.1138,0.0403963,0.929855,-0.0830024,-0.0225994,0.119168 70 | 1.6434e+09,-0.381548,0.114364,0.0375376,0.916479,-0.0877941,-0.0274159,0.109808 71 | 1.6434e+09,-0.416353,0.112909,0.0292265,0.901692,-0.0868493,-0.0314472,0.103466 72 | 1.6434e+09,-0.453583,0.116343,0.0251479,0.88323,-0.0875303,-0.039953,0.0976676 73 | 1.6434e+09,-0.487066,0.12303,0.0234965,0.864337,-0.0898207,-0.0441341,0.0908616 74 | 1.6434e+09,-0.518464,0.129198,0.0206655,0.84503,-0.0913591,-0.0551194,0.0853422 75 | 1.6434e+09,-0.541548,0.133183,0.017947,0.829859,-0.0922982,-0.0604742,0.0803218 76 | 1.6434e+09,-0.547918,0.135468,0.017264,0.82531,-0.0936654,-0.0677919,0.0777105 77 | 1.6434e+09,-0.545399,0.133635,0.0161275,0.827298,-0.0937249,-0.0747354,0.0738051 78 | 1.6434e+09,-0.525591,0.12627,0.0168082,0.841146,-0.0937165,-0.07334,0.0731864 79 | 1.6434e+09,-0.471911,0.106638,0.0160055,0.875027,-0.0962055,-0.0716543,0.0813273 80 | 1.6434e+09,-0.406004,0.0879693,0.0146021,0.90951,-0.0994779,-0.0258449,0.101613 81 | 1.6434e+09,-0.337673,0.0698392,0.0138156,0.938567,-0.0831052,-0.0342451,0.10244 82 | 1.6434e+09,-0.24965,0.0680221,0.00692027,0.965919,-0.0776977,-0.022006,0.1229 83 | 1.6434e+09,-0.159152,0.0638239,0.000109728,0.985189,-0.0718607,-0.0152582,0.142762 84 | 1.6434e+09,-0.082744,0.0613807,-0.00926799,0.994636,-0.063994,-0.0107557,0.158487 85 | 1.6434e+09,-0.0142774,0.0561799,-0.0171713,0.998171,-0.0576373,-0.00707403,0.171228 86 | -------------------------------------------------------------------------------- /data/plotBias.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | data = csvread('imu_bias.csv'); 4 | ba = data(:, 1:3); 5 | bg = data(:, 4:6); 6 | sigma_ba = data(:, 7:9); 7 | sigma_bg = data(:, 10:12); 8 | 9 | %% 10 | figure('Name','Accelerometer Bias KF','NumberTitle','off'); 11 | subplot(311) 12 | plot(ba(:,1), 'LineWidth', 3); 13 | hold on; 14 | plot(ba(:,1) + sigma_ba(:,1), '--r', 'LineWidth', 3); 15 | hold on; 16 | plot(ba(:,1) - sigma_ba(:,1), '--r', 'LineWidth', 3); 17 | hold off; 18 | ylabel('ba_x'); 19 | grid; 20 | title('ba_x time plot'); 21 | subplot(312) 22 | plot(ba(:,2), 'LineWidth', 3); 23 | hold on; 24 | plot(ba(:,2) + sigma_ba(:,2), '--r', 'LineWidth', 3); 25 | hold on; 26 | plot(ba(:,2) - sigma_ba(:,2), '--r', 'LineWidth', 3); 27 | hold off; 28 | ylabel('ba_y'); 29 | grid; 30 | title('ba_y time plot'); 31 | subplot(313) 32 | plot(ba(:,3), 'LineWidth', 3); 33 | hold on; 34 | plot(ba(:,3) + sigma_ba(:,3), '--r', 'LineWidth', 3); 35 | hold on; 36 | plot(ba(:,3) - sigma_ba(:,3), '--r', 'LineWidth', 3); 37 | hold off; 38 | ylabel('ba_z'); 39 | grid; 40 | title('ba_z time plot'); 41 | %% 42 | figure('Name','Gyroscope Bias KF','NumberTitle','off'); 43 | subplot(311) 44 | plot(bg(:,1), 'LineWidth', 3); 45 | hold on; 46 | plot(bg(:,1) + sigma_bg(:,1), '--r', 'LineWidth', 3); 47 | hold on; 48 | plot(bg(:,1) - sigma_bg(:,1), '--r', 'LineWidth', 3); 49 | hold off; 50 | ylabel('bg_x'); 51 | grid; 52 | title('bg_x time plot'); 53 | subplot(312) 54 | plot(bg(:,2), 'LineWidth', 3); 55 | hold on; 56 | plot(bg(:,2) + sigma_bg(:,2), '--r', 'LineWidth', 3); 57 | hold on; 58 | plot(bg(:,2) - sigma_bg(:,2), '--r', 'LineWidth', 3); 59 | hold off; 60 | ylabel('bg_y'); 61 | grid; 62 | title('bg_y time plot'); 63 | subplot(313) 64 | plot(bg(:,3), 'LineWidth', 3); 65 | hold on; 66 | plot(bg(:,3) + sigma_bg(:,3), '--r', 'LineWidth', 3); 67 | hold on; 68 | plot(bg(:,3) - sigma_bg(:,3), '--r', 'LineWidth', 3); 69 | hold off; 70 | ylabel('bg_z'); 71 | title('bg_z time plot'); 72 | grid; -------------------------------------------------------------------------------- /data/plotCalib_camera_imu.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | data_ext_calib = csvread('camera_imu_calib_extrinsics.csv'); 4 | quat = data_ext_calib(:, 1:4); 5 | xyz = data_ext_calib(:, 5:7); 6 | sigma_xyz = data_ext_calib(:, 11:13); 7 | sigma_rxryrz = data_ext_calib(:, 8:10)*180/pi; 8 | data_dt_calib = csvread('camera_imu_calib_extrinsics.csv'); 9 | dt = data_dt_calib(:, 1); 10 | sigma_dt = data_dt_calib(:, 2); 11 | %% 12 | figure('Name','Calibration XYZ KF','NumberTitle','off'); 13 | subplot(311) 14 | plot(xyz(:,1), 'LineWidth', 3); 15 | %hold off; 16 | hold on; 17 | plot(xyz(:,1) + sigma_xyz(:,1), '--r', 'LineWidth', 3); 18 | hold on; 19 | plot(xyz(:,1) - sigma_xyz(:,1), '--r', 'LineWidth', 3); 20 | hold off; 21 | ylabel('Calib X [m]','fontweight','bold','fontsize',16); 22 | grid; 23 | title('Calib X [m]','fontweight','bold','fontsize',16); 24 | xlabel('Time [s]','fontweight','bold','fontsize',16); 25 | legend('Estimate', '1 \sigma bound'); 26 | set(gca,'FontSize', 24); 27 | 28 | subplot(312) 29 | plot(xyz(:,2), 'LineWidth', 3); 30 | %hold off; 31 | hold on; 32 | plot(xyz(:,2) + sigma_xyz(:,2), '--r', 'LineWidth', 3); 33 | hold on; 34 | plot(xyz(:,2) - sigma_xyz(:,2), '--r', 'LineWidth', 3); 35 | hold off; 36 | ylabel('Calib Y [m]','fontweight','bold','fontsize',16); 37 | grid; 38 | title('Calib Y [m]','fontweight','bold','fontsize',16); 39 | xlabel('Time [s]','fontweight','bold','fontsize',16); 40 | legend('Estimate', '1 \sigma bound'); 41 | set(gca,'FontSize', 24); 42 | 43 | subplot(313) 44 | plot(xyz(:,3), 'LineWidth', 3); 45 | %hold off; 46 | hold on; 47 | plot(xyz(:,3) + sigma_xyz(:,3), '--r', 'LineWidth', 3); 48 | hold on; 49 | plot(xyz(:,3) - sigma_xyz(:,3), '--r', 'LineWidth', 3); 50 | hold off; 51 | ylabel('Calib Z [m]','fontweight','bold','fontsize',16); 52 | grid; 53 | title('Calib Z [m]','fontweight','bold','fontsize',16); 54 | xlabel('Time [s]','fontweight','bold','fontsize',16); 55 | legend('Estimate', '1 \sigma bound'); 56 | set(gca,'FontSize', 24); 57 | 58 | 59 | %% 60 | eulerangleDegrees = []; 61 | for i=1:length(quat) 62 | quat_i = quaternion(quat(i, 4), quat(i, 1), quat(i, 2), quat(i, 3)); 63 | eulerAngles = eulerd(quat_i, 'XYZ', 'frame'); 64 | euler_x = wrapTo360(eulerAngles(:, 1)); 65 | euler_y = eulerAngles(:, 2); 66 | euler_z = eulerAngles(:, 3); 67 | eulerangleDegrees = [eulerangleDegrees; [euler_x, euler_y, euler_z]]; 68 | end 69 | eulerangleDegreesMinus = eulerangleDegrees - sigma_rxryrz; 70 | eulerangleDegreesPlus = eulerangleDegrees + sigma_rxryrz; 71 | 72 | %% 73 | figure('Name','Rotation Calibration KF','NumberTitle','off'); 74 | subplot(311) 75 | plot(eulerangleDegrees(:,1), 'LineWidth', 3); 76 | hold on; 77 | plot(eulerangleDegreesMinus(:,1), '--r', 'LineWidth', 3); 78 | hold on; 79 | plot(eulerangleDegreesPlus(:,1), '--r', 'LineWidth', 3); 80 | hold off; 81 | ylabel('Euler X','fontweight','bold','fontsize',16); 82 | grid; 83 | title('Euler X [deg]','fontweight','bold','fontsize',16); 84 | xlabel('Time [s]','fontweight','bold','fontsize',16); 85 | legend('Estimate', '1 \sigma bound'); 86 | set(gca,'FontSize', 24); 87 | 88 | subplot(312) 89 | plot(eulerangleDegrees(:,2), 'LineWidth', 3); 90 | hold on; 91 | plot(eulerangleDegreesMinus(:,2),'--r', 'LineWidth', 3); 92 | hold on; 93 | plot(eulerangleDegreesPlus(:,2),'--r', 'LineWidth', 3); 94 | hold off; 95 | ylabel('Euler Y','fontweight','bold','fontsize',16); 96 | grid; 97 | title('Euler Y [deg]','fontweight','bold','fontsize',16); 98 | xlabel('Time [s]','fontweight','bold','fontsize',16); 99 | legend('Estimate', '1 \sigma bound'); 100 | set(gca,'FontSize', 24); 101 | 102 | subplot(313) 103 | plot(eulerangleDegrees(:,3), 'LineWidth', 3); 104 | hold on; 105 | plot(eulerangleDegreesMinus(:,3),'--r', 'LineWidth', 3); 106 | hold on; 107 | plot(eulerangleDegreesPlus(:,3),'--r', 'LineWidth', 3); 108 | hold off; 109 | ylabel('Euler Z','fontweight','bold','fontsize',16); 110 | grid; 111 | title('Euler Z [deg]','fontweight','bold','fontsize',16); 112 | xlabel('Time [s]','fontweight','bold','fontsize',16); 113 | 114 | legend('Estimate', '1 \sigma bound'); 115 | set(gca,'FontSize', 24); -------------------------------------------------------------------------------- /data/plotCalib_lidar_imu.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | data_ext_calib = csvread('lidar_imu_calib_extrinsics.csv'); 4 | quat = data_ext_calib(:, 1:4); 5 | xyz = data_ext_calib(:, 5:7); 6 | sigma_xyz = data_ext_calib(:, 11:13); 7 | sigma_rxryrz = data_ext_calib(:, 8:10)*180/pi; 8 | data_dt_calib = csvread('lidar_imu_calib_extrinsics.csv'); 9 | dt = data_dt_calib(:, 1); 10 | sigma_dt = data_dt_calib(:, 2); 11 | %% 12 | figure('Name','Calibration XYZ KF','NumberTitle','off'); 13 | subplot(311) 14 | plot(xyz(:,1), 'LineWidth', 3); 15 | %hold off; 16 | hold on; 17 | plot(xyz(:,1) + sigma_xyz(:,1), '--r', 'LineWidth', 3); 18 | hold on; 19 | plot(xyz(:,1) - sigma_xyz(:,1), '--r', 'LineWidth', 3); 20 | hold off; 21 | ylabel('Calib X [m]','fontweight','bold','fontsize',16); 22 | grid; 23 | title('Calib X [m]','fontweight','bold','fontsize',16); 24 | xlabel('Time [s]','fontweight','bold','fontsize',16); 25 | legend('Estimate', '1 \sigma bound'); 26 | set(gca,'FontSize', 24); 27 | 28 | subplot(312) 29 | plot(xyz(:,2), 'LineWidth', 3); 30 | %hold off; 31 | hold on; 32 | plot(xyz(:,2) + sigma_xyz(:,2), '--r', 'LineWidth', 3); 33 | hold on; 34 | plot(xyz(:,2) - sigma_xyz(:,2), '--r', 'LineWidth', 3); 35 | hold off; 36 | ylabel('Calib Y [m]','fontweight','bold','fontsize',16); 37 | grid; 38 | title('Calib Y [m]','fontweight','bold','fontsize',16); 39 | xlabel('Time [s]','fontweight','bold','fontsize',16); 40 | legend('Estimate', '1 \sigma bound'); 41 | set(gca,'FontSize', 24); 42 | 43 | subplot(313) 44 | plot(xyz(:,3), 'LineWidth', 3); 45 | %hold off; 46 | hold on; 47 | plot(xyz(:,3) + sigma_xyz(:,3), '--r', 'LineWidth', 3); 48 | hold on; 49 | plot(xyz(:,3) - sigma_xyz(:,3), '--r', 'LineWidth', 3); 50 | hold off; 51 | ylabel('Calib Z [m]','fontweight','bold','fontsize',16); 52 | grid; 53 | title('Calib Z [m]','fontweight','bold','fontsize',16); 54 | xlabel('Time [s]','fontweight','bold','fontsize',16); 55 | legend('Estimate', '1 \sigma bound'); 56 | set(gca,'FontSize', 24); 57 | 58 | 59 | %% 60 | eulerangleDegrees = []; 61 | for i=1:length(quat) 62 | quat_i = quaternion(quat(i, 4), quat(i, 1), quat(i, 2), quat(i, 3)); 63 | eulerAngles = eulerd(quat_i, 'XYZ', 'frame'); 64 | euler_x = wrapTo360(eulerAngles(:, 1)); 65 | euler_y = eulerAngles(:, 2); 66 | euler_z = eulerAngles(:, 3); 67 | eulerangleDegrees = [eulerangleDegrees; [euler_x, euler_y, euler_z]]; 68 | end 69 | eulerangleDegreesMinus = eulerangleDegrees - sigma_rxryrz; 70 | eulerangleDegreesPlus = eulerangleDegrees + sigma_rxryrz; 71 | 72 | %% 73 | figure('Name','Rotation Calibration KF','NumberTitle','off'); 74 | subplot(311) 75 | plot(eulerangleDegrees(:,1), 'LineWidth', 3); 76 | hold on; 77 | plot(eulerangleDegreesMinus(:,1), '--r', 'LineWidth', 3); 78 | hold on; 79 | plot(eulerangleDegreesPlus(:,1), '--r', 'LineWidth', 3); 80 | hold off; 81 | ylabel('Euler X','fontweight','bold','fontsize',16); 82 | grid; 83 | title('Euler X [deg]','fontweight','bold','fontsize',16); 84 | xlabel('Time [s]','fontweight','bold','fontsize',16); 85 | ylim([170, 190]); 86 | legend('Estimate', '1 \sigma bound'); 87 | set(gca,'FontSize', 24); 88 | 89 | subplot(312) 90 | plot(eulerangleDegrees(:,2), 'LineWidth', 3); 91 | hold on; 92 | plot(eulerangleDegreesMinus(:,2),'--r', 'LineWidth', 3); 93 | hold on; 94 | plot(eulerangleDegreesPlus(:,2),'--r', 'LineWidth', 3); 95 | hold off; 96 | ylabel('Euler Y','fontweight','bold','fontsize',16); 97 | grid; 98 | title('Euler Y [deg]','fontweight','bold','fontsize',16); 99 | xlabel('Time [s]','fontweight','bold','fontsize',16); 100 | ylim([-10, 10]); 101 | legend('Estimate', '1 \sigma bound'); 102 | set(gca,'FontSize', 24); 103 | 104 | subplot(313) 105 | plot(eulerangleDegrees(:,3), 'LineWidth', 3); 106 | hold on; 107 | plot(eulerangleDegreesMinus(:,3),'--r', 'LineWidth', 3); 108 | hold on; 109 | plot(eulerangleDegreesPlus(:,3),'--r', 'LineWidth', 3); 110 | hold off; 111 | ylabel('Euler Z','fontweight','bold','fontsize',16); 112 | grid; 113 | title('Euler Z [deg]','fontweight','bold','fontsize',16); 114 | xlabel('Time [s]','fontweight','bold','fontsize',16); 115 | ylim([-10, 10]); 116 | legend('Estimate', '1 \sigma bound'); 117 | set(gca,'FontSize', 24); 118 | 119 | 120 | -------------------------------------------------------------------------------- /data/plotResidual_cameraimulidar.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | data_residual = csvread('residual_cameraimulidar.csv'); 4 | plot(data_residual); 5 | grid; 6 | 7 | -------------------------------------------------------------------------------- /data/plotTrajectory.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc; 4 | data = csvread('imu_trajectory.csv'); 5 | xyz = data(:, 6:8); 6 | 7 | 8 | %% 9 | figure('Name','Trajectory 3D XYZ KF','NumberTitle','off'); 10 | plot3(xyz(1,1), xyz(1,2), xyz(1,3), 'ro','MarkerSize',12,'MarkerFaceColor',[1 .6 .6]); 11 | hold on; 12 | plot3(xyz(:, 1), xyz(:, 2), xyz(:, 3), 'b', 'LineWidth', 1.5); 13 | hold on; 14 | plot3(xyz(end, 1), xyz(end, 2), xyz(end, 3), 'go','MarkerSize',12,'MarkerFaceColor',[0.6 1 1]); 15 | hold off; 16 | grid; 17 | axis equal; 18 | xlabel('X [m]','fontweight','bold','fontsize',16); 19 | ylabel('Y [m]','fontweight','bold','fontsize',16); 20 | zlabel('Z [m]','fontweight','bold','fontsize',16); 21 | title('Trajectory of the Lidar-IMU system in IMU Frame','fontweight','bold','fontsize',16); 22 | legend('Start', 'Estimated IMU Trajectory', 'End', 'fontweight','bold','fontsize',16); 23 | set(gca,'FontSize', 20); 24 | 25 | 26 | -------------------------------------------------------------------------------- /data/plotVelocity.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | data = csvread('imu_velocity.csv'); 4 | velocity = data(:,1:3); 5 | sigma_velocity = data(:, 4:6); 6 | 7 | %% 8 | figure('Name','Velocity KF','NumberTitle','off'); 9 | subplot(311) 10 | plot(velocity(:,1), 'LineWidth', 3); 11 | hold on; 12 | plot(velocity(:,1) + sigma_velocity(:,1), '--r', 'LineWidth', 3); 13 | hold on; 14 | plot(velocity(:,1) - sigma_velocity(:,1), '--r', 'LineWidth', 3); 15 | hold off; 16 | ylabel('v_x'); 17 | grid; 18 | title('v_x time plot'); 19 | legend('Estimate', '1 \sigma bound'); 20 | subplot(312) 21 | plot(velocity(:,2), 'LineWidth', 3); 22 | hold on; 23 | plot(velocity(:,2) + sigma_velocity(:,2), '--r', 'LineWidth', 3); 24 | hold on; 25 | plot(velocity(:,2) - sigma_velocity(:,2), '--r', 'LineWidth', 3); 26 | hold off; 27 | ylabel('v_y'); 28 | grid; 29 | title('v_y time plot'); 30 | legend('Estimate', '1 \sigma bound'); 31 | subplot(313) 32 | plot(velocity(:,3), 'LineWidth', 3); 33 | hold on; 34 | plot(velocity(:,3) + sigma_velocity(:,3), '--r', 'LineWidth', 3); 35 | hold on; 36 | plot(velocity(:,3) - sigma_velocity(:,3), '--r', 'LineWidth', 3); 37 | hold off; 38 | ylabel('v_z'); 39 | grid; 40 | title('v_z time plot'); 41 | legend('Estimate', '1 \sigma bound'); -------------------------------------------------------------------------------- /data/pylon_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 1920 3 | image_height: 1200 4 | # distortion_parameters: 5 | k1: -0.1374906235082657 6 | k2: 0.05783702444800811 7 | p1: 0.000180013561902521 8 | p2: 0.0006508618187655724 9 | k3: 0 10 | # projection_parameters: 11 | fx: 1053.8767420505942 12 | fy: 1052.6765254287882 13 | cx: 967.3899880574555 14 | cy: 587.1422341787483 15 | 16 | -------------------------------------------------------------------------------- /data/residual_cameraimulidar.csv: -------------------------------------------------------------------------------- 1 | 0 2 | 0 3 | 0 4 | 0 5 | 0 6 | 0 7 | 0 8 | 0 9 | 0 10 | 0 11 | 0 12 | 0 13 | 0 14 | 0 15 | 0 16 | 0 17 | 0 18 | 0 19 | 0 20 | 0 21 | 0 22 | 0 23 | 0 24 | 0 25 | 0 26 | 0.109788 27 | 0.109788 28 | 0.109788 29 | 0.109788 30 | 0.0232869 31 | 0.0232869 32 | 0.0232869 33 | 0.0232869 34 | 0.0226532 35 | 0.0226532 36 | 0.0226532 37 | 0.0226532 38 | 0.00676088 39 | 0.00676088 40 | 0.00676088 41 | 0.00676088 42 | 0.00354337 43 | 0.00354337 44 | 0.00354337 45 | 0.00354337 46 | 0.0158006 47 | 0.0158006 48 | 0.0158006 49 | 0.0158006 50 | 0.018782 51 | 0.018782 52 | 0.018782 53 | 0.018782 54 | 0.0255352 55 | 0.0255352 56 | 0.0255352 57 | 0.0255352 58 | 0.018621 59 | 0.018621 60 | 0.018621 61 | 0.018621 62 | 0.0200791 63 | 0.0200791 64 | 0.0200791 65 | 0.0200791 66 | 0.0393991 67 | 0.0393991 68 | 0.0393991 69 | 0.0393991 70 | 0.0187284 71 | 0.0187284 72 | 0.0187284 73 | 0.0187284 74 | 0.0394334 75 | 0.0394334 76 | 0.0394334 77 | 0.0394334 78 | 0.0220953 79 | 0.0220953 80 | 0.0220953 81 | 0.0220953 82 | 0.0110226 83 | 0.0110226 84 | 0.0110226 85 | 0.0110226 86 | 0.0132243 87 | 0.0132243 88 | 0.0132243 89 | 0.0132243 90 | 0.0227436 91 | 0.0227436 92 | 0.0227436 93 | 0.0227436 94 | 0.00781753 95 | 0.00781753 96 | 0.00781753 97 | 0.00781753 98 | 0.0272355 99 | 0.0272355 100 | 0.0272355 101 | 0.0272355 102 | 0.00834333 103 | 0.00834333 104 | 0.00834333 105 | 0.00834333 106 | 0.0121419 107 | 0.0121419 108 | 0.0121419 109 | 0.0121419 110 | 0.0340096 111 | 0.0340096 112 | 0.0340096 113 | 0.0340096 114 | 0.0151723 115 | 0.0151723 116 | 0.0151723 117 | 0.0151723 118 | 0.0302939 119 | 0.0302939 120 | 0.0302939 121 | 0.0302939 122 | 0.0280349 123 | 0.0280349 124 | 0.0280349 125 | 0.0280349 126 | 0.0339183 127 | 0.0339183 128 | 0.0339183 129 | 0.0339183 130 | 0.011332 131 | 0.011332 132 | 0.011332 133 | 0.011332 134 | 0.0233435 135 | 0.0233435 136 | 0.0233435 137 | 0.0233435 138 | 0.0241227 139 | 0.0241227 140 | 0.0241227 141 | 0.0241227 142 | 0.0212235 143 | 0.0212235 144 | 0.0212235 145 | 0.0212235 146 | 0.016116 147 | 0.016116 148 | 0.016116 149 | 0.016116 150 | 0.0239523 151 | 0.0239523 152 | 0.0239523 153 | 0.0239523 154 | 0.0230835 155 | 0.0230835 156 | 0.0230835 157 | 0.0230835 158 | 0.0250707 159 | 0.0250707 160 | 0.0250707 161 | 0.0250707 162 | 0.0371718 163 | 0.0371718 164 | 0.0371718 165 | 0.0371718 166 | 0.0518114 167 | 0.0518114 168 | 0.0518114 169 | 0.0518114 170 | 0.0270872 171 | 0.0270872 172 | 0.0270872 173 | 0.0270872 174 | 0.0183459 175 | 0.0183459 176 | 0.0183459 177 | 0.0183459 178 | 0.0427403 179 | 0.0427403 180 | 0.0427403 181 | 0.0427403 182 | 0.0318441 183 | 0.0318441 184 | 0.0318441 185 | 0.0318441 186 | 0.0423584 187 | 0.0423584 188 | 0.0423584 189 | 0.0423584 190 | 0.0204876 191 | 0.0204876 192 | 0.0204876 193 | 0.0204876 194 | 0.0159696 195 | 0.0159696 196 | 0.0159696 197 | 0.0159696 198 | 0.0132789 199 | 0.0132789 200 | 0.0132789 201 | 0.0132789 202 | 0.0120181 203 | 0.0120181 204 | 0.0120181 205 | 0.0120181 206 | 0.0194127 207 | 0.0194127 208 | 0.0194127 209 | 0.0194127 210 | 0.0312255 211 | 0.0312255 212 | 0.0312255 213 | 0.0312255 214 | 0.0170354 215 | 0.0170354 216 | 0.0170354 217 | 0.0170354 218 | 0.0257152 219 | 0.0257152 220 | 0.0257152 221 | 0.0257152 222 | 0.0301831 223 | 0.0301831 224 | 0.0301831 225 | 0.0301831 226 | 0.0230433 227 | 0.0230433 228 | 0.0230433 229 | 0.0230433 230 | 0.0182437 231 | 0.0182437 232 | 0.0182437 233 | 0.0182437 234 | 0.0203225 235 | 0.0203225 236 | 0.0203225 237 | 0.0203225 238 | 0.0222449 239 | 0.0222449 240 | 0.0222449 241 | 0.0222449 242 | 0.0152555 243 | 0.0152555 244 | 0.0152555 245 | 0.0152555 246 | 0.00972222 247 | 0.00972222 248 | 0.00972222 249 | 0.00972222 250 | 0.0195565 251 | 0.0195565 252 | 0.0195565 253 | 0.0195565 254 | 0.0144112 255 | 0.0144112 256 | 0.0144112 257 | 0.0144112 258 | 0.0196969 259 | 0.0196969 260 | 0.0196969 261 | 0.0196969 262 | 0.0172075 263 | 0.0172075 264 | 0.0172075 265 | 0.0172075 266 | 0.0211793 267 | 0.0211793 268 | 0.0211793 269 | 0.0211793 270 | 0.0325992 271 | 0.0325992 272 | 0.0325992 273 | 0.0325992 274 | 0.0429205 275 | 0.0429205 276 | 0.0429205 277 | 0.0429205 278 | 0.0222745 279 | 0.0222745 280 | 0.0222745 281 | 0.0222745 282 | 0.0251552 283 | 0.0251552 284 | 0.0251552 285 | 0.0251552 286 | 0.0275784 287 | 0.0275784 288 | 0.0275784 289 | 0.0275784 290 | 0.0239184 291 | 0.0239184 292 | 0.0239184 293 | 0.0239184 294 | 0.0315766 295 | 0.0315766 296 | 0.0315766 297 | 0.0315766 298 | 0.0244353 299 | 0.0244353 300 | 0.0244353 301 | 0.0244353 302 | 0.0147248 303 | 0.0147248 304 | 0.0147248 305 | 0.0147248 306 | 0.014439 307 | 0.014439 308 | 0.014439 309 | 0.014439 310 | 0.0332814 311 | 0.0332814 312 | 0.0332814 313 | 0.0332814 314 | 0.0195762 315 | 0.0195762 316 | 0.0195762 317 | 0.0195762 318 | 0.0330638 319 | 0.0330638 320 | 0.0330638 321 | 0.0330638 322 | 0.0195918 323 | 0.0195918 324 | 0.0195918 325 | 0.0195918 326 | 0.0172913 327 | 0.0172913 328 | 0.0172913 329 | 0.0172913 330 | 0.0189412 331 | 0.0189412 332 | 0.0189412 333 | 0.0189412 334 | 0.0207437 335 | 0.0207437 336 | 0.0207437 337 | 0.0207437 338 | 0.0307108 339 | 0.0307108 340 | 0.0307108 341 | 0.0307108 342 | 0.0323403 343 | 0.0323403 344 | 0.0323403 345 | -------------------------------------------------------------------------------- /data/resultsfromcameraimucalibration.m: -------------------------------------------------------------------------------- 1 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_06_51.txt'); 2 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_11_20.txt'); 3 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_15_06.txt'); 4 | %I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_20_21.txt'); 5 | I_T_C_mocal = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/mocalResidual/I_T_C_2022_01_28_15_30_47.txt'); 6 | 7 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_06_51.txt'); 8 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_11_20.txt'); 9 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_15_06.txt'); 10 | %I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_20_21.txt'); 11 | I_T_C_repErr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/cam_imu_results/repErrResidual/I_T_C_2022_01_28_15_30_47.txt'); 12 | 13 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_06_51.txt'); 14 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_11_20.txt'); 15 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_15_06.txt'); 16 | %I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_20_21.txt'); 17 | I_T_C_kalibr = importdata('/home/usl/catkin_ws/src/camera_imu_lidar_calibration/data/kalibr_results/I_T_C_kalibr_2022_01_28_15_30_47.txt'); 18 | 19 | I_R_C_mocal = I_T_C_mocal(1:3, 1:3); 20 | I_t_C_mocal = I_T_C_mocal(1:3, 4)'; 21 | eul_mocal = rotm2eul(I_R_C_mocal)*180/pi; 22 | t_eul_trans_mocal = [eul_mocal, I_t_C_mocal] 23 | 24 | I_R_C_repErr = I_T_C_repErr(1:3, 1:3); 25 | I_t_C_repErr = I_T_C_repErr(1:3, 4)'; 26 | eul_repErr = rotm2eul(I_R_C_repErr)*180/pi; 27 | t_eul_trans_repErr = [eul_repErr, I_t_C_repErr] 28 | 29 | I_R_C_kalibr = I_T_C_kalibr(1:3, 1:3); 30 | I_t_C_kalibr = I_T_C_kalibr(1:3, 4)'; 31 | eul_kalibr = rotm2eul(I_R_C_kalibr)*180/pi; 32 | t_eul_trans_kalibr = [eul_kalibr, I_t_C_kalibr] -------------------------------------------------------------------------------- /launch/camera_imu_lidar_calibration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | [0.0, 0.0, 9.81] 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /launch/camera_imu_lidar_calibration_bagcreater.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/ros_calib_init.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /launch/ros_ndt_lodom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | camera_imu_lidar_calibration 5 | 1.0.0 6 | 7 | Camera-Inertial-Lidar Calibration 8 | 9 | 10 | 11 | Subodh Mishra 12 | Subodh Mishra 13 | 14 | 15 | GNU General Public License v3.0 16 | 17 | 18 | catkin 19 | 20 | 21 | cmake_modules 22 | roscpp 23 | rosbag 24 | tf 25 | std_msgs 26 | geometry_msgs 27 | sensor_msgs 28 | nav_msgs 29 | ndt_omp 30 | tf 31 | imuPacket 32 | pcl_ros 33 | pcl_conversions 34 | cv_bridge 35 | 36 | 37 | roscpp 38 | rosbag 39 | tf 40 | std_msgs 41 | geometry_msgs 42 | sensor_msgs 43 | nav_msgs 44 | ndt_omp 45 | tf 46 | imuPacket 47 | pcl_ros 48 | pcl_conversions 49 | cv_bridge 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /src/core/calibManager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 12/8/20. 3 | // 4 | 5 | #ifndef CALIB_ESTIMATOR_CALIBMANAGER_H 6 | #define CALIB_ESTIMATOR_CALIBMANAGER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "init/InertialInitializer.h" 15 | 16 | #include "state/State.h" 17 | #include "state/StateHelper.h" 18 | #include "state/Propagator.h" 19 | #include "update/UpdaterLidarOdometry.h" 20 | #include "update/UpdaterCameraTracking.h" 21 | #include "update/UpdaterCameraLidarConstraint.h" 22 | 23 | #include "track/lidarOdometry.h" 24 | #include "track/cameraPoseTracking.h" 25 | 26 | #include "track/lidarPlaneDetector.h" 27 | 28 | #include "calibManagerOptions.h" 29 | 30 | #include "utils/pcl_utils.h" 31 | 32 | namespace calib_estimator { 33 | /// Core class that manages the entire system 34 | class calibManager { 35 | public: 36 | /// Constructor that will load all configuration variables 37 | calibManager(calibManagerOptions& param_); 38 | 39 | /// Feed function for inertial data 40 | void feed_measurement_imu(double timestamp_imu, Eigen::Vector3d wm, Eigen::Vector3d am); 41 | 42 | /// Feed function for lidar data 43 | void feed_measurement_lidar(double timestamp_lidar, pcl::PointCloud::Ptr cloud_in); 44 | 45 | /// Feed function for camera data 46 | void feed_measurement_camera(double timestamp_camera, cv::Mat image_in); 47 | 48 | /// If we initialized or not 49 | bool initialized() { 50 | return is_initialized_calibrator; 51 | } 52 | 53 | /// Timestamp the system was initialized at 54 | double initialized_time() { 55 | return startup_time; 56 | } 57 | 58 | /// Accessor to get the current state 59 | State* get_state() { 60 | return state; 61 | } 62 | 63 | /// Accessor to get the current propagator 64 | Propagator* get_propagator() { 65 | return propagator; 66 | } 67 | 68 | /// Accessor to Lidar Odometry object 69 | LidarOdometry::Ptr get_track_lidar() { 70 | return LOdom; 71 | } 72 | 73 | /// Accessor to Camera Odometry object 74 | cameraPoseTracking::Ptr get_track_camera() { 75 | return cameraTracking; 76 | } 77 | 78 | /// Returns the last timestamp we have marginalized (true if we have a state) 79 | bool hist_last_marg_state(double ×tamp, Eigen::Matrix &stateinG) { 80 | if(hist_last_marginalized_time != -1) { 81 | timestamp = hist_last_marginalized_time; 82 | stateinG = hist_stateinG.at(hist_last_marginalized_time); 83 | return true; 84 | } else { 85 | timestamp = -1; 86 | stateinG.setZero(); 87 | return false; 88 | } 89 | } 90 | 91 | /// This will deskew a single point 92 | Eigen::Vector3d deskewPoint(Eigen::Matrix start_point_state, 93 | Eigen::Matrix current_point_state, 94 | Eigen::Vector3d skewedPoint, 95 | Eigen::Matrix3d I_R_L, 96 | Eigen::Vector3d I_t_L); 97 | 98 | /// This will deskew the entire scan/pointcloud 99 | void do_undistortion(double timestamp, 100 | pcl::PointCloud& scan_raw, 101 | pcl::PointCloud::Ptr& scan_out, 102 | Eigen::Matrix4d& T_ndt_predict); 103 | 104 | /// This function will try to initialize the state 105 | /// This function could also be repurposed to re-initialize the system after failure 106 | bool try_to_initialize(); 107 | 108 | /// Boolean if we are initialized or not 109 | bool is_initialized_calibrator = false; 110 | 111 | /// G_T_I0; 112 | Eigen::Matrix4d G_T_I0 = Eigen::Matrix4d::Identity(); 113 | 114 | /// G_T_I1; 115 | Eigen::Matrix4d G_T_I1 = Eigen::Matrix4d::Identity(); 116 | 117 | /// Get undistorted cloud 118 | VPointCloud get_undistorted_cloud() { 119 | return undistorted_cloud; 120 | } 121 | 122 | /// Get the time stamp of the first scan (used for building map) 123 | double get_map_time() { 124 | return map_time; 125 | } 126 | 127 | /// Print State for debugging 128 | void logData(); 129 | 130 | /// Get the points on the detected calibration board 131 | TPointCloud::Ptr getCalibrationBoardPoints(); 132 | 133 | /// Project points 134 | void projectLidarPointsOnImage(); 135 | 136 | protected: 137 | 138 | /// This will do propagation and updates 139 | // Lidar 140 | void do_propagate_update(double timestamp); 141 | // Camera 142 | bool do_propagate_update(double timestamp, bool boarddetected, cv::Mat image); 143 | // Lidar-Camera 144 | void do_lidar_camera_update(); 145 | 146 | ///The following will update our historical tracking information 147 | void update_keyframe_historical_information(); 148 | 149 | 150 | /// Manager of parameters 151 | calibManagerOptions params; 152 | 153 | /// Our master state object 154 | State* state; 155 | 156 | /// Propagator of our state 157 | Propagator* propagator; 158 | 159 | /// State initializer 160 | InertialInitializer* initializer; 161 | 162 | /// Lidar Updater 163 | UpdaterLidarOdometry* updaterLidarOdometry; 164 | 165 | /// Camera Updater 166 | UpdaterCameraTracking* updaterCameraTracking; 167 | 168 | /// Lidar-Camera Updater 169 | UpdaterCameraLidarConstraint* updaterCameraLidarConstraint; 170 | 171 | /// Lidar Odometry object (Tracker) 172 | LidarOdometry::Ptr LOdom; 173 | 174 | /// Camera Odometry object (Tracker) 175 | cameraPoseTracking::Ptr cameraTracking; 176 | 177 | /// Track the distance travelled 178 | double timelastupdate = -1; 179 | double distance = 0; 180 | 181 | /// Start-up time of the filter 182 | double startup_time = -1; 183 | 184 | /// Historical information of the filter 185 | double hist_last_marginalized_time = -1; 186 | std::map > hist_stateinG; 187 | 188 | std::ofstream trajfile_csv; 189 | std::ofstream bias_csv; 190 | std::ofstream velocity_csv; 191 | std::ofstream calib_lidar_imu_extrinsic_csv; 192 | std::ofstream calib_lidar_imu_dt_csv; 193 | std::ofstream calib_camera_imu_extrinsic_csv; 194 | std::ofstream calib_camera_imu_dt_csv; 195 | std::ofstream residual_filename_csv_writer; 196 | 197 | /// Raw Pointcloud 198 | TPointCloud raw_cloud; 199 | 200 | /// Undistorted Pointcloud 201 | VPointCloud undistorted_cloud; 202 | 203 | /// For Surfel Association 204 | double map_time; 205 | bool first_propagation = true; 206 | 207 | /// Update flags 208 | bool did_update_lidarimu_1 = false, did_update_lidarimu_2 = false; 209 | bool did_update_cameraimu = false; 210 | 211 | /// Reprojection Error for verification 212 | double reprojection_error; 213 | 214 | /// For pixel based update 215 | std::vector imagepoints; 216 | std::vector objectpoints_c0; 217 | 218 | /// Timestamps 219 | double timestamp_lidar; 220 | double timestamp_camera; 221 | 222 | /// Planar measurements 223 | std::map nd_c_vector; 224 | std::map nd_l_vector; 225 | 226 | /// Calibration board detector Lidar 227 | lidarPlaneDetector* lidar_plane_detector; 228 | TPointCloud::Ptr calibration_board_points; 229 | 230 | /// Camera image 231 | cv::Mat image_measurement; 232 | /// Camera params 233 | int image_height; 234 | int image_width; 235 | cv::Mat K, D; 236 | 237 | double residual_value = 0; 238 | }; 239 | } 240 | #endif //CALIB_ESTIMATOR_CALIBMANAGER_H 241 | -------------------------------------------------------------------------------- /src/core/calibManagerOptions.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 12/8/20. 3 | // 4 | 5 | #ifndef CALIB_ESTIMATOR_CALIBMANAGEROPTIONS_H 6 | #define CALIB_ESTIMATOR_CALIBMANAGEROPTIONS_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace std; 26 | using namespace calib_core; 27 | 28 | namespace calib_estimator { 29 | /// Struct which stores all options needed for state estimation 30 | struct calibManagerOptions { 31 | /// ESTIMATOR =============================== 32 | 33 | StateOptions state_options; 34 | 35 | // Amount of time we will initialize over (seconds) 36 | double init_window_time = 1.0; 37 | 38 | // Variance threshold on our acceleration to be classified as moving 39 | double init_imu_thresh = 1.0; 40 | 41 | // This function will print out all estimator settings loaded. 42 | void print_estimator() { 43 | printf("ESTIMATOR PARAMETERS:\n"); 44 | state_options.print(); 45 | printf("\t- init_window_time: %.2f\n", init_window_time); 46 | printf("\t- init_imu_thresh: %.2f\n", init_imu_thresh); 47 | } 48 | 49 | /// NOISE / CHI2 ============================ 50 | // IMU noise (gyroscope and accelerometer) 51 | Propagator::NoiseManager imu_noises; 52 | 53 | // Update options for estimator (noise and chi2 multiplier) 54 | UpdaterOptions updaterOptions; 55 | 56 | // This function will print out all noise parameters loaded. 57 | void print_noise() { 58 | printf("NOISE PARAMETERS:\n"); 59 | imu_noises.print(); 60 | printf("\tUpdater Estimator Feats:\n"); 61 | updaterOptions.print(); 62 | } 63 | 64 | /// STATE DEFAULTS ============================ 65 | // Gravity in global frame 66 | Eigen::Vector3d gravity = {0.0, 0.0, 9.81}; 67 | 68 | // Time offset between lidar and IMU 69 | double calib_lidar_imu_dt = 0.0; 70 | // Lidar IMU extrinsics (q_LtoI, p_LinI). Note the 71 | // difference between "to" and "in" 72 | Eigen::Matrix lidar_imu_extrinsics; 73 | 74 | // Time offset between camera and IMU 75 | double calib_camera_imu_dt = 0.0; 76 | // Lidar IMU extrinsics (q_CtoI, p_CinI). Note the 77 | // difference between "to" and "in" 78 | Eigen::Matrix camera_imu_extrinsics; 79 | 80 | // This function will print out all state defaults loaded. 81 | void print_state() { 82 | printf("STATE PARAMETERS:\n"); 83 | printf("\t- gravity: %.3f, %.3f, %.3f\n", gravity(0), gravity(1), gravity(2)); 84 | printf("\t- calib_camimu_dt: %.4f\n", calib_camera_imu_dt); 85 | std::cout << "cam_imu_extrinsic(0:3):" << endl << camera_imu_extrinsics.block(0,0,4,1).transpose() << std::endl; 86 | std::cout << "cam_imu_extrinsic(4:6):" << endl << camera_imu_extrinsics.block(4,0,3,1).transpose() << std::endl; 87 | printf("\t- calib_lidar_imu_dt: %.4f\n", calib_lidar_imu_dt); 88 | std::cout << "lidar_imu_extrinsic(0:3):" << endl << lidar_imu_extrinsics.block(0,0,4,1).transpose() << std::endl; 89 | std::cout << "lidar_imu_extrinsic(4:6):" << endl << lidar_imu_extrinsics.block(4,0,3,1).transpose() << std::endl; 90 | } 91 | 92 | /// LIDAR Odometry (Tracker) 93 | // Resolution of space in 3D for doing NDT based matching 94 | double ndtResolution = 0.5; 95 | 96 | /// Camera Pose Tracking 97 | double checkerboard_dx = 0.05; // 5 cm 98 | double checkerboard_dy = 0.05; // 5 cm 99 | int checkerboard_rows = 6; 100 | int checkerboard_cols = 7; 101 | std::string camera_calibration_file_path = "/home/usl/catkin_ws/src/camera_imu_calibration/config/pylon_calib.yaml"; 102 | void print_trackers() { 103 | printf("LIDAR Odometry PARAMETERS:\n"); 104 | printf("\t- ndtResolution: %.3f\n", ndtResolution); 105 | printf("CAMERA Tracking PARAMETERS:\n"); 106 | printf("checkerboard_dx: %0.4f\n", checkerboard_dx); 107 | printf("checkerboard_dy: %0.4f\n", checkerboard_dy); 108 | printf("checkerboard_rows: %d\n", checkerboard_rows); 109 | printf("checkerboard_cols: %d\n", checkerboard_cols); 110 | printf("camera_calibration_file_path: %s\n", camera_calibration_file_path.c_str()); 111 | } 112 | 113 | bool do_undistortion = true; 114 | 115 | /// CSV file as output 116 | std::string inertial_trajectory_filename; 117 | std::string inertial_bias_filename; 118 | std::string inertial_velocity_filename; 119 | std::string lidar_inertial_calib_extrinsic_filename; 120 | std::string lidar_inertial_calib_dt_filename; 121 | std::string visual_inertial_calib_extrinsic_filename; 122 | std::string visual_inertial_calib_dt_filename; 123 | std::string lidar_odometry_trajectory_filename; 124 | 125 | /// Scan output folder names 126 | std::string raw_scan_folder_name; 127 | std::string deskewed_scan_folder_name; 128 | 129 | /// Initial lidar inertial calibration result filename 130 | std::string init_lidar_inertial_calibration_result_filename; 131 | 132 | /// Final lidar inertial calibration result filename 133 | std::string lidar_inertial_calibration_result_filename; 134 | 135 | /// Initial camera inertial calibration result filename 136 | std::string init_camera_inertial_calibration_result_filename; 137 | 138 | /// Final camera inertial calibration result filename 139 | std::string camera_inertial_calibration_result_filename; 140 | 141 | /// camera lidar calibration result filename 142 | std::string camera_lidar_calibration_result_filename; 143 | 144 | /// Residual 145 | std::string residual_filename; 146 | 147 | /// Map size params 148 | bool limit_map_size = true; 149 | int no_of_scans_for_map = 100; 150 | 151 | /// Map clouddownsampling flag 152 | bool downSampleForMapping = false; 153 | 154 | /// related to map generation 155 | bool gen_map_data = false; 156 | }; 157 | }; 158 | #endif //CALIB_ESTIMATOR_CALIBMANAGEROPTIONS_H 159 | -------------------------------------------------------------------------------- /src/init/InertialInitializer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/28/20. 3 | // 4 | 5 | #include "InertialInitializer.h" 6 | 7 | using namespace calib_core; 8 | 9 | void InertialInitializer::feed_imu(double timestamp, Eigen::Matrix wm, Eigen::Matrix am) { 10 | /// Create our IMU data object 11 | IMUDATA data; 12 | data.timestamp = timestamp; 13 | data.wm = wm; 14 | data.am = am; 15 | 16 | /// Append it our vector 17 | imu_data.emplace_back(data); 18 | 19 | /// Delete all measurements older than three of our initialization windows 20 | auto it0 = imu_data.begin(); 21 | while (it0 != imu_data.end() && it0->timestamp < timestamp - 3*_window_length) { 22 | it0 = imu_data.erase(it0); 23 | } 24 | } 25 | 26 | bool InertialInitializer::initialize_with_imu(double &time0, /// Timestamp that the returned state is at 27 | Eigen::Matrix &q_GtoI0, /// q_GtoI0 Orientation at the initialization 28 | Eigen::Matrix &b_w0, /// b_w0 gyro bias at initialization 29 | Eigen::Matrix &v_I0inG, /// v_I0inG Velocity at initialization 30 | Eigen::Matrix &b_a0, /// b_a0 Acceleration bias at initialization 31 | Eigen::Matrix &p_I0inG, /// p_I0inG Position at initialization 32 | bool wait_for_jerk) /// wait_for_jerk if true we will wait for a "jerk" 33 | { 34 | /// Return if we do not have any measurements 35 | if(imu_data.empty()) { 36 | printf(YELLOW "[InertialInitializer::initialize_with_imu] Cannot attempt to initialize as we have no imu data \n" RESET); 37 | return false; 38 | } 39 | 40 | /// Newest IMU timestamp 41 | double newesttime = imu_data.at(imu_data.size()-1).timestamp; 42 | 43 | /// First, lets collect two windows of IMU readings 44 | /// One is from the newest measurement to 1 window_length back in time 45 | /// Second is from 1 window_length back in time to 2 window_lengths back in time 46 | std::vector window_newest, window_secondnew; 47 | for(IMUDATA data : imu_data) { 48 | if(data.timestamp > newesttime - 1*_window_length && data.timestamp <= newesttime) { 49 | window_newest.emplace_back(data); 50 | } 51 | if(data.timestamp > newesttime - 2*_window_length && data.timestamp <= newesttime - 1*_window_length) { 52 | window_secondnew.emplace_back(data); 53 | } 54 | } 55 | 56 | /// Return if either of the windows is empty 57 | if(window_secondnew.empty() || window_newest.empty()) { 58 | printf(YELLOW "[InertialInitializer::initialize_with_imu] One of the windows is empty, not enough readings \n" RESET); 59 | return false; 60 | } 61 | 62 | /// Calculate the sample variance for the newest window 63 | Eigen::Matrix a_avg = Eigen::Matrix::Zero(); 64 | for(IMUDATA data : window_newest) { 65 | a_avg += data.am; 66 | } 67 | a_avg /= (int)window_newest.size(); 68 | double a_var = 0; 69 | for(IMUDATA data : window_newest){ 70 | a_var += (data.am - a_avg).dot(data.am - a_avg); 71 | } 72 | a_var /= ((int)window_newest.size()-1); 73 | double a_stdev = std::sqrt(a_var); 74 | 75 | /// If the above standard dev is below a threshold and we are waiting for a "jerk" then we want to wait till it is exceeded 76 | if(a_stdev < _imu_excite_threshold && wait_for_jerk) { 77 | printf(YELLOW "[InertialInitializer::initialize_with_imu] Not enough IMU excitation, below threshold %0.4f < %0.4f\n" RESET, a_var, _imu_excite_threshold); 78 | return false; 79 | } 80 | 81 | /// Sum of the linear and angular readings from the window with older data (the secondnew window) 82 | Eigen::Matrix linsum = Eigen::Matrix::Zero(); 83 | Eigen::Matrix angsum = Eigen::Matrix::Zero(); 84 | for(IMUDATA data : window_secondnew) { 85 | linsum += data.am; 86 | angsum += data.wm; 87 | } 88 | Eigen::Matrix linavg = linsum/window_secondnew.size(); 89 | Eigen::Matrix angavg = angsum/window_secondnew.size(); 90 | /// Calculate variance and stdev of the second window 91 | double a_var2 = 0; 92 | for(IMUDATA data : window_secondnew) { 93 | a_var2 += (data.am - linavg).dot((data.am - linavg)); 94 | } 95 | a_var2 /= ((int)window_secondnew.size()-1); 96 | double a_stdev2 = std::sqrt(a_var2); 97 | 98 | /// If the second window is above the excitation threshold and we are not waiting for a jerk to happen then 99 | /// We must be stationary, so we return from here and wait until we are stationary 100 | if((a_stdev > _imu_excite_threshold || a_stdev2 > _imu_excite_threshold) && !wait_for_jerk) { 101 | printf(YELLOW "[InertialInitializer::initialize_with_imu] too much IMU excitation when we are not waiting for jerk, above threshold %.4f,%.4f > %.4f\n" RESET,a_var,a_var2,_imu_excite_threshold); 102 | return false; 103 | } 104 | 105 | /// Get Z axis 106 | Eigen::Matrix z_axis = linavg/linavg.norm(); 107 | /// Create an X axis 108 | Eigen::Matrix e_1; 109 | e_1 << 1, 0, 0; 110 | Eigen::Matrix x_axis = Eigen::Matrix::Zero(); 111 | x_axis = e_1 - z_axis*z_axis.transpose()*e_1; 112 | x_axis = x_axis/x_axis.norm(); 113 | /// Create an Y axis 114 | Eigen::Matrix y_axis = Eigen::Matrix::Zero(); 115 | y_axis = skew_x(z_axis)*x_axis; 116 | 117 | /// From these get the Rotation matrix 118 | Eigen::Matrix Ro; 119 | Ro.block(0, 0, 3, 1) = x_axis; 120 | Ro.block(0, 1, 3, 1) = y_axis; 121 | Ro.block(0, 2, 3, 1) = z_axis; 122 | 123 | /// Create our state variables 124 | Eigen::Matrix q_GtoI = rot_2_quat(Ro); 125 | 126 | /// Set the biases equal to the sample mean calculated 127 | Eigen::Matrix bg = angavg; 128 | Eigen::Matrix ba = linavg - Ro*_gravity; 129 | 130 | /// Set our state variables 131 | time0 = window_secondnew.at(window_secondnew.size()-1).timestamp; 132 | q_GtoI0 = q_GtoI; 133 | b_w0 = bg; 134 | v_I0inG = Eigen::Matrix::Zero(); 135 | b_a0 = ba; 136 | p_I0inG = Eigen::Matrix::Zero(); 137 | 138 | printf(GREEN "[InertialInitializer::initialize_with_imu] Initialization done!!!!\n"); 139 | return true; 140 | } -------------------------------------------------------------------------------- /src/init/InertialInitializer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/28/20. 3 | // 4 | 5 | #ifndef CALIB_CORE_INERTIALINITIALIZER_H 6 | #define CALIB_CORE_INERTIALINITIALIZER_H 7 | 8 | #include 9 | 10 | #include "utils/quat_ops.h" 11 | #include "utils/color.h" 12 | 13 | namespace calib_core { 14 | class InertialInitializer { 15 | public: 16 | /// Struct for a single imu measurement 17 | struct IMUDATA { 18 | /// Timestamp of the reading 19 | double timestamp; 20 | /// Gyroscope reading: angular velocity (rad/s) 21 | Eigen::Matrix wm; 22 | /// Accelerometer reading: linear acceleration (m/s^2) 23 | Eigen::Matrix am; 24 | }; 25 | 26 | /// Default constructor 27 | InertialInitializer(Eigen::Matrix gravity, double window_length, double imu_excite_threshold): 28 | _gravity(gravity), _window_length(window_length), _imu_excite_threshold(imu_excite_threshold) {} 29 | 30 | /// Stores incoming imu readings 31 | void feed_imu(double time_stamp, Eigen::Matrix wm, Eigen::Matrix am); 32 | 33 | /// Try to initialize the system using just the imu 34 | /// Returns true if we have successfully initialized our system 35 | bool initialize_with_imu(double &time0, /// Timestamp that the returned state is at 36 | Eigen::Matrix &q_GtoI0, /// q_GtoI0 Orientation at the initialization 37 | Eigen::Matrix &b_w0, /// b_w0 gyro bias at initialization 38 | Eigen::Matrix &v_I0inG, /// v_I0inG Velocity at initialization 39 | Eigen::Matrix &b_a0, /// b_a0 Acceleration bias at initialization 40 | Eigen::Matrix &p_I0inG, /// p_I0inG Position at initialization 41 | bool wait_for_jerk = true); /// wait_for_jerk if true we will wait for a "jerk" 42 | 43 | protected: 44 | /// Gravity vector 45 | Eigen::Matrix _gravity; 46 | /// Amount of time we will initialize over (seconds) 47 | double _window_length; 48 | /// Variance threshold on our acceleration to be classified as moving 49 | double _imu_excite_threshold; 50 | /// Our history of IMU messages (time, angular, linear) 51 | std::vector imu_data; 52 | }; 53 | }; 54 | #endif //CALIB_CORE_INERTIALINITIALIZER_H 55 | -------------------------------------------------------------------------------- /src/relpose/relativePose.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/29/20. 3 | // 4 | 5 | #ifndef CALIB_RELATIVEPOSE_H 6 | #define CALIB_RELATIVEPOSE_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "types/Pose.h" 12 | 13 | using namespace calib_type; 14 | 15 | namespace calib_core { 16 | class relativePose { 17 | public: 18 | /// Time stamp of scan i 19 | double timestamp_i; 20 | /// Time stamp of scan j 21 | double timestamp_j; 22 | /// Odometry pose 23 | Eigen::Matrix4d odometry_ij; 24 | }; 25 | } 26 | #endif //CALIB_RELATIVEPOSE_H 27 | -------------------------------------------------------------------------------- /src/ros_camera_imu_lidar_calibration_bag_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 3/28/22. 3 | // 4 | 5 | 6 | #include "ros/ros.h" 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "ros/ros.h" 20 | #include "std_msgs/String.h" 21 | 22 | #include 23 | 24 | /** 25 | * This tutorial demonstrates simple sending of messages over the ROS system. 26 | */ 27 | int main(int argc, char **argv){ 28 | ros::init(argc, argv, "ros_camera_imu_lidar_calibration_bag_generator"); 29 | ros::NodeHandle nh("~"); 30 | 31 | /// Our topics (IMU and LIDAR) 32 | std::string topic_imu; 33 | std::string topic_lidar; 34 | std::string topic_camera; 35 | nh.param("topic_imu", topic_imu, "/imu"); 36 | nh.param("topic_lidar", topic_lidar, "/lidar"); 37 | nh.param("topic_camera", topic_camera, "/camera"); 38 | 39 | /// Location of the ROS bag we want to read in 40 | std::string path_to_bag; 41 | std::string path_out_bag; 42 | nh.param("path_bag", path_to_bag, "/home/usl/datasets/ouster_vectornav.bag"); 43 | ROS_INFO_STREAM("ROS BAG PATH is: " << path_to_bag.c_str()); 44 | nh.param("path_out_bag", path_out_bag, "/home/usl/datasets/ouster_vectornav.bag"); 45 | ROS_INFO_STREAM("ROS Output BAG PATH is: " << path_out_bag.c_str()); 46 | 47 | /// Get our start location and how much of the bag we want to play 48 | /// Make the bag duration < 0 to just process to the end of the bag 49 | double bag_start, bag_durr; 50 | nh.param("bag_start", bag_start, 0); 51 | nh.param("bag_durr", bag_durr, -1); 52 | ROS_INFO_STREAM("bag start: " << bag_start); 53 | ROS_INFO_STREAM("bag duration: " << bag_durr); 54 | 55 | /// Load rosbag here, and find messages we can play 56 | rosbag::Bag bag; 57 | bag.open(path_to_bag, rosbag::bagmode::Read); 58 | 59 | /// We should load the bag as a view 60 | /// Here we go from beginning of the bag to the end of the bag 61 | rosbag::View view_full; 62 | rosbag::View view; 63 | 64 | /// Start a few seconds in from the full view time 65 | /// If we have a negative duration then use the full bag length 66 | view_full.addQuery(bag); 67 | ros::Time time_init = view_full.getBeginTime(); 68 | time_init += ros::Duration(bag_start); 69 | ros::Time time_finish = 70 | (bag_durr < 0) ? view_full.getEndTime() : time_init + ros::Duration(bag_durr); 71 | ROS_INFO_STREAM("Time start = " << time_init.toSec()); 72 | ROS_INFO_STREAM("Time end = " << time_finish.toSec()); 73 | view.addQuery(bag, time_init, time_finish); 74 | 75 | /// Check to make sure we have data to play 76 | if (view.size() == 0) { 77 | ROS_ERROR_STREAM("No messages to play on specified topics. Exiting."); 78 | ros::shutdown(); 79 | return EXIT_FAILURE; 80 | } 81 | bool first_run = true; 82 | double timestamp_first_frame = 0; 83 | double timestamp_current = 0; 84 | double timestamp_first_m = 0; 85 | double timestamp_current_m = 0; 86 | 87 | /// 88 | rosbag::Bag* bag_out; 89 | bag_out = new rosbag::Bag(path_out_bag.c_str(), rosbag::bagmode::Write); 90 | 91 | for (const rosbag::MessageInstance& m : view) { 92 | /// If ROS wants us to stop, break out 93 | if (!ros::ok()) 94 | break; 95 | sensor_msgs::Imu::ConstPtr s_imu = m.instantiate(); 96 | sensor_msgs::PointCloud2::ConstPtr s_lidar = m.instantiate(); 97 | sensor_msgs::Image::ConstPtr s_image = m.instantiate(); 98 | std::string sensor_name = ""; 99 | if (s_imu != nullptr && m.getTopic() == topic_imu || s_lidar != nullptr && m.getTopic() == topic_lidar 100 | || s_image != nullptr && m.getTopic() == topic_camera) { 101 | if(s_imu != nullptr) { 102 | bag_out->write("/vectornav/IMU", (*s_imu).header.stamp, s_imu); 103 | } else if (s_image != nullptr) { 104 | bag_out->write("/pylon_camera_node/image_raw", (*s_image).header.stamp, s_image); 105 | } else { 106 | bag_out->write("/os_cloud_node/points", (*s_lidar).header.stamp, s_lidar); 107 | } 108 | } 109 | } 110 | bag_out->close(); 111 | ROS_INFO_STREAM("Created bag file"); 112 | return EXIT_SUCCESS; 113 | } -------------------------------------------------------------------------------- /src/state/Propagator.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/6/20. 3 | // 4 | 5 | #ifndef CALIB_PROPAGATOR_H 6 | #define CALIB_PROPAGATOR_H 7 | 8 | #include "State.h" 9 | #include "StateHelper.h" 10 | #include "utils/quat_ops.h" 11 | #include "utils/color.h" 12 | 13 | using namespace calib_core; 14 | namespace calib_estimator { 15 | /// Performs the state covariance and mean propagation using IMU measurement 16 | class Propagator { 17 | public: 18 | /// Struct for a single IMU measurement (time, w_m, a_m) 19 | struct IMUDATA { 20 | /// Timestamp of the reading 21 | double timestamp; 22 | 23 | /// Gyroscope reading, angular velocity (rad/s) 24 | Eigen::Matrix wm; 25 | 26 | /// Accelerometer reading, linear acceleration (m/s^2) 27 | Eigen::Matrix am; 28 | }; 29 | 30 | /// Struct of imu noise parameters 31 | struct NoiseManager { 32 | /// Gyroscope white noise (rad/s/sqrt(hz)) 33 | double sigma_w = 1.6968e-04; 34 | /// Gyroscope random walk (rad/s^2/sqrt(hz)) 35 | double sigma_wb = 1.9393e-05; 36 | /// Accelerometer white noise (m/s^2/sqrt(hz)) 37 | double sigma_a = 2.0000e-3; 38 | /// Accelerometer random walk (m/s^3/sqrt(hz)) 39 | double sigma_ab = 3.0000e-03; 40 | /// Print function of noise parameters loaded 41 | void print() { 42 | printf("\t- gyroscope_noise white noise: %.6f\n", sigma_w); 43 | printf("\t- accelerometer_noise white noise: %.5f\n", sigma_a); 44 | printf("\t- gyroscope_random_walk noise: %.7f\n", sigma_wb); 45 | printf("\t- accelerometer_random_walk noise: %.7f\n", sigma_ab); 46 | } 47 | }; 48 | /// Default constructor 49 | Propagator(NoiseManager noises, Eigen::Vector3d gravity) { 50 | _noises = noises; 51 | _gravity = gravity; 52 | last_prop_time_offset = 0.0; 53 | } 54 | /// Stores incoming inertial readings 55 | void feed_imu(double timestamp, /// timestamp of any imu reading 56 | Eigen::Vector3d wm, /// Gyro angular reading 57 | Eigen::Vector3d am) /// Accelerometer reading 58 | { 59 | /// Create our imu data object 60 | IMUDATA data; 61 | data.timestamp = timestamp; 62 | data.wm = wm; 63 | data.am = am; 64 | 65 | /// Append it to our vector 66 | imu_data.push_back(data); 67 | 68 | /// Loop through and delete IMU messages that are older than 20 seconds 69 | auto it0 = imu_data.begin(); 70 | while(it0 != imu_data.end()) { 71 | if(timestamp-(*it0).timestamp > 20) 72 | it0 = imu_data.erase(it0); 73 | else 74 | it0++; 75 | } 76 | } 77 | /// Propagate state up to given timestamp and then clone 78 | /** 79 | * This will first collect all imu readings that occured between the 80 | * *current* state time and the new time we want the state to be at. 81 | * If we don't have any imu readings we will try to extrapolate into the future. 82 | * After propagating the mean and covariance using our dynamics, 83 | * We clone the current imu pose as a new clone in our state. 84 | */ 85 | void propagate_and_clone(State *state, /// Pointer to state 86 | double timestamp); /// Time to propagate to and clone at 87 | /// Gets what the state and its covariance will be at a given timestamp 88 | /** 89 | * This can be used to find what the state will be in the "future" without propagating it. 90 | * This will propagate a clone of the current IMU state and its covariance matrix. 91 | * This is typically used to provide high frequency pose estimates between updates. 92 | */ 93 | void fast_state_propagate(State *state, /// Pointer to state 94 | double timestamp, /// Time to propagate to 95 | Eigen::Matrix &state_plus); /// The propagated state (q_GtoI, p_IinG, v_IinG, w_IinI) 96 | 97 | void fast_state_propagate(Eigen::Matrix state, 98 | Eigen::Matrix bg, 99 | Eigen::Matrix ba, 100 | Eigen::Matrix& state_plus, 101 | double time_0, 102 | double time_1); 103 | 104 | /// Function that given current imu_data, will select imu readings between two times 105 | /** 106 | * This will create measurements that we will integrate with, and an extra measurement at the end. 107 | * We use the @ref interpolate_data() function to "cut" the imu readings at the begining and end of the integration. 108 | * The timestamps passed should already take into account the time offset values. 109 | */ 110 | static std::vector select_IMU_readings(const std::vector& imu_data, /// IMU data we will select measurements from 111 | double time0, /// Start timestamp 112 | double time1); /// End timestamp 113 | /// Helper function that will linearly interpolate IMU readings 114 | /** 115 | * This should be used instead of just "cutting" imu messages that bound the camera times 116 | * Give better time offset if we use this function, could try other orders/splines if the imu is slow. 117 | */ 118 | static IMUDATA interpolate_data(const IMUDATA imu_1, /// imu at begining of interpolation interval 119 | const IMUDATA imu_2, /// imu at end of interpolation interval 120 | double timestamp) /// Timestamp being interpolated to 121 | { 122 | /// time-distant lambda 123 | double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp); 124 | /// interpolate between the two times 125 | IMUDATA data; 126 | data.timestamp = timestamp; 127 | data.am = (1-lambda) * imu_1.am + lambda * imu_2.am; 128 | data.wm = (1-lambda) * imu_1.wm + lambda * imu_2.wm; 129 | return data; 130 | } 131 | 132 | std::vector get_imu_data() { 133 | return imu_data; 134 | } 135 | 136 | protected: 137 | /// Estimate for time offset at last propagation time 138 | double last_prop_time_offset = -INFINITY; 139 | bool have_last_prop_time_offset = false; 140 | /// Propagates the state forward using the IMU data and computes the noise covariance and state transition 141 | void predict_and_compute(State *state, /// Pointer to state 142 | const IMUDATA data_minus, /// IMU readings at the beginning of interval 143 | const IMUDATA data_plus, /// IMU readings at the end of the interval 144 | Eigen::Matrix &F, /// State-transition matrix over the interval 145 | Eigen::Matrix &Qd); /// Discrete-time noise 146 | /// Discrete imu mean propagation 147 | void predict_mean_discrete(State *state, /// Pointer to state 148 | double dt, /// Time we should integrate over 149 | const Eigen::Vector3d &w_hat1, /// Angular velocity with bias removed 150 | const Eigen::Vector3d &a_hat1, /// Linear acceleration with bias removed 151 | const Eigen::Vector3d &w_hat2, /// Next angular velocity with bias removed 152 | const Eigen::Vector3d &a_hat2, /// Next linear acceleration with bias removed 153 | Eigen::Vector4d &new_q, /// The resulting new orientation after integration 154 | Eigen::Vector3d &new_v, /// The resulting new velocity after integration 155 | Eigen::Vector3d &new_p);/// The resulting new position after integration 156 | /// Runga Kutta 4 imu mean propagation 157 | void predict_mean_rk4(State *state, /// Pointer to state 158 | double dt, /// Time we should integrate over 159 | const Eigen::Vector3d &w_hat1, /// Angular velocity with bias removed 160 | const Eigen::Vector3d &a_hat1, /// Linear acceleration with bias removed 161 | const Eigen::Vector3d &w_hat2, /// Next angular velocity with bias removed 162 | const Eigen::Vector3d &a_hat2, /// Next linear acceleration with bias removed 163 | Eigen::Vector4d &new_q, /// The resulting new orientation after integration 164 | Eigen::Vector3d &new_v, /// The resulting new velocity after integration 165 | Eigen::Vector3d &new_p);/// The resulting new position after integration 166 | /// Container for the noise values 167 | NoiseManager _noises; 168 | /// Our history of IMU messages (time, angular, linear) 169 | std::vector imu_data; 170 | /// Gravity vector 171 | Eigen::Matrix _gravity; 172 | }; 173 | }; 174 | #endif //CALIB_PROPAGATOR_H 175 | 176 | -------------------------------------------------------------------------------- /src/state/State.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/6/20. 3 | // 4 | 5 | #include "State.h" 6 | 7 | using namespace calib_core; 8 | using namespace calib_estimator; 9 | 10 | State::State(StateOptions &options) { 11 | // Save our options 12 | _options = options; 13 | 14 | // Append the imu to the state and covariance 15 | int current_id = 0; 16 | _imu = new IMU(); 17 | _imu->set_local_id(current_id); 18 | _variables.push_back(_imu); 19 | current_id += _imu->size(); 20 | 21 | // Lidar to IMU time offset 22 | _calib_dt_LIDARtoIMU = new Vec(1); 23 | if (_options.do_calib_lidar_imu_timeoffset) { 24 | _calib_dt_LIDARtoIMU->set_local_id(current_id); 25 | _variables.push_back(_calib_dt_LIDARtoIMU); 26 | current_id += _calib_dt_LIDARtoIMU->size(); 27 | } 28 | 29 | // Allocate lidar imu extrinsic transform 30 | _calib_LIDARtoIMU = new Pose(); 31 | if(_options.do_calib_lidar_imu_extrinsic) { 32 | _calib_LIDARtoIMU->set_local_id(current_id); 33 | _variables.push_back(_calib_LIDARtoIMU); 34 | current_id += _calib_LIDARtoIMU->size(); 35 | } 36 | 37 | // Camera to IMU time offset 38 | _calib_dt_CAMERAtoIMU = new Vec(1); 39 | if (_options.do_calib_camera_imu_timeoffset) { 40 | _calib_dt_CAMERAtoIMU->set_local_id(current_id); 41 | _variables.push_back(_calib_dt_CAMERAtoIMU); 42 | current_id += _calib_dt_CAMERAtoIMU->size(); 43 | } 44 | 45 | // Allocate camera imu extrinsic transform 46 | _calib_CAMERAtoIMU = new Pose(); 47 | if(_options.do_calib_camera_imu_extrinsic) { 48 | _calib_CAMERAtoIMU->set_local_id(current_id); 49 | _variables.push_back(_calib_CAMERAtoIMU); 50 | current_id += _calib_CAMERAtoIMU->size(); 51 | } 52 | 53 | // Finally initialize our covariance to small value 54 | _Cov = 1e-3*Eigen::MatrixXd::Identity(current_id, current_id); 55 | 56 | // Finally, set some of our priors for our calibration parameters 57 | /// Lidar-IMU 58 | if (_options.do_calib_lidar_imu_timeoffset) { 59 | _Cov(_calib_dt_LIDARtoIMU->id(),_calib_dt_LIDARtoIMU->id()) = std::pow(_options.time_offset_noise_lidarimu, 2); 60 | } 61 | 62 | if(_options.do_calib_lidar_imu_extrinsic) { 63 | double rot_x_cov = std::pow(_options.rot_x_noise_lidarimu, 2); 64 | double rot_y_cov = std::pow(_options.rot_y_noise_lidarimu, 2); 65 | double rot_z_cov = std::pow(_options.rot_z_noise_lidarimu, 2); 66 | Eigen::MatrixXd Rot_cov = Eigen::MatrixXd::Identity(3, 3); 67 | Rot_cov(0, 0) = rot_x_cov; 68 | Rot_cov(1, 1) = rot_y_cov; 69 | Rot_cov(2, 2) = rot_z_cov; 70 | 71 | double trans_x_cov = std::pow(_options.trans_x_noise_lidarimu, 2); 72 | double trans_y_cov = std::pow(_options.trans_y_noise_lidarimu, 2); 73 | double trans_z_cov = std::pow(_options.trans_z_noise_lidarimu, 2); 74 | Eigen::MatrixXd Trans_cov = Eigen::MatrixXd::Identity(3, 3); 75 | Trans_cov(0, 0) = trans_x_cov; 76 | Trans_cov(1, 1) = trans_y_cov; 77 | Trans_cov(2, 2) = trans_z_cov; 78 | 79 | std::cout << "Rot_cov \n" << std::endl; 80 | std::cout << Rot_cov << std::endl; 81 | 82 | std::cout << "Trans_cov \n" << std::endl; 83 | std::cout << Trans_cov << std::endl; 84 | 85 | _Cov.block(_calib_LIDARtoIMU->id(),_calib_LIDARtoIMU->id(),3,3) = Rot_cov; 86 | _Cov.block(_calib_LIDARtoIMU->id()+3,_calib_LIDARtoIMU->id()+3,3,3) = Trans_cov; 87 | } 88 | 89 | /// Camera-IMU 90 | if (_options.do_calib_camera_imu_timeoffset) { 91 | _Cov(_calib_dt_CAMERAtoIMU->id(),_calib_dt_CAMERAtoIMU->id()) = std::pow(_options.time_offset_noise_cameraimu, 2); 92 | } 93 | 94 | if(_options.do_calib_camera_imu_extrinsic) { 95 | double rot_x_cov = std::pow(_options.rot_x_noise_cameraimu, 2); 96 | double rot_y_cov = std::pow(_options.rot_y_noise_cameraimu, 2); 97 | double rot_z_cov = std::pow(_options.rot_z_noise_cameraimu, 2); 98 | Eigen::MatrixXd Rot_cov = Eigen::MatrixXd::Identity(3, 3); 99 | Rot_cov(0, 0) = rot_x_cov; 100 | Rot_cov(1, 1) = rot_y_cov; 101 | Rot_cov(2, 2) = rot_z_cov; 102 | 103 | double trans_x_cov = std::pow(_options.trans_x_noise_cameraimu, 2); 104 | double trans_y_cov = std::pow(_options.trans_y_noise_cameraimu, 2); 105 | double trans_z_cov = std::pow(_options.trans_z_noise_cameraimu, 2); 106 | Eigen::MatrixXd Trans_cov = Eigen::MatrixXd::Identity(3, 3); 107 | Trans_cov(0, 0) = trans_x_cov; 108 | Trans_cov(1, 1) = trans_y_cov; 109 | Trans_cov(2, 2) = trans_z_cov; 110 | 111 | std::cout << "Rot_cov \n" << std::endl; 112 | std::cout << Rot_cov << std::endl; 113 | 114 | std::cout << "Trans_cov \n" << std::endl; 115 | std::cout << Trans_cov << std::endl; 116 | 117 | _Cov.block(_calib_CAMERAtoIMU->id(),_calib_CAMERAtoIMU->id(),3,3) = Rot_cov; 118 | _Cov.block(_calib_CAMERAtoIMU->id()+3,_calib_CAMERAtoIMU->id()+3,3,3) = Trans_cov; 119 | } 120 | } -------------------------------------------------------------------------------- /src/state/State.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/6/20. 3 | // 4 | 5 | #ifndef CALIB_STATE_H 6 | #define CALIB_STATE_H 7 | 8 | #include 9 | #include 10 | 11 | #include "types/Type.h" 12 | #include "types/IMU.h" 13 | #include "types/Vec.h" 14 | #include "types/Pose.h" 15 | #include "StateOptions.h" 16 | 17 | 18 | using namespace calib_core; 19 | using namespace calib_type; 20 | 21 | namespace calib_estimator { 22 | class State { 23 | public: 24 | State(StateOptions &options_); 25 | ~State() {} 26 | 27 | /// Returns timestep of the clone that we will marginalize 28 | double margtimestep() { 29 | double time = INFINITY; 30 | for (std::pair &clone_imu : _clones_IMU) { 31 | if(clone_imu.first < time) { 32 | time = clone_imu.first; 33 | } 34 | } 35 | return time; 36 | } 37 | 38 | /// Calculates the current max size of the covariance 39 | int max_covariance_size() { 40 | return (int)_Cov.rows(); 41 | } 42 | 43 | /// Current timestamp (should be the last update time) 44 | double _timestamp; 45 | 46 | /// Struct constaining filter options 47 | StateOptions _options; 48 | 49 | /// Pointer to active IMU State (q_GtoI, p_IinG, V_IinG, bg, ba) 50 | IMU *_imu; 51 | 52 | /// Map between scanning times and clone poses (q_GtoIi, p_IiinG) 53 | std::map _clones_IMU; 54 | 55 | /// Time offset between base IMU to lidar (t_imu = t_lidar + t_offset) 56 | Vec *_calib_dt_LIDARtoIMU; 57 | 58 | /// Calib pose for IMU Lidar system I_R_L, I_t_L 59 | Pose* _calib_LIDARtoIMU; 60 | 61 | /// Time offset between base IMU to camera (t_imu = t_camera + t_offset) 62 | Vec *_calib_dt_CAMERAtoIMU; 63 | 64 | /// Calib pose for IMU Lidar system I_R_C, I_t_C 65 | Pose* _calib_CAMERAtoIMU; 66 | 67 | private: 68 | /// Define that the state helper is a friend class of this class 69 | /// So that it can access the following private members 70 | friend class StateHelper; 71 | /// Covariance of all active variables 72 | Eigen::MatrixXd _Cov; 73 | /// Vector of variables 74 | std::vector _variables; 75 | }; 76 | }; 77 | #endif //CALIB_STATE_H 78 | -------------------------------------------------------------------------------- /src/state/StateHelper.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/8/20. 3 | // 4 | 5 | #ifndef CALIB_STATEHELPER_H 6 | #define CALIB_STATEHELPER_H 7 | 8 | #include "State.h" 9 | #include "utils/color.h" 10 | 11 | #include 12 | 13 | using namespace calib_core; 14 | 15 | namespace calib_estimator { 16 | /// This is a helper function which manipulates the State and its covariance 17 | /// This class has all functions that change the covariance along with addition and removing elements from the state. 18 | /// All functions are static. 19 | class StateHelper { 20 | public: 21 | /// Performs EKF propagation of the state covariance 22 | static void EKFPropagation(State *state, /// Pointer to the state 23 | const std::vector &order_NEW, /// Contiguous variables that have evolved according to this state transition 24 | const std::vector &order_OLD, /// Variable ordering used in the state transition 25 | const Eigen::MatrixXd &Phi, /// State transition matrix 26 | const Eigen::MatrixXd &Q); /// Additive state propagation noise matrix 27 | 28 | /// Performs EKF update of the state 29 | static void EKFUpdate(State *state, /// Pointer to the state 30 | const std::vector &H_order, /// Variable ordering used in compressed Jacobian 31 | const Eigen::MatrixXd &H, /// Condensed Jacobian of updating measurement 32 | const Eigen::VectorXd &res, /// residual of updating measurement 33 | const Eigen::MatrixXd &R); /// updating measurment covariance 34 | 35 | /// The following will calculate a smaller covariance for a given set of variables 36 | static Eigen::MatrixXd get_marginal_covariance(State *state, /// Pointer to state 37 | const std::vector &small_variables); /// Vector of variables whose marginal covariance is desired 38 | 39 | /// The following will calculate the full covariance matrix 40 | static Eigen::MatrixXd get_full_covariance(State *state); 41 | 42 | /// Marginalize 43 | static void marginalize(State *state, /// Pointer to state 44 | Type *marginalize); /// Pointer to variable to marginalize 45 | 46 | /// Clones "variables to clone" and places it at the end of covariance 47 | static Type* clone (State *state, /// Pointer to state 48 | Type *variable_to_clone); /// Pointer to variable that will be cloned 49 | 50 | /// Initializes a new variable into covariance 51 | static bool initialize(State *state, /// Pointer to state 52 | Type *new_variable, /// Pointer to variable to be initialized 53 | const std::vector &H_order, /// Vector of pointers in order they are contained in the condensed state Jacobian 54 | Eigen::MatrixXd &H_R, /// Jacobian of initializing measurements wrt variables in H_order 55 | Eigen::MatrixXd &H_L, /// Jacobian of initializing measurments wrt new variable 56 | Eigen::MatrixXd &R, /// Covariance of initializing measurements 57 | Eigen::VectorXd &res, /// Residual of initializing measurements 58 | double chi_2_mult); /// Value we should multiply the chi2 threshold by (larger means it will be accepting more measurements) 59 | 60 | /// Initializes a new variable into covariance (H_L must be invertible) 61 | static void initialize_invertible(State *state, 62 | Type *new_variable, 63 | const std::vector &H_order, 64 | const Eigen::MatrixXd &H_R, 65 | const Eigen::MatrixXd &H_L, 66 | const Eigen::MatrixXd &R, 67 | const Eigen::VectorXd &res); 68 | 69 | /// Augment the state with a stochastic copy of the current IMU pose 70 | static void augment_clone(State *state, /// Pointer to state 71 | Eigen::Matrix last_w); /// The estimated angular velocity at the cloning time 72 | /// can be used to estimate the imu-lidar time offset 73 | /// Remove the oldest clone, if we have more than the max clone count 74 | /// This will marginalize the clone from our covariance and remove it from our state. 75 | static void marginalize_old_clone(State *state) { 76 | if ((int) state->_clones_IMU.size() > state->_options.max_clone_size) { 77 | double marginal_time = state->margtimestep(); 78 | assert(marginal_time != INFINITY); 79 | StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time)); 80 | /// Note that the marginalizer should have already deleted the clone 81 | /// Thus we just need to remove the pointer to it from our state 82 | state->_clones_IMU.erase(marginal_time); 83 | } 84 | } 85 | 86 | private: 87 | /// All function in this class should be static 88 | /// Thus an instance of this class cannot be created 89 | StateHelper() {} 90 | }; 91 | } 92 | #endif //CALIB_STATEHELPER_H 93 | -------------------------------------------------------------------------------- /src/state/StateOptions.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/6/20. 3 | // 4 | 5 | #ifndef CALIB_STATEOPTIONS_H 6 | #define CALIB_STATEOPTIONS_H 7 | 8 | #include 9 | 10 | using namespace calib_type; 11 | 12 | namespace calib_estimator { 13 | /// Struct which stores all filter options 14 | struct StateOptions { 15 | /// Bool to determine whether or not to do First Estimate Jacobians 16 | bool do_fej = true; 17 | 18 | /// Bool to determine whether or not to use imu message averaging 19 | bool imu_avg = false; 20 | 21 | /// Bool to determine whether we want to use Runge-Kutta 4 IMU integration 22 | bool use_rk4_integration = true; 23 | 24 | /// Bool to determine whether or not to calibrate Lidar to IMU time offset 25 | bool do_calib_lidar_imu_timeoffset = false; 26 | 27 | /// Bool to determine whether or not to calibrate LIDAR to IMU (I_R_L (I_q_L), I_t_L) extrinsics 28 | bool do_calib_lidar_imu_extrinsic = false; 29 | 30 | /// Bool to determine whether or not to calibrate Lidar to IMU time offset 31 | bool do_calib_camera_imu_timeoffset = false; 32 | 33 | /// Bool to determine whether or not to calibrate LIDAR to IMU (I_R_L (I_q_L), I_t_L) extrinsics 34 | bool do_calib_camera_imu_extrinsic = false; 35 | 36 | /// Max clone size of sliding window 37 | int max_clone_size = 11; 38 | 39 | /// Initial Lidar-IMU Uncertainty 40 | double rot_x_noise_lidarimu = 0.2; 41 | double rot_y_noise_lidarimu = 0.2; 42 | double rot_z_noise_lidarimu = 0.2; 43 | double trans_x_noise_lidarimu = 0.3; 44 | double trans_y_noise_lidarimu = 0.3; 45 | double trans_z_noise_lidarimu = 0.3; 46 | double time_offset_noise_lidarimu = 0.01; 47 | 48 | /// Initial Camera-IMU Uncertainty 49 | double rot_x_noise_cameraimu = 0.2; 50 | double rot_y_noise_cameraimu = 0.2; 51 | double rot_z_noise_cameraimu = 0.2; 52 | double trans_x_noise_cameraimu = 0.3; 53 | double trans_y_noise_cameraimu = 0.3; 54 | double trans_z_noise_cameraimu = 0.3; 55 | double time_offset_noise_cameraimu = 0.01; 56 | 57 | /// Print function of what parameters we have loaded 58 | void print() { 59 | printf("\t- use_fej: %d\n", do_fej); 60 | printf("\t- use_imuavg: %d\n", imu_avg); 61 | printf("\t- use_rk4int: %d\n", use_rk4_integration); 62 | printf("\t- do_calib_lidar_imu_timeoffset?: %d\n", do_calib_lidar_imu_timeoffset); 63 | printf("\t- do_calib_lidar_imu_extrinsic?: %d\n", do_calib_lidar_imu_extrinsic); 64 | printf("\t- do_calib_camera_imu_timeoffset?: %d\n", do_calib_camera_imu_timeoffset); 65 | printf("\t- do_calib_camera_imu_extrinsic?: %d\n", do_calib_camera_imu_extrinsic); 66 | printf("\t- max_clone_size: %d\n", max_clone_size); 67 | 68 | printf("\t- Lidar-IMU \n"); 69 | printf("\t- rot_x_noise_lidarimu: %f\n", rot_x_noise_lidarimu); 70 | printf("\t- rot_y_noise_lidarimu: %f\n", rot_y_noise_lidarimu); 71 | printf("\t- rot_z_noise_lidarimu: %f\n", rot_z_noise_lidarimu); 72 | printf("\t- trans_x_noise_lidarimu: %f\n", trans_x_noise_lidarimu); 73 | printf("\t- trans_y_noise_lidarimu: %f\n", trans_y_noise_lidarimu); 74 | printf("\t- trans_z_noise_lidarimu: %f\n", trans_z_noise_lidarimu); 75 | printf("\t- time_offset_noise_lidarimu: %f\n", time_offset_noise_lidarimu); 76 | 77 | printf("\t- Camera-IMU \n"); 78 | printf("\t- rot_x_noise_cameraimu: %f\n", rot_x_noise_cameraimu); 79 | printf("\t- rot_y_noise_cameraimu: %f\n", rot_y_noise_cameraimu); 80 | printf("\t- rot_z_noise_cameraimu: %f\n", rot_z_noise_cameraimu); 81 | printf("\t- trans_x_noise_cameraimu: %f\n", trans_x_noise_cameraimu); 82 | printf("\t- trans_y_noise_cameraimu: %f\n", trans_y_noise_cameraimu); 83 | printf("\t- trans_z_noise_cameraimu: %f\n", trans_z_noise_cameraimu); 84 | printf("\t- time_offset_noise_cameraimu: %f\n", time_offset_noise_cameraimu); 85 | } 86 | }; 87 | }; 88 | #endif //CALIB_STATEOPTIONS_H 89 | -------------------------------------------------------------------------------- /src/track/cameraPoseTracking.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 3/24/22. 3 | // 4 | 5 | #include "cameraPoseTracking.h" 6 | 7 | namespace calib_core { 8 | cameraPoseTracking::cameraPoseTracking(double dx_, double dy_, 9 | int checkerboard_rows_, 10 | int checkerboard_cols_, 11 | std::string cam_config_file_path_) { 12 | cam_config_file_path = cam_config_file_path_; 13 | dx = dx_; 14 | dy = dy_; 15 | checkerboard_rows = checkerboard_rows_; 16 | checkerboard_cols = checkerboard_cols_; 17 | 18 | projection_matrix = cv::Mat::zeros(3, 3, CV_64F); 19 | distCoeff = cv::Mat::zeros(5, 1, CV_64F); 20 | 21 | readCameraParams(cam_config_file_path, image_height, image_width, distCoeff, projection_matrix); 22 | 23 | for(int i = 0; i < checkerboard_rows; i++) 24 | for (int j = 0; j < checkerboard_cols; j++) 25 | object_points.emplace_back(cv::Point3f(i*dx, j*dy, 0.0)); 26 | 27 | first_frame = true; 28 | 29 | C0_T_Ck_1 = Eigen::Matrix4d::Identity(); 30 | } 31 | 32 | void cameraPoseTracking::readCameraParams(std::string cam_config_file_path, 33 | int &image_height, 34 | int &image_width, 35 | cv::Mat &D, 36 | cv::Mat &K) { 37 | cv::FileStorage fs_cam_config(cam_config_file_path, cv::FileStorage::READ); 38 | if(!fs_cam_config.isOpened()) 39 | std::cerr << "Error: Wrong path: " << cam_config_file_path << std::endl; 40 | fs_cam_config["image_height"] >> image_height; 41 | fs_cam_config["image_width"] >> image_width; 42 | std::cout << "image_height: " << image_height << std::endl; 43 | std::cout << "image_width: " << image_width << std::endl; 44 | fs_cam_config["k1"] >> D.at(0); 45 | fs_cam_config["k2"] >> D.at(1); 46 | fs_cam_config["p1"] >> D.at(2); 47 | fs_cam_config["p2"] >> D.at(3); 48 | fs_cam_config["k3"] >> D.at(4); 49 | fs_cam_config["fx"] >> K.at(0, 0); 50 | fs_cam_config["fy"] >> K.at(1, 1); 51 | fs_cam_config["cx"] >> K.at(0, 2); 52 | fs_cam_config["cy"] >> K.at(1, 2); 53 | C_T_B_eig = Eigen::Matrix4d::Identity(); 54 | B_T_C_eig = Eigen::Matrix4d::Identity(); 55 | } 56 | 57 | bool cameraPoseTracking::feedImage(double timestamp, cv::Mat input_image, 58 | std::vector &objectpoints_C0, 59 | std::vector &imagepoints) { 60 | current_timestamp = timestamp; 61 | image_in_for_poseestimation = input_image.clone(); 62 | image_in_for_visualization = input_image; 63 | cv::cvtColor(image_in_for_poseestimation, image_in_for_poseestimation, cv::COLOR_BGR2GRAY); 64 | image_points.clear(); 65 | imagepoints.clear(); 66 | bool boardDetectedInCam = cv::findChessboardCorners(image_in_for_poseestimation, 67 | cv::Size(checkerboard_cols, checkerboard_rows), 68 | image_points); 69 | image_points_undistorted.clear(); 70 | if(boardDetectedInCam){ 71 | cv::undistortPoints(image_points, image_points_undistorted, projection_matrix, distCoeff); 72 | imagepoints = image_points_undistorted; 73 | cv::drawChessboardCorners(image_in_for_visualization, cv::Size(checkerboard_cols, checkerboard_rows), 74 | image_points, boardDetectedInCam); 75 | if(boardDetectedInCam) { 76 | assert(image_points.size() == object_points.size()); 77 | estimateCameraPose(); 78 | objectpoints_C0.clear(); 79 | objectpoints_C0 = object_points_C0; 80 | } 81 | } 82 | return boardDetectedInCam; 83 | } 84 | 85 | void cameraPoseTracking::solvePnPProblem() { 86 | cv::solvePnP(object_points, image_points, projection_matrix, distCoeff, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE); 87 | } 88 | 89 | double cameraPoseTracking::visualizeImageProjections(cv::Mat rvec, cv::Mat tvec) { 90 | std::vector projected_points; 91 | cv::projectPoints(object_points_C0, rvec, tvec, projection_matrix, distCoeff, projected_points, cv::noArray()); 92 | 93 | double error = 0; 94 | for(int i = 0; i < projected_points.size(); i++){ 95 | // cv::circle(image_in_for_visualization, projected_points[i], 3, cv::Scalar(185, 185, 45), -1, cv::LINE_AA, 0); 96 | error += (projected_points[i].x - image_points[i].x)*(projected_points[i].x - image_points[i].x); 97 | error += (projected_points[i].y - image_points[i].y)*(projected_points[i].y - image_points[i].y); 98 | cv::arrowedLine(image_in_for_visualization, image_points[i], projected_points[i], cv::Scalar(185, 185, 45), 99 | 2, cv::LINE_4); 100 | } 101 | 102 | error = sqrt(error/projected_points.size()); 103 | return error; 104 | } 105 | 106 | Eigen::Vector4d cameraPoseTracking::getPlaneEquation() { 107 | return nd_c; 108 | } 109 | 110 | void cameraPoseTracking::estimateCameraPose() { 111 | solvePnPProblem(); 112 | 113 | cv::Rodrigues(rvec, C_R_B_cv); 114 | cv::cv2eigen(C_R_B_cv, C_R_B_eig); 115 | C_p_B_eig = Eigen::Vector3d(tvec.at(0), tvec.at(1), tvec.at(2)); 116 | n_c = C_R_B_eig.block<3,1>(0, 2); 117 | d_c = -n_c.dot(C_p_B_eig); 118 | nd_c = Eigen::Vector4d(n_c.x(), n_c.y(), n_c.z(), d_c); 119 | C_T_B_eig.block(0, 0, 3, 3) = C_R_B_eig; 120 | C_T_B_eig.block(0, 3, 3, 1) = C_p_B_eig; 121 | 122 | B_T_C_eig = C_T_B_eig.inverse(); 123 | 124 | if (first_frame) { 125 | B_T_C_eig_first = B_T_C_eig; 126 | Eigen::Matrix3d W_R_C0 = B_T_C_eig_first.block(0, 0, 3, 3); 127 | Eigen::Vector3d W_t_C0 = B_T_C_eig_first.block(0, 3, 3, 1); 128 | Eigen::Matrix3d C0_R_W = W_R_C0.transpose(); 129 | Eigen::Vector3d C0_t_W = -C0_R_W*W_t_C0; 130 | object_points_C0.clear(); 131 | for (int i = 0; i < object_points.size(); ++i) { 132 | Eigen::Vector3d X_B = Eigen::Vector3d(object_points[i].x, object_points[i].y, object_points[i].z); 133 | Eigen::Vector3d X_C0 = C0_R_W*X_B + C0_t_W; 134 | cv::Point3d X_C0_cv = cv::Point3d(X_C0.x(), X_C0.y(), X_C0.z()); 135 | object_points_C0.push_back(X_C0_cv); 136 | } 137 | } 138 | 139 | C0_T_Ck = B_T_C_eig_first.inverse() * B_T_C_eig; 140 | Eigen::Matrix4d deltaPose = C0_T_Ck_1.inverse()*C0_T_Ck; 141 | 142 | if(!first_frame) { 143 | latestRP.timestamp_i = previous_timestamp; 144 | latestRP.timestamp_j = current_timestamp; 145 | latestRP.odometry_ij = deltaPose; 146 | } 147 | 148 | currentpose.timestamp = current_timestamp; 149 | currentpose.pose = C0_T_Ck; 150 | 151 | C0_T_Ck_1 = C0_T_Ck; 152 | previous_timestamp = current_timestamp; 153 | first_frame = false; 154 | } 155 | 156 | double cameraPoseTracking::checkReprojections(Eigen::Matrix4d I_T_C, Eigen::Matrix4d I0_T_Ik) { 157 | Eigen::Matrix4d b_T_c = I_T_C.inverse() * I0_T_Ik * I_T_C; 158 | Eigen::Matrix4d c_T_b = b_T_c.inverse(); 159 | Eigen::Matrix3d c_R_b = c_T_b.block(0, 0, 3, 3); 160 | cv::Mat c_R_b_cv, rvec; 161 | Eigen::Vector3d c_t_b = c_T_b.block(0, 3, 3, 1); 162 | cv::Mat c_t_b_cv; 163 | cv::eigen2cv(c_R_b, c_R_b_cv); 164 | cv::eigen2cv(c_t_b, c_t_b_cv); 165 | cv::Rodrigues(c_R_b_cv, rvec); 166 | 167 | double reperr = visualizeImageProjections(rvec, c_t_b_cv); 168 | cumulative_rep_err += reperr; 169 | cv::putText(image_in_for_visualization, //target image 170 | "Current Rep Error: " + std::to_string(reperr), //text 171 | cv::Point(image_in_for_visualization.cols / 2-400, 80), 172 | cv::FONT_HERSHEY_DUPLEX, 173 | 2.0, 174 | CV_RGB(118, 185, 0), //font color 175 | 4); 176 | cv::putText(image_in_for_visualization, //target image 177 | "Cumulative Rep Error: " + std::to_string(cumulative_rep_err), //text 178 | cv::Point(image_in_for_visualization.cols / 2-400, 160), 179 | cv::FONT_HERSHEY_DUPLEX, 180 | 2.0, 181 | CV_RGB(118, 185, 0), //font color 182 | 4); 183 | return reperr; 184 | } 185 | 186 | relativePose cameraPoseTracking::getRelativePose() { 187 | return latestRP; 188 | } 189 | 190 | cameraPoseTracking::Odom cameraPoseTracking::getCameraPose() { 191 | return currentpose; 192 | } 193 | } -------------------------------------------------------------------------------- /src/track/cameraPoseTracking.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 3/24/22. 3 | // 4 | 5 | #ifndef CAMERA_IMU_LIDAR_CALIBRATION_CAMERAPOSETRACKING_H 6 | #define CAMERA_IMU_LIDAR_CALIBRATION_CAMERAPOSETRACKING_H 7 | 8 | #include 9 | #include 10 | 11 | #include "utils/quat_ops.h" 12 | #include "utils/math_utils.h" 13 | #include "utils/eigen_utils.h" 14 | 15 | #include "types/Pose.h" 16 | #include "relpose/relativePose.h" 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace calib_core { 25 | class cameraPoseTracking { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | typedef std::shared_ptr Ptr; 29 | struct Odom { 30 | double timestamp; 31 | Eigen::Matrix4d pose; 32 | }; 33 | explicit cameraPoseTracking(double dx_ = 0.05, double dy_ = 0.05, 34 | int checkerboard_rows_ = 6, 35 | int checkerboard_cols_ = 7, 36 | std::string cam_config_file_path_ = ""); 37 | void readCameraParams(std::string cam_config_file_path, 38 | int &image_height, int &image_width, 39 | cv::Mat &D, cv::Mat &K); 40 | bool feedImage(double timestamp, cv::Mat input_image, 41 | std::vector &objectpoints_C0, std::vector &imagepoints); 42 | Eigen::Vector4d getPlaneEquation(); 43 | void solvePnPProblem(); 44 | double visualizeImageProjections(cv::Mat rvec, cv::Mat tvec); 45 | void estimateCameraPose(); 46 | Odom getCameraPose(); 47 | relativePose getRelativePose(); 48 | double checkReprojections(Eigen::Matrix4d I_T_C, Eigen::Matrix4d I0_T_Ik); 49 | 50 | private: 51 | cv::Mat image_in_for_poseestimation; 52 | cv::Mat image_in_for_visualization; 53 | 54 | cv::Mat projection_matrix; 55 | cv::Mat distCoeff; 56 | int image_height, image_width; 57 | 58 | std::vector image_points; 59 | std::vector image_points_undistorted; 60 | std::vector object_points; 61 | std::vector object_points_C0; 62 | 63 | double dx, dy; 64 | int checkerboard_rows, checkerboard_cols; 65 | 66 | cv::Mat tvec, rvec; 67 | cv::Mat C_R_B_cv; 68 | Eigen::Vector3d n_c; 69 | double d_c; 70 | Eigen::Vector4d nd_c; // [n_c, d_c] 71 | 72 | Eigen::Matrix3d C_R_B_eig; 73 | Eigen::Vector3d C_p_B_eig; 74 | 75 | Eigen::Matrix4d C_T_B_eig; 76 | Eigen::Matrix4d B_T_C_eig; 77 | Eigen::Matrix4d B_T_C_eig_first; 78 | 79 | std::string cam_config_file_path; 80 | 81 | bool first_frame; 82 | double current_timestamp; 83 | double previous_timestamp; 84 | 85 | Odom currentpose; 86 | 87 | Eigen::Matrix4d C0_T_Ck; 88 | Eigen::Matrix4d C0_T_Ck_1; 89 | 90 | relativePose latestRP; // latest relative pose 91 | 92 | double cumulative_rep_err = 0; 93 | }; 94 | } 95 | 96 | #endif //CAMERA_IMU_LIDAR_CALIBRATION_CAMERAPOSETRACKING_H 97 | -------------------------------------------------------------------------------- /src/track/lidarOdometry.cpp: -------------------------------------------------------------------------------- 1 | #include "lidarOdometry.h" 2 | 3 | namespace calib_core { 4 | 5 | LidarOdometry::LidarOdometry(double ndt_resolution, std::string lo_trajectory_filename, bool downsample_for_mapping) 6 | : map_cloud_(new VPointCloud()), scan_in_target_global_(new VPointCloud()) { 7 | ndt_omp_ = ndtInit(ndt_resolution); 8 | latestRP.odometry_ij = Eigen::Matrix4d::Identity(); 9 | trajfile_csv.open(lo_trajectory_filename); 10 | downsampleForMapping = downsample_for_mapping; 11 | } 12 | 13 | pclomp::NormalDistributionsTransform::Ptr 14 | LidarOdometry::ndtInit(double ndt_resolution) { 15 | auto ndt_omp = pclomp::NormalDistributionsTransform::Ptr 16 | (new pclomp::NormalDistributionsTransform()); 17 | ndt_omp->setResolution(ndt_resolution); 18 | ndt_omp->setNumThreads(16); 19 | ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7); 20 | ndt_omp->setTransformationEpsilon(1e-3); 21 | ndt_omp->setStepSize(0.01); 22 | ndt_omp->setMaximumIterations(50); 23 | return ndt_omp; 24 | } 25 | 26 | void LidarOdometry::feedScan(double timestamp, 27 | VPointCloud::Ptr cur_scan, 28 | Eigen::Matrix4d pose_predict) { 29 | odom_curr.timestamp = timestamp; 30 | odom_curr.pose = Eigen::Matrix4d::Identity(); 31 | pcl::PointCloud::Ptr scan_in_target(new pcl::PointCloud()); 32 | current_scan = *cur_scan; 33 | if(map_cloud_->empty()) { 34 | scan_in_target = cur_scan; 35 | } else { 36 | Eigen::Matrix4d T_LM_predict = odom_.back().pose*pose_predict; 37 | registration(cur_scan, T_LM_predict, odom_curr.pose, scan_in_target); 38 | } 39 | 40 | if(first_scan) { 41 | odom_.push_back(odom_curr); 42 | updateKeyScan(current_scan, odom_curr); 43 | first_scan = false; 44 | } else { 45 | size_t lastIdx = odom_.size() - 1; 46 | Odom odom_i = odom_[lastIdx]; 47 | Odom odom_j = odom_curr; 48 | latestRP.timestamp_i = odom_i.timestamp; 49 | Eigen::Matrix4d w_T_i = odom_i.pose; 50 | latestRP.timestamp_j = odom_j.timestamp; 51 | Eigen::Matrix4d w_T_j = odom_j.pose; 52 | Eigen::Matrix4d i_T_j = w_T_i.inverse()*w_T_j; 53 | latestRP.odometry_ij = i_T_j; 54 | } 55 | } 56 | 57 | void LidarOdometry::append_and_update(bool update_map) { 58 | Eigen::Matrix3d R_curr = odom_curr.pose.block(0, 0, 3, 3); 59 | Eigen::Quaterniond quat_curr(R_curr); 60 | trajfile_csv << odom_curr.timestamp << "," << quat_curr.x() << "," << quat_curr.y() << "," << quat_curr.z() << ","<< quat_curr.w() << "," 61 | << odom_curr.pose(0, 3) << "," << odom_curr.pose(1, 3) << ","<< odom_curr.pose(2, 3) << std::endl; 62 | 63 | odom_.push_back(odom_curr); 64 | 65 | if(update_map) { 66 | updateKeyScan(current_scan, odom_curr); 67 | } 68 | 69 | } 70 | 71 | void LidarOdometry::registration(const VPointCloud::Ptr& cur_scan, 72 | const Eigen::Matrix4d& pose_predict, 73 | Eigen::Matrix4d& pose_out, 74 | VPointCloud::Ptr scan_in_target) { 75 | pcl::PointCloud::Ptr p_filtered_cloud(new pcl::PointCloud); 76 | downsampleCloud(cur_scan, p_filtered_cloud, 0.1); 77 | 78 | ndt_omp_->setInputSource(p_filtered_cloud); 79 | ndt_omp_->align(*scan_in_target, pose_predict.cast()); 80 | pose_out = ndt_omp_->getFinalTransformation().cast(); 81 | } 82 | 83 | void LidarOdometry::updateKeyScan(const VPointCloud cur_scan, 84 | const Odom& odom) { 85 | if(checkKeyScan(odom)) { 86 | VPointCloud ::Ptr filtered_cloud(new VPointCloud()); 87 | VPointCloud::Ptr input_scan(new VPointCloud); 88 | *input_scan = cur_scan; 89 | if(downsampleForMapping) { 90 | downsampleCloud(input_scan, filtered_cloud, 0.1); 91 | } else { 92 | *filtered_cloud = *input_scan; 93 | } 94 | VPointCloud::Ptr scan_in_target(new VPointCloud ()); 95 | pcl::transformPointCloud(*filtered_cloud, *scan_in_target, odom.pose); 96 | scan_in_target_global_->clear(); 97 | *scan_in_target_global_ = *scan_in_target; 98 | *map_cloud_ += *scan_in_target; 99 | ndt_omp_->setInputTarget(map_cloud_); 100 | key_frame_index_.push_back(odom_.size()); 101 | } 102 | } 103 | 104 | bool LidarOdometry::checkKeyScan(const Odom &odom) { 105 | static Eigen::Vector3d position_last(0,0,0); 106 | static Eigen::Vector3d ypr_last(0, 0, 0); 107 | 108 | Eigen::Vector3d position_now = odom.pose.block<3, 1>(0, 3); 109 | double dist = (position_now - position_last).norm(); 110 | 111 | const Eigen::Matrix3d rotation(odom.pose.block<3, 3>(0, 0)); 112 | Eigen::Vector3d ypr = mathutils::R2ypr(rotation); 113 | Eigen::Vector3d delta_angle = ypr - ypr_last; 114 | for (size_t i = 0; i < 3; i++) 115 | delta_angle(i) = normalize_angle(delta_angle(i)); 116 | delta_angle = delta_angle.cwiseAbs(); 117 | if (key_frame_index_.size() == 0 || 118 | dist > 0.2 || 119 | delta_angle(0) > 5.0 || 120 | delta_angle(1) > 5.0 || 121 | delta_angle(2) > 5.0) { 122 | position_last = position_now; 123 | ypr_last = ypr; 124 | is_KF_ = true; 125 | kf_odom_.push_back(odom); 126 | // ROS_WARN_STREAM("[LidarOdometry::checkKeyScan] This is a Key Frame, returning true"); 127 | return true; 128 | } 129 | // ROS_INFO_STREAM("[LidarOdometry::checkKeyScan] Not a Key Frame, returning false"); 130 | is_KF_ = false; 131 | return false; 132 | } 133 | 134 | void LidarOdometry::setTargetMap(VPointCloud::Ptr map_cloud_in) { 135 | map_cloud_->clear(); 136 | pcl::copyPointCloud(*map_cloud_in, *map_cloud_); 137 | ndt_omp_->setInputTarget(map_cloud_); 138 | } 139 | 140 | void LidarOdometry::clearOdometryData() { 141 | key_frame_index_.clear(); 142 | odom_.clear(); 143 | kf_odom_.clear(); 144 | } 145 | 146 | } 147 | -------------------------------------------------------------------------------- /src/track/lidarOdometry.h: -------------------------------------------------------------------------------- 1 | #ifndef CALIB_LIDARODOMETRY_H 2 | #define CALIB_LIDARODOMETRY_H 3 | 4 | #include "utils/math_utils.h" 5 | #include "utils/eigen_utils.h" 6 | #include "utils/quat_ops.h" 7 | #include "utils/pcl_utils.h" 8 | 9 | #include 10 | 11 | #include "types/Pose.h" 12 | #include "relpose/relativePose.h" 13 | 14 | #include 15 | 16 | namespace calib_core { 17 | class LidarOdometry { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | typedef std::shared_ptr Ptr; 21 | 22 | struct Odom { 23 | double timestamp; 24 | Eigen::Matrix4d pose; // current scan to first scan 25 | }; 26 | 27 | explicit LidarOdometry(double ndtResolution = 0.5, 28 | std::string lo_trajectory_filename="", 29 | bool downsample_for_mapping = false); 30 | 31 | static pclomp::NormalDistributionsTransform::Ptr ndtInit( 32 | double ndt_resolution); 33 | 34 | void feedScan(double timestamp, 35 | VPointCloud::Ptr cur_scan, 36 | Eigen::Matrix4d pose_predict = Eigen::Matrix4d::Identity()); 37 | 38 | void detectPlanarTargetInFirstFrame(double timestamp); 39 | 40 | void clearOdometryData(); 41 | 42 | void setTargetMap(VPointCloud::Ptr map_cloud_in); 43 | 44 | void saveTargetMap(const std::string& path) const { 45 | ROS_INFO_STREAM("Save NDT target map to " << path 46 | << "; size: " << map_cloud_->size()); 47 | pcl::io::savePCDFileASCII(path, *map_cloud_); 48 | } 49 | 50 | const VPointCloud::Ptr getTargetMap() { 51 | return map_cloud_; 52 | } 53 | 54 | const VPointCloud::Ptr getScanInTarget() { 55 | return scan_in_target_global_; 56 | } 57 | 58 | const pclomp::NormalDistributionsTransform::Ptr& getNDTPtr() const { 59 | return ndt_omp_; 60 | } 61 | 62 | const Eigen::aligned_vector &get_odom_data() const { 63 | return odom_; 64 | } 65 | 66 | const Odom &get_current_odometry() { 67 | return odom_curr; 68 | } 69 | 70 | const relativePose get_latest_relativePose() { 71 | return latestRP; 72 | } 73 | 74 | const Eigen::aligned_vector &get_kf_odom_data() const { 75 | return kf_odom_; 76 | } 77 | 78 | const bool &isKeyFrame() const { 79 | return is_KF_; 80 | } 81 | 82 | void append_and_update(bool update_map = true); 83 | 84 | private: 85 | void registration(const VPointCloud::Ptr& cur_scan, 86 | const Eigen::Matrix4d& pose_predict, 87 | Eigen::Matrix4d& pose_out, 88 | VPointCloud::Ptr scan_in_target); 89 | void updateKeyScan(const VPointCloud cur_scan, const Odom& odom); 90 | bool checkKeyScan(const Odom& odom); 91 | 92 | // Normalize angle to be between [-180, 180] 93 | static inline double normalize_angle(double ang_degree) { 94 | if(ang_degree > 180) 95 | ang_degree -= 360; 96 | if(ang_degree < -180) 97 | ang_degree += 360; 98 | return ang_degree; 99 | } 100 | private: 101 | VPointCloud::Ptr map_cloud_; 102 | VPointCloud::Ptr scan_in_target_global_; 103 | pclomp::NormalDistributionsTransform::Ptr ndt_omp_; 104 | 105 | std::vector key_frame_index_; 106 | Eigen::aligned_vector kf_odom_; 107 | Eigen::aligned_vector odom_; 108 | Odom odom_curr; 109 | relativePose latestRP; // latest relative pose 110 | bool is_KF_; 111 | 112 | std::ofstream trajfile_csv; 113 | VPointCloud current_scan; 114 | 115 | bool first_scan = true; 116 | bool downsampleForMapping; 117 | }; 118 | } 119 | 120 | #endif //CALIB_LIDARODOMETRY_H 121 | 122 | -------------------------------------------------------------------------------- /src/track/lidarPlaneDetector.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 4/3/22. 3 | // 4 | 5 | #include "lidarPlaneDetector.h" 6 | 7 | namespace calib_core { 8 | TPointCloud::Ptr lidarPlaneDetector::detectCalibrationTarget(TPointCloud::Ptr scan_raw) { 9 | TPointCloud::Ptr cloud_filtered_x(new TPointCloud); 10 | TPointCloud::Ptr cloud_filtered_xy(new TPointCloud); 11 | TPointCloud::Ptr cloud_filtered_xyz(new TPointCloud); 12 | TPointCloud::Ptr plane(new TPointCloud); 13 | TPointCloud::Ptr plane_filtered(new TPointCloud); 14 | 15 | /// Pass through filters 16 | pcl::PassThrough pass_x; 17 | pass_x.setInputCloud(scan_raw); 18 | pass_x.setFilterFieldName("x"); 19 | pass_x.setFilterLimits(0.5, 2); 20 | pass_x.filter(*cloud_filtered_x); 21 | 22 | pcl::PassThrough pass_y; 23 | pass_y.setInputCloud(cloud_filtered_x); 24 | pass_y.setFilterFieldName("y"); 25 | pass_y.setFilterLimits(-1, 1); 26 | pass_y.filter(*cloud_filtered_xy); 27 | 28 | pcl::PassThrough pass_z; 29 | pass_z.setInputCloud(cloud_filtered_xy); 30 | pass_z.setFilterFieldName("z"); 31 | pass_z.setFilterLimits(-0.5, 0.5); 32 | pass_z.filter(*cloud_filtered_xyz); 33 | 34 | /// Plane Segmentation 35 | pcl::SampleConsensusModelPlane::Ptr model_p(new pcl::SampleConsensusModelPlane(cloud_filtered_xyz)); 36 | pcl::RandomSampleConsensus ransac(model_p); 37 | ransac.setDistanceThreshold(0.01); 38 | ransac.computeModel(); 39 | std::vector inliers_indicies; 40 | ransac.getInliers(inliers_indicies); 41 | pcl::copyPointCloud(*cloud_filtered_xyz, inliers_indicies, *plane); 42 | double a = ransac.model_coefficients_(0); 43 | double b = ransac.model_coefficients_(1); 44 | double c = ransac.model_coefficients_(2); 45 | double sqrt_abc = sqrt(a*a + b*b + c*c); 46 | double d = ransac.model_coefficients_(3); 47 | 48 | nd_l = Eigen::Vector4d(a/sqrt_abc, b/sqrt_abc,c/sqrt_abc, d/sqrt_abc); 49 | 50 | /// Statistical Outlier Removal 51 | pcl::StatisticalOutlierRemoval sor; 52 | sor.setInputCloud(plane); 53 | sor.setMeanK (50); 54 | sor.setStddevMulThresh (1); 55 | sor.filter (*plane_filtered); 56 | 57 | return plane_filtered; 58 | } 59 | 60 | Eigen::Vector4d lidarPlaneDetector::getPlaneEquation() { 61 | return nd_l; 62 | } 63 | } -------------------------------------------------------------------------------- /src/track/lidarPlaneDetector.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 4/3/22. 3 | // 4 | 5 | #ifndef CAMERA_IMU_LIDAR_CALIBRATION_LIDARPLANEDETECTOR_H 6 | #define CAMERA_IMU_LIDAR_CALIBRATION_LIDARPLANEDETECTOR_H 7 | 8 | #define PCL_NO_PRECOMPILE // !! BEFORE ANY PCL INCLUDE!! 9 | 10 | #include "utils/math_utils.h" 11 | #include "utils/eigen_utils.h" 12 | #include "utils/quat_ops.h" 13 | #include "utils/pcl_utils.h" 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace calib_core { 29 | class lidarPlaneDetector { 30 | public: 31 | lidarPlaneDetector() { 32 | std::cout <<"-- Plane Target Detector Initialized --" << std::endl; 33 | } 34 | TPointCloud::Ptr detectCalibrationTarget(TPointCloud::Ptr scan_raw); 35 | Eigen::Vector4d getPlaneEquation(); 36 | 37 | private: 38 | Eigen::Vector4d nd_l; 39 | }; 40 | } 41 | #endif //CAMERA_IMU_LIDAR_CALIBRATION_LIDARPLANEDETECTOR_H 42 | -------------------------------------------------------------------------------- /src/types/IMU.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/6/20. 3 | // 4 | 5 | #ifndef CALIB_IMU_H 6 | #define CALIB_IMU_H 7 | 8 | #include "Pose.h" 9 | #include "utils/quat_ops.h" 10 | 11 | namespace calib_type { 12 | 13 | 14 | /** 15 | * @brief Derived Type class that implements an IMU state 16 | * 17 | * Contains a Pose, Vec velocity, Vec gyro bias, and Vec accel bias. 18 | * This should be similar to that of the standard MSCKF state besides the ordering. 19 | * The pose is first, followed by velocity, etc. 20 | */ 21 | class IMU : public Type { 22 | 23 | public: 24 | 25 | IMU() : Type(15) { 26 | _pose = new Pose(); 27 | _v = new Vec(3); 28 | _bg = new Vec(3); 29 | _ba = new Vec(3); 30 | 31 | Eigen::Matrix imu0; 32 | imu0.setZero(); 33 | imu0(3) = 1.0; 34 | 35 | set_value(imu0); 36 | set_fe(imu0); 37 | } 38 | 39 | ~IMU() { 40 | delete _pose; 41 | delete _v; 42 | delete _bg; 43 | delete _ba; 44 | } 45 | 46 | /** 47 | * @brief Sets id used to track location of variable in the filter covariance 48 | * 49 | * Note that we update the sub-variables also. 50 | * 51 | * @param new_id entry in filter covariance corresponding to this variable 52 | */ 53 | void set_local_id(int new_id) override { 54 | _id = new_id; 55 | _pose->set_local_id(new_id); 56 | _v->set_local_id(_pose->id()+_pose->size()); 57 | _bg->set_local_id(_v->id()+_v->size()); 58 | _ba->set_local_id(_bg->id()+_bg->size()); 59 | } 60 | 61 | /** 62 | * @brief Performs update operation using JPLQuat update for orientation, then vector updates for 63 | * position, velocity, gyro bias, and accel bias (in that order). 64 | * 65 | * @param dx 15 DOF vector encoding update using the following order (q, p, v, bg, ba) 66 | */ 67 | void update(const Eigen::VectorXd dx) override { 68 | 69 | assert(dx.rows() == _size); 70 | 71 | Eigen::Matrix newX = _value; 72 | 73 | Eigen::Matrix dq; 74 | dq << .5 * dx.block(0, 0, 3, 1), 1.0; 75 | dq = calib_core::quatnorm(dq); 76 | 77 | newX.block(0, 0, 4, 1) = calib_core::quat_multiply(dq, quat()); 78 | newX.block(4, 0, 3, 1) += dx.block(3, 0, 3, 1); 79 | 80 | newX.block(7, 0, 3, 1) += dx.block(6, 0, 3, 1); 81 | newX.block(10, 0, 3, 1) += dx.block(9, 0, 3, 1); 82 | newX.block(13, 0, 3, 1) += dx.block(12, 0, 3, 1); 83 | 84 | set_value(newX); 85 | 86 | } 87 | 88 | 89 | /** 90 | * @brief Sets the value of the estimate 91 | * @param new_value New value we should set 92 | */ 93 | void set_value(const Eigen::MatrixXd new_value) override { 94 | 95 | assert(new_value.rows() == 16); 96 | assert(new_value.cols() == 1); 97 | 98 | _pose->set_value(new_value.block(0, 0, 7, 1)); 99 | _v->set_value(new_value.block(7, 0, 3, 1)); 100 | _bg->set_value(new_value.block(10, 0, 3, 1)); 101 | _ba->set_value(new_value.block(13, 0, 3, 1)); 102 | 103 | _value = new_value; 104 | } 105 | 106 | /** 107 | * @brief Sets the value of the first estimate 108 | * @param new_value New value we should set 109 | */ 110 | void set_fe(const Eigen::MatrixXd new_value) override { 111 | 112 | assert(new_value.rows() == 16); 113 | assert(new_value.cols() == 1); 114 | 115 | _pose->set_fe(new_value.block(0, 0, 7, 1)); 116 | _v->set_fe(new_value.block(7, 0, 3, 1)); 117 | _bg->set_fe(new_value.block(10, 0, 3, 1)); 118 | _ba->set_fe(new_value.block(13, 0, 3, 1)); 119 | 120 | _fe = new_value; 121 | } 122 | 123 | Type *clone() override { 124 | Type *Clone = new IMU(); 125 | Clone->set_value(value()); 126 | Clone->set_fe(fe()); 127 | return Clone; 128 | } 129 | 130 | /** 131 | * @brief Used to find the components inside the IMU if needed. 132 | * If the passed variable is a sub-variable or the current variable this will return it. 133 | * Otherwise it will return a nullptr, meaning that it was unable to be found. 134 | * 135 | * @param check variable to find 136 | */ 137 | Type *check_if_same_variable(const Type *check) override { 138 | if (check == this) { 139 | return this; 140 | } else if (check == _pose) { 141 | return _pose; 142 | } else if (check == q()) { 143 | return q(); 144 | } else if (check == p()) { 145 | return p(); 146 | } else if (check == _v) { 147 | return _v; 148 | } else if (check == _bg) { 149 | return bg(); 150 | } else if (check == _ba) { 151 | return ba(); 152 | } else { 153 | return nullptr; 154 | } 155 | } 156 | 157 | /// Rotation access 158 | Eigen::Matrix Rot() const { 159 | return _pose->Rot(); 160 | } 161 | 162 | /// FEJ Rotation access 163 | Eigen::Matrix Rot_fe() const { 164 | return _pose->Rot_fe(); 165 | } 166 | 167 | /// Rotation access quaternion 168 | Eigen::Matrix quat() const { 169 | return _pose->quat(); 170 | } 171 | 172 | /// FEJ Rotation access quaternion 173 | Eigen::Matrix quat_fe() const { 174 | return _pose->quat_fe(); 175 | } 176 | 177 | 178 | /// Position access 179 | Eigen::Matrix pos() const { 180 | return _pose->pos(); 181 | } 182 | 183 | /// FEJ position access 184 | Eigen::Matrix pos_fe() const { 185 | return _pose->pos_fe(); 186 | } 187 | 188 | /// Velocity access 189 | Eigen::Matrix vel() const { 190 | return _v->value(); 191 | } 192 | 193 | // FEJ velocity access 194 | Eigen::Matrix vel_fe() const { 195 | return _v->fe(); 196 | } 197 | 198 | /// Gyro bias access 199 | Eigen::Matrix bias_g() const { 200 | return _bg->value(); 201 | } 202 | 203 | /// FEJ gyro bias access 204 | Eigen::Matrix bias_g_fe() const { 205 | return _bg->fe(); 206 | } 207 | 208 | /// Accel bias access 209 | Eigen::Matrix bias_a() const { 210 | return _ba->value(); 211 | } 212 | 213 | // FEJ accel bias access 214 | Eigen::Matrix bias_a_fe() const { 215 | return _ba->fe(); 216 | } 217 | 218 | /// Pose type access 219 | Pose *pose() { 220 | return _pose; 221 | } 222 | 223 | /// Quaternion type access 224 | Quat *q() { 225 | return _pose->q(); 226 | } 227 | 228 | /// Position type access 229 | Vec *p() { 230 | return _pose->p(); 231 | } 232 | 233 | /// Velocity type access 234 | Vec *v() { 235 | return _v; 236 | } 237 | 238 | /// Gyroscope bias access 239 | Vec *bg() { 240 | return _bg; 241 | } 242 | 243 | /// Acceleration bias access 244 | Vec *ba() { 245 | return _ba; 246 | } 247 | 248 | protected: 249 | 250 | /// Pose subvariable 251 | Pose *_pose; 252 | 253 | /// Velocity subvariable 254 | Vec *_v; 255 | 256 | /// Gyroscope bias subvariable 257 | Vec *_bg; 258 | 259 | /// Acceleration bias subvariable 260 | Vec *_ba; 261 | }; 262 | 263 | }; 264 | #endif //CALIB_IMU_H 265 | -------------------------------------------------------------------------------- /src/types/Pose.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/5/20. 3 | // 4 | 5 | #ifndef CALIB_POSE_H 6 | #define CALIB_POSE_H 7 | 8 | #include "Vec.h" 9 | #include "Quat.h" 10 | #include "utils/quat_ops.h" 11 | 12 | namespace calib_type { 13 | 14 | 15 | /** 16 | * @brief Derived Type class that implements a 6 d.o.f pose 17 | * 18 | * Internally we use a JPLQuat quaternion representation for the orientation and 3D Vec position. 19 | * Please see JPLQuat for details on its update procedure and its left multiplicative error. 20 | */ 21 | class Pose : public Type { 22 | 23 | public: 24 | 25 | Pose() : Type(6) { 26 | 27 | //Initialize subvariables 28 | _q = new Quat(); 29 | _p = new Vec(3); 30 | 31 | Eigen::Matrix pose0; 32 | pose0.setZero(); 33 | pose0(3) = 1.0; 34 | set_value(pose0); 35 | set_fe(pose0); 36 | } 37 | 38 | ~Pose() { 39 | delete _q; 40 | delete _p; 41 | } 42 | 43 | /** 44 | * @brief Sets id used to track location of variable in the filter covariance 45 | * 46 | * Note that we update the sub-variables also. 47 | * 48 | * @param new_id entry in filter covariance corresponding to this variable 49 | */ 50 | void set_local_id(int new_id) override { 51 | _id = new_id; 52 | _q->set_local_id(new_id); 53 | _p->set_local_id(new_id+_q->size()); 54 | } 55 | 56 | /** 57 | * @brief Update q and p using a the JPLQuat update for orientation and vector update for position 58 | * 59 | * @param dx Correction vector (orientation then position) 60 | */ 61 | void update(const Eigen::VectorXd dx) override { 62 | 63 | assert(dx.rows() == _size); 64 | 65 | Eigen::Matrix newX = _value; 66 | 67 | Eigen::Matrix dq; 68 | dq << .5 * dx.block(0, 0, 3, 1), 1.0; 69 | dq = calib_core::quatnorm(dq); 70 | 71 | //Update orientation 72 | newX.block(0, 0, 4, 1) = calib_core::quat_multiply(dq, quat()); 73 | 74 | //Update position 75 | newX.block(4, 0, 3, 1) += dx.block(3, 0, 3, 1); 76 | 77 | set_value(newX); 78 | 79 | } 80 | 81 | /** 82 | * @brief Sets the value of the estimate 83 | * @param new_value New value we should set 84 | */ 85 | void set_value(const Eigen::MatrixXd new_value) override { 86 | 87 | assert(new_value.rows() == 7); 88 | assert(new_value.cols() == 1); 89 | 90 | //Set orientation value 91 | _q->set_value(new_value.block(0, 0, 4, 1)); 92 | 93 | //Set position value 94 | _p->set_value(new_value.block(4, 0, 3, 1)); 95 | 96 | _value = new_value; 97 | } 98 | 99 | /** 100 | * @brief Sets the value of the first estimate 101 | * @param new_value New value we should set 102 | */ 103 | void set_fe(const Eigen::MatrixXd new_value) override { 104 | 105 | assert(new_value.rows() == 7); 106 | assert(new_value.cols() == 1); 107 | 108 | //Set orientation fej value 109 | _q->set_fe(new_value.block(0, 0, 4, 1)); 110 | 111 | //Set position fej value 112 | _p->set_fe(new_value.block(4, 0, 3, 1)); 113 | 114 | _fe = new_value; 115 | } 116 | 117 | Type *clone() override { 118 | Type *Clone = new Pose(); 119 | Clone->set_value(value()); 120 | Clone->set_fe(fe()); 121 | return Clone; 122 | } 123 | 124 | /** 125 | * @brief Used to find the components inside the Pose if needed 126 | * 127 | * If the passed variable is a sub-variable or the current variable this will return it. 128 | * Otherwise it will return a nullptr, meaning that it was unable to be found. 129 | * 130 | * @param check variable to find 131 | */ 132 | Type *check_if_same_variable(const Type *check) override { 133 | if (check == this) { 134 | return this; 135 | } else if (check == _q) { 136 | return q(); 137 | } else if (check == _p) { 138 | return p(); 139 | } else { 140 | return nullptr; 141 | } 142 | } 143 | 144 | /// Rotation access 145 | Eigen::Matrix Rot() const { 146 | return _q->Rot(); 147 | } 148 | 149 | /// FEJ Rotation access 150 | Eigen::Matrix Rot_fe() const { 151 | return _q->Rot_fe();; 152 | } 153 | 154 | /// Rotation access as quaternion 155 | Eigen::Matrix quat() const { 156 | return _q->value(); 157 | } 158 | 159 | /// FEJ Rotation access as quaternion 160 | Eigen::Matrix quat_fe() const { 161 | return _q->fe(); 162 | } 163 | 164 | 165 | /// Position access 166 | Eigen::Matrix pos() const { 167 | return _p->value(); 168 | } 169 | 170 | // FEJ position access 171 | Eigen::Matrix pos_fe() const { 172 | return _p->fe(); 173 | } 174 | 175 | // Quaternion type access 176 | Quat *q() { 177 | return _q; 178 | } 179 | 180 | // Position type access 181 | Vec *p() { 182 | return _p; 183 | } 184 | 185 | protected: 186 | 187 | /// Subvariable containing orientation 188 | Quat *_q; 189 | 190 | /// Subvariable containg position 191 | Vec *_p; 192 | 193 | }; 194 | 195 | }; 196 | #endif //CALIB_POSE_H 197 | -------------------------------------------------------------------------------- /src/types/Quat.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/5/20. 3 | // 4 | 5 | #ifndef CALIB_QUAT_H 6 | #define CALIB_QUAT_H 7 | 8 | #include "Type.h" 9 | #include "utils/quat_ops.h" 10 | 11 | namespace calib_type { 12 | 13 | /** 14 | * @brief Derived Type class that implements JPL quaternion 15 | * 16 | * This quaternion uses a left-multiplicative error state and follows the "JPL convention". 17 | * Please checkout our utility functions in the quat_ops.h file. 18 | * We recommend that people new quaternions check out the following resources: 19 | * - http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf 20 | * - ftp://naif.jpl.nasa.gov/pub/naif/misc/Quaternion_White_Paper/Quaternions_White_Paper.pdf 21 | */ 22 | class Quat : public Type { 23 | 24 | public: 25 | 26 | Quat() : Type(3) { 27 | Eigen::Matrix q0; 28 | q0.setZero(); 29 | q0(3) = 1.0; 30 | set_value(q0); 31 | set_fe(q0); 32 | } 33 | 34 | ~Quat() {} 35 | 36 | /** 37 | * @brief Implements update operation by left-multiplying the current 38 | * quaternion with a quaternion built from a small axis-angle perturbation. 39 | * 40 | * @f[ 41 | * \bar{q}=norm\Big(\begin{bmatrix} 0.5*\mathbf{\theta_{dx}} \\ 1 \end{bmatrix}\Big) \hat{\bar{q}} 42 | * @f] 43 | * 44 | * @param dx Axis-angle representation of the perturbing quaternion 45 | */ 46 | void update(const Eigen::VectorXd dx) override { 47 | 48 | assert(dx.rows() == _size); 49 | 50 | //Build perturbing quaternion 51 | Eigen::Matrix dq; 52 | dq << .5 * dx, 1.0; 53 | dq = calib_core::quatnorm(dq); 54 | 55 | //Update estimate and recompute R 56 | set_value(calib_core::quat_multiply(dq, _value)); 57 | 58 | } 59 | 60 | /** 61 | * @brief Sets the value of the estimate and recomputes the internal rotation matrix 62 | * @param new_value New value for the quaternion estimate 63 | */ 64 | void set_value(const Eigen::MatrixXd new_value) override { 65 | 66 | assert(new_value.rows() == 4); 67 | assert(new_value.cols() == 1); 68 | 69 | _value = new_value; 70 | 71 | //compute associated rotation 72 | _R = calib_core::quat_2_Rot(new_value); 73 | } 74 | 75 | Type *clone() override { 76 | Type *Clone = new Quat(); 77 | Clone->set_value(value()); 78 | Clone->set_fe(fe()); 79 | return Clone; 80 | } 81 | 82 | /** 83 | * @brief Sets the fej value and recomputes the fej rotation matrix 84 | * @param new_value New value for the quaternion estimate 85 | */ 86 | void set_fe(const Eigen::MatrixXd new_value) override { 87 | 88 | assert(new_value.rows() == 4); 89 | assert(new_value.cols() == 1); 90 | 91 | _fe = new_value; 92 | 93 | //compute associated rotation 94 | _Rfe = calib_core::quat_2_Rot(new_value); 95 | } 96 | 97 | /// Rotation access 98 | Eigen::Matrix Rot() const { 99 | return _R; 100 | } 101 | 102 | /// FEJ Rotation access 103 | Eigen::Matrix Rot_fe() const { 104 | return _Rfe; 105 | } 106 | 107 | protected: 108 | 109 | // Stores the rotation 110 | Eigen::Matrix _R; 111 | 112 | // Stores the first-estimate rotation 113 | Eigen::Matrix _Rfe; 114 | 115 | }; 116 | }; 117 | 118 | #endif //CALIB_QUAT_H 119 | -------------------------------------------------------------------------------- /src/types/Type.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/5/20. 3 | // 4 | 5 | #ifndef CALIB_TYPE_H 6 | #define CALIB_TYPE_H 7 | 8 | #include 9 | 10 | namespace calib_type { 11 | 12 | 13 | /** 14 | * @brief Base class for estimated variables. 15 | * 16 | * This class is used how variables are represented or updated (e.g., vectors or quaternions). 17 | * Each variable is defined by its error state size and its location in the covariance matrix. 18 | * We additionally require all sub-types to have a update procedure. 19 | */ 20 | class Type { 21 | 22 | public: 23 | 24 | /** 25 | * @brief Default constructor for our Type 26 | * 27 | * @param size_ degrees of freedom of variable (i.e., the size of the error state) 28 | */ 29 | Type(int size_) { 30 | _size = size_; 31 | } 32 | 33 | virtual ~Type() {}; 34 | 35 | /** 36 | * @brief Sets id used to track location of variable in the filter covariance 37 | * 38 | * @param new_id entry in filter covariance corresponding to this variable 39 | */ 40 | virtual void set_local_id(int new_id) { 41 | _id = new_id; 42 | } 43 | 44 | /** 45 | * @brief Access to variable id (i.e. its location in the covariance) 46 | */ 47 | int id() { 48 | return _id; 49 | } 50 | 51 | /** 52 | * @brief Access to variable size (i.e. its error state size) 53 | */ 54 | int size() { 55 | return _size; 56 | } 57 | 58 | /** 59 | * @brief Update variable due to perturbation of error state 60 | * 61 | * @param dx Perturbation used to update the variable through a defined "boxplus" operation 62 | */ 63 | virtual void update(const Eigen::VectorXd dx) = 0; 64 | 65 | /** 66 | * @brief Access variable's estimate 67 | */ 68 | virtual Eigen::MatrixXd value() const { 69 | return _value; 70 | } 71 | 72 | /** 73 | * @brief Access variable's first-estimate 74 | */ 75 | virtual Eigen::MatrixXd fe() const { 76 | return _fe; 77 | } 78 | 79 | /** 80 | * @brief Overwrite value of state's estimate 81 | * @param new_value New value that will overwrite state's value 82 | */ 83 | virtual void set_value(const Eigen::MatrixXd new_value) { 84 | assert(_value.rows()==new_value.rows()); 85 | assert(_value.cols()==new_value.cols()); 86 | _value = new_value; 87 | } 88 | 89 | /** 90 | * @brief Overwrite value of first-estimate 91 | * @param new_value New value that will overwrite state's fej 92 | */ 93 | virtual void set_fe(const Eigen::MatrixXd new_value) { 94 | assert(_fe.rows()==new_value.rows()); 95 | assert(_fe.cols()==new_value.cols()); 96 | _fe = new_value; 97 | } 98 | 99 | /** 100 | * @brief Create a clone of this variable 101 | */ 102 | virtual Type *clone() = 0; 103 | 104 | /** 105 | * @brief Determine if "check" is the same variable 106 | * If the passed variable is a sub-variable or the current variable this will return it. 107 | * Otherwise it will return a nullptr, meaning that it was unable to be found. 108 | * 109 | * @param check Type pointer to compare to 110 | */ 111 | virtual Type *check_if_same_variable(const Type *check) { 112 | if (check == this) { 113 | return this; 114 | } else { 115 | return nullptr; 116 | } 117 | } 118 | 119 | protected: 120 | 121 | /// First-estimate 122 | Eigen::MatrixXd _fe; 123 | 124 | /// Current best estimate 125 | Eigen::MatrixXd _value; 126 | 127 | /// Location of error state in covariance 128 | int _id = -1; 129 | 130 | /// Dimension of error state 131 | int _size = -1; 132 | 133 | 134 | }; 135 | } 136 | #endif //CALIB_TYPE_H 137 | -------------------------------------------------------------------------------- /src/types/Vec.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/5/20. 3 | // 4 | 5 | #ifndef CALIB_VEC_H 6 | #define CALIB_VEC_H 7 | 8 | #include "Type.h" 9 | 10 | namespace calib_type { 11 | /** 12 | * @brief Derived Type class that implements vector variables 13 | */ 14 | class Vec : public Type { 15 | 16 | public: 17 | 18 | /** 19 | * @brief Default constructor for Vec 20 | * @param dim Size of the vector (will be same as error state) 21 | */ 22 | Vec(int dim) : Type(dim) { 23 | _value = Eigen::VectorXd::Zero(dim); 24 | _fe = Eigen::VectorXd::Zero(dim); 25 | } 26 | 27 | ~Vec() {} 28 | 29 | /** 30 | * @brief Implements the update operation through standard vector addition 31 | * @param dx Additive error state correction 32 | */ 33 | void update(const Eigen::VectorXd dx) override { 34 | assert(dx.rows() == _size); 35 | set_value(_value + dx); 36 | } 37 | 38 | /** 39 | * @brief Performs all the cloning 40 | */ 41 | Type *clone() override { 42 | Type *Clone = new Vec(_size); 43 | Clone->set_value(value()); 44 | Clone->set_fe(fe()); 45 | return Clone; 46 | } 47 | }; 48 | }; 49 | 50 | #endif //CALIB_VEC_H 51 | -------------------------------------------------------------------------------- /src/update/UpdaterCameraLidarConstraint.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 4/5/22. 3 | // 4 | 5 | #ifndef CAMERA_IMU_LIDAR_CALIBRATION_UPDATERCAMERALIDARCONSTRAINT_H 6 | #define CAMERA_IMU_LIDAR_CALIBRATION_UPDATERCAMERALIDARCONSTRAINT_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "utils/color.h" 13 | #include "utils/quat_ops.h" 14 | 15 | #include "state/State.h" 16 | #include "state/StateHelper.h" 17 | 18 | #include "UpdaterOptions.h" 19 | 20 | #include 21 | #include 22 | 23 | namespace calib_estimator { 24 | class UpdaterCameraLidarConstraint { 25 | public: 26 | UpdaterCameraLidarConstraint(UpdaterOptions &options) : _options(options) { 27 | for(int i = 1; i < 500; i++) { 28 | boost::math::chi_squared chi_squared_dist(i); 29 | chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); 30 | } 31 | } 32 | 33 | /// Given plane measurement from both camera and lidar, estimate the state 34 | double updatePlaneToPlaneConstraint(State *current_state, 35 | Eigen::Vector4d nd_c, double timestamp_camera, 36 | Eigen::Vector4d nd_l, double timestamp_lidar, 37 | bool &did_update); 38 | 39 | /// Given plane measurement from both camera and lidar, estimate the state 40 | void updatePlaneToPlaneConstraint(State *current_state, Eigen::Matrix4d G_T_I0, 41 | std::vector object_points_c0, 42 | Eigen::Vector4d nd_L, 43 | double timestamp, bool &did_update); 44 | 45 | /// Given plane measurement from both camera and lidar, estimate the state 46 | void updatePlaneToPlaneConstraint(State *current_state, Eigen::Matrix4d G_T_I0, 47 | std::vector object_points_c0, 48 | std::map nd_l_vector, 49 | bool &did_update); 50 | protected: 51 | 52 | /// Options used during update 53 | UpdaterOptions _options; 54 | 55 | /// Chi squared 95th percentile table (lookup would be size of residual) 56 | std::map chi_squared_table; 57 | }; 58 | } 59 | #endif //CAMERA_IMU_LIDAR_CALIBRATION_UPDATERCAMERALIDARCONSTRAINT_H 60 | -------------------------------------------------------------------------------- /src/update/UpdaterCameraTracking.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 3/24/22. 3 | // 4 | 5 | #ifndef CAMERA_IMU_LIDAR_CALIBRATION_UPDATERCAMERATRACKING_H 6 | #define CAMERA_IMU_LIDAR_CALIBRATION_UPDATERCAMERATRACKING_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "state/State.h" 13 | #include "state/StateHelper.h" 14 | #include "utils/quat_ops.h" 15 | #include "utils/color.h" 16 | 17 | #include "UpdaterOptions.h" 18 | 19 | #include "relpose/relativePose.h" 20 | 21 | #include 22 | #include 23 | 24 | namespace calib_estimator { 25 | /// Will compute the system for the lidar odometry measurement and update the filter 26 | class UpdaterCameraTracking { 27 | public: 28 | UpdaterCameraTracking(UpdaterOptions &options) : _options(options) { 29 | for(int i = 1; i < 500; i++) { 30 | boost::math::chi_squared chi_squared_dist(i); 31 | chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); 32 | } 33 | } 34 | 35 | /// Given camera odometry, this will use them to update the state 36 | /// state: The current state of the system 37 | /// lodom: relative lidar odometry result that can be used for update 38 | double updateImage2Image(State *current_state, relativePose lodom, bool &did_update); 39 | 40 | /// Given camera odometry, this will use them to update the state 41 | /// state: The current state of the system 42 | /// L0_T_Lk: global camera odometry result that can be used for update 43 | double updateImage2FirstImage(State *current_state, Eigen::Matrix4d L0_T_Lk, Eigen::Matrix4d G_T_I1, double timestamp, bool &did_update); 44 | 45 | /// Given camera pixel detections, this will use them to update the state 46 | /// state: The current state of the system 47 | double updatePixelBased(State *current_state, Eigen::Matrix4d G_T_I0, 48 | std::vector pixel_measurements, 49 | std::vector object_points_c0, 50 | double timestamp, bool &did_update); 51 | 52 | protected: 53 | 54 | /// Options used during update 55 | UpdaterOptions _options; 56 | 57 | /// Chi squared 95th percentile table (lookup would be size of residual) 58 | std::map chi_squared_table; 59 | 60 | }; 61 | }; 62 | 63 | #endif //CAMERA_IMU_LIDAR_CALIBRATION_UPDATERCAMERATRACKING_H 64 | -------------------------------------------------------------------------------- /src/update/UpdaterLidarOdometry.h: -------------------------------------------------------------------------------- 1 | #ifndef CALIB_ESTIMATOR_UPDATERHEC_H 2 | #define CALIB_ESTIMATOR_UPDATERHEC_H 3 | 4 | #include 5 | #include "state/State.h" 6 | #include "state/StateHelper.h" 7 | #include "utils/quat_ops.h" 8 | #include "utils/color.h" 9 | 10 | #include "UpdaterOptions.h" 11 | 12 | #include "relpose/relativePose.h" 13 | 14 | #include 15 | #include 16 | 17 | namespace calib_estimator { 18 | /// Will compute the system for the lidar odometry measurement and update the filter 19 | class UpdaterLidarOdometry { 20 | public: 21 | UpdaterLidarOdometry(UpdaterOptions &options) : _options(options) { 22 | for(int i = 1; i < 500; i++) { 23 | boost::math::chi_squared chi_squared_dist(i); 24 | chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); 25 | } 26 | } 27 | 28 | /// Given lidar odometry, this will use them to update the state 29 | /// state: The current state of the system 30 | /// lodom: relative lidar odometry result that can be used for update 31 | void updateScan2Scan(State *current_state, relativePose lodom, bool &did_update); 32 | void updateScan2GlobalMapRotation(State *current_state, Eigen::Matrix4d L1_T_Lk, Eigen::Matrix4d G_T_I1, double timestamp, bool& did_update); 33 | 34 | /// Given lidar odometry, this will use them to update the state 35 | /// state: The current state of the system 36 | /// L0_T_Lk: global lidar odometry result that can be used for update 37 | void updateScan2GlobalMapTranslation(State *current_state, Eigen::Matrix4d L1_T_Lk, Eigen::Matrix4d G_T_I1, double timestamp, bool &did_update); 38 | 39 | // /// Find the clone index closest to the queried timestamp 40 | // static double findClosestCloneTimeStamp(State *state, double timestamp); 41 | 42 | protected: 43 | 44 | /// Options used during update 45 | UpdaterOptions _options; 46 | 47 | /// Chi squared 95th percentile table (lookup would be size of residual) 48 | std::map chi_squared_table; 49 | 50 | }; 51 | }; 52 | #endif //CALIB_ESTIMATOR_UPDATERHEC_H -------------------------------------------------------------------------------- /src/update/UpdaterOptions.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/28/20. 3 | // 4 | 5 | #ifndef CALIB_ESTIMATOR_UPDATEROPTIONS_H 6 | #define CALIB_ESTIMATOR_UPDATEROPTIONS_H 7 | 8 | namespace calib_estimator { 9 | /// Struct which stores general updater options 10 | struct UpdaterOptions { 11 | /// What chi2_multiploer should we apply 12 | int chi2_multiplier = 5; 13 | 14 | /// Noise levels 15 | double noise_translation = 0.1; 16 | double noise_rotation = 0.1; 17 | double noise_pixel = 0.05; 18 | 19 | /// Do chi2 check? 20 | bool do_chi2_check = true; 21 | 22 | /// Print function of what parameters we have loaded 23 | void print() { 24 | printf("\t- chi2 multiplier: %d\n", chi2_multiplier); 25 | printf("\t- Noise translation: %f\n", noise_translation); 26 | printf("\t- Noise rotation: %f\n", noise_rotation); 27 | printf("\t- Noise pixel: %f\n", noise_pixel); 28 | printf("\t- Do chi2 check during update: %d\n", do_chi2_check); 29 | } 30 | }; 31 | }; 32 | #endif //CALIB_ESTIMATOR_UPDATEROPTIONS_H 33 | -------------------------------------------------------------------------------- /src/utils/color.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 11/7/20. 3 | // 4 | 5 | #ifndef CALIB_COLOR_H 6 | #define CALIB_COLOR_H 7 | 8 | #define RESET "\033[0m" 9 | #define BLACK "\033[30m" /* Black */ 10 | #define RED "\033[31m" /* Red */ 11 | #define GREEN "\033[32m" /* Green */ 12 | #define YELLOW "\033[33m" /* Yellow */ 13 | #define BLUE "\033[34m" /* Blue */ 14 | #define MAGENTA "\033[35m" /* Magenta */ 15 | #define CYAN "\033[36m" /* Cyan */ 16 | #define WHITE "\033[37m" /* White */ 17 | #define REDPURPLE "\033[95m" /* Red Purple */ 18 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 19 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 20 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 21 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 22 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 23 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 24 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 25 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 26 | #define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */ 27 | 28 | #endif //CALIB_COLOR_H 29 | -------------------------------------------------------------------------------- /src/utils/eigen_utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by usl on 12/10/20. 3 | // 4 | 5 | #ifndef CALIB_EIGEN_UTILS_H 6 | #define CALIB_EIGEN_UTILS_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace Eigen { 17 | 18 | template 19 | using aligned_vector = std::vector>; 20 | 21 | template 22 | using aligned_deque = std::deque>; 23 | 24 | template 25 | using aligned_map = std::map, 26 | Eigen::aligned_allocator>>; 27 | 28 | template 29 | using aligned_unordered_map = 30 | std::unordered_map, std::equal_to, 31 | Eigen::aligned_allocator>>; 32 | 33 | /** sorts vectors from large to small 34 | * vec: vector to be sorted 35 | * sorted_vec: sorted results 36 | * ind: the position of each element in the sort result in the original vector 37 | * https://www.programmersought.com/article/343692646/ 38 | */ 39 | inline void sort_vec(const Eigen::Vector3d& vec, 40 | Eigen::Vector3d& sorted_vec, 41 | Eigen::Vector3i& ind) { 42 | ind = Eigen::Vector3i::LinSpaced(vec.size(), 0, vec.size()-1);//[0 1 2] 43 | auto rule=[vec](int i, int j)->bool{ 44 | return vec(i)>vec(j); 45 | }; // regular expression, as a predicate of sort 46 | 47 | std::sort(ind.data(), ind.data()+ind.size(), rule); 48 | 49 | // The data member function returns a pointer to the first element of VectorXd, 50 | // similar to begin() 51 | for (int i=0;i 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace calib_core { 16 | 17 | typedef pcl::PointXYZI VPoint; 18 | typedef pcl::PointCloud VPointCloud; 19 | 20 | typedef pcl::PointXYZRGB colorPointT; 21 | typedef pcl::PointCloud colorPointCloudT; 22 | 23 | struct PointXYZIR8Y { 24 | PCL_ADD_POINT4D 25 | ; // quad-word XYZ 26 | float intensity; ///< laser intensity reading 27 | uint8_t ring; ///< laser ring number 28 | uint32_t t; 29 | float range;EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 30 | } EIGEN_ALIGN16; 31 | 32 | inline void downsampleCloud(pcl::PointCloud::Ptr in_cloud, 33 | pcl::PointCloud::Ptr out_cloud, 34 | float in_leaf_size) { 35 | pcl::VoxelGrid sor; 36 | sor.setInputCloud(in_cloud); 37 | sor.setLeafSize((float)in_leaf_size, (float)in_leaf_size, (float)in_leaf_size); 38 | sor.filter(*out_cloud); 39 | } 40 | } 41 | 42 | POINT_CLOUD_REGISTER_POINT_STRUCT( 43 | calib_core::PointXYZIR8Y, 44 | (float, x, x) 45 | (float, y, y) 46 | (float, z, z) 47 | (float, intensity, intensity) 48 | (uint8_t, ring, ring) 49 | (uint32_t, t, t)) 50 | 51 | typedef calib_core::PointXYZIR8Y TPoint; 52 | typedef pcl::PointCloud TPointCloud; 53 | 54 | namespace calib_core { 55 | inline void TPointCloud2VPointCloud(TPointCloud::Ptr input_pc, 56 | VPointCloud::Ptr output_pc) { 57 | output_pc->header = input_pc->header; 58 | output_pc->height = input_pc->height; 59 | output_pc->width = input_pc->width; 60 | output_pc->is_dense = input_pc->is_dense; 61 | output_pc->resize(output_pc->width * output_pc->height); 62 | for(int h = 0; h < input_pc->height; h++) { 63 | for(int w = 0; w < input_pc->width; w++) { 64 | if(isnan(input_pc->at(w,h).x)||isnan(input_pc->at(w,h).y)||isnan(input_pc->at(w,h).z)) 65 | continue; 66 | calib_core::VPoint point; 67 | point.x = input_pc->at(w,h).x; 68 | point.y = input_pc->at(w,h).y; 69 | point.z = input_pc->at(w,h).z; 70 | point.intensity = input_pc->at(w,h).intensity; 71 | output_pc->at(w,h) = point; 72 | } 73 | } 74 | } 75 | } 76 | 77 | namespace calib_core { 78 | inline void downsampleCloud(pcl::PointCloud::Ptr in_cloud, 79 | pcl::PointCloud::Ptr out_cloud, 80 | float in_leaf_size) { 81 | pcl::VoxelGrid sor; 82 | sor.setInputCloud(in_cloud); 83 | sor.setLeafSize((float)in_leaf_size, (float)in_leaf_size, (float)in_leaf_size); 84 | sor.filter(*out_cloud); 85 | } 86 | } 87 | #endif //CALIB_PCL_UTILS_H 88 | --------------------------------------------------------------------------------