├── pic ├── ui.png └── 3imu.png ├── depend_pack.rosinstall ├── .gitmodules ├── .gitignore ├── test └── li_calib_gui.cpp ├── include ├── utils │ ├── tic_toc.h │ ├── ceres_callbacks.h │ ├── pcl_utils.h │ ├── eigen_utils.hpp │ ├── dataset_reader.h │ ├── math_utils.h │ └── vlp_common.h ├── core │ ├── inertial_initializer.h │ ├── lidar_odometry.h │ ├── surfel_association.h │ ├── calibration.hpp │ ├── trajectory_manager.h │ └── scan_undistortion.h └── ui │ ├── calib_ui.h │ └── calib_helper.h ├── calib.sh ├── launch └── licalib_gui.launch ├── CMakeLists.txt ├── src ├── core │ ├── inertial_initializer.cpp │ ├── lidar_odometry.cpp │ ├── surfel_association.cpp │ └── trajectory_manager.cpp └── ui │ ├── calib_ui.cpp │ └── calib_helper.cpp ├── package.xml ├── README.md └── LICENSE /pic/ui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/APRIL-ZJU/lidar_IMU_calib/HEAD/pic/ui.png -------------------------------------------------------------------------------- /pic/3imu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/APRIL-ZJU/lidar_IMU_calib/HEAD/pic/3imu.png -------------------------------------------------------------------------------- /depend_pack.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ndt_omp 3 | uri: https://github.com/APRIL-ZJU/ndt_omp.git 4 | 5 | 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/Pangolin"] 2 | path = thirdparty/Pangolin 3 | url = https://github.com/stevenlovegrove/Pangolin.git 4 | [submodule "thirdparty/Kontiki"] 5 | path = thirdparty/Kontiki 6 | url = https://github.com/APRIL-ZJU/Kontiki.git 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pdf 2 | *.bag 3 | 4 | # Built objects 5 | *.so 6 | *.a 7 | *.pyc 8 | 9 | # Documentation build artifacts 10 | docs/_build/ 11 | 12 | # Python build artifacts 13 | build/ 14 | dist/ 15 | *.egg-info/ 16 | 17 | #CLion / PyCharm directories 18 | .cache/ 19 | .idea/ 20 | cmake-build-*/ 21 | 22 | build-*/ 23 | thirdparty/build-*/ 24 | thirdparty/build-* 25 | -------------------------------------------------------------------------------- /test/li_calib_gui.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | #include 23 | 24 | using namespace licalib; 25 | 26 | int main(int argc, char **argv) { 27 | ros::init(argc, argv, "li_calib_gui"); 28 | ros::NodeHandle n("~"); 29 | 30 | CalibInterface calib_app(n); 31 | 32 | calib_app.renderingLoop(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /include/utils/tic_toc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace licalib { 28 | 29 | class TicToc { 30 | public: 31 | TicToc() { 32 | tic(); 33 | } 34 | 35 | void tic() { 36 | start = std::chrono::system_clock::now(); 37 | } 38 | 39 | double toc() { 40 | end = std::chrono::system_clock::now(); 41 | std::chrono::duration elapsed_seconds = end - start; 42 | return elapsed_seconds.count() * 1000; 43 | } 44 | 45 | private: 46 | std::chrono::time_point start, end; 47 | }; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /calib.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | bag_path="/home/ha/rosbag/li_calib_data" 4 | 5 | outdoor_sync_bag_name=( 6 | #"Court-01.bag" 7 | #"Court-02.bag" 8 | #"Court-03.bag" 9 | #"Court-04.bag" 10 | #"Court-05.bag" 11 | ) 12 | 13 | indoor_sync_bag_name=( 14 | "Garage-01.bag" 15 | #"Garage-02.bag" 16 | #"Garage-03.bag" 17 | #"Garage-04.bag" 18 | #"Garage-05.bag" 19 | ) 20 | 21 | imu_topic_name=( 22 | "/imu1/data_sync" 23 | #"/imu2/data_sync" 24 | #"/imu3/data_sync" 25 | ) 26 | 27 | bag_start=1 28 | bag_durr=30 29 | scan4map=15 30 | timeOffsetPadding=0.015 31 | 32 | show_ui=true #false 33 | 34 | bag_count=-1 35 | sync_bag_name=(${outdoor_sync_bag_name[*]} ${indoor_sync_bag_name[*]}) 36 | for i in "${!sync_bag_name[@]}"; do 37 | let bag_count=bag_count+1 38 | 39 | ndtResolution=0.5 # indoor 40 | if [ $bag_count -lt ${#outdoor_sync_bag_name[*]} ]; then 41 | ndtResolution=1.0 # outdoor 42 | fi 43 | 44 | for j in "${!imu_topic_name[@]}"; do 45 | path_bag="$bag_path/${sync_bag_name[i]}" 46 | 47 | echo "topic_imu:=${imu_topic_name[j]}" 48 | echo "path_bag:=${path_bag}" 49 | echo "ndtResolution:=${ndtResolution}" 50 | echo "==============" 51 | 52 | roslaunch li_calib licalib_gui.launch \ 53 | topic_imu:="${imu_topic_name[j]}" \ 54 | path_bag:="${path_bag}" \ 55 | bag_start:="${bag_start}" \ 56 | bag_durr:="${bag_durr}" \ 57 | scan4map:="${scan4map}" \ 58 | lidar_model:="VLP_16" \ 59 | time_offset_padding:="${timeOffsetPadding}"\ 60 | ndtResolution:="${ndtResolution}" \ 61 | show_ui:="${show_ui}" 62 | done 63 | done 64 | -------------------------------------------------------------------------------- /launch/licalib_gui.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 | 31 | -------------------------------------------------------------------------------- /include/core/inertial_initializer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef CALIBR_INERTIALINITIALIZER_H 22 | #define CALIBR_INERTIALINITIALIZER_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace licalib { 30 | 31 | 32 | class InertialInitializer { 33 | 34 | public: 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | 37 | typedef std::shared_ptr Ptr; 38 | 39 | explicit InertialInitializer() : rotaion_initialized_(false), 40 | q_ItoS_est_(Eigen::Quaterniond::Identity()) { 41 | } 42 | 43 | bool EstimateRotation(TrajectoryManager::Ptr traj_manager, 44 | const Eigen::aligned_vector& odom_data); 45 | 46 | bool isInitialized() { 47 | return rotaion_initialized_; 48 | } 49 | 50 | Eigen::Quaterniond getQ_ItoS() { 51 | return q_ItoS_est_; 52 | } 53 | 54 | 55 | private: 56 | bool rotaion_initialized_; 57 | Eigen::Quaterniond q_ItoS_est_; 58 | 59 | }; 60 | 61 | 62 | } 63 | 64 | #endif //CALIBR_INERTIALINITIALIZER_H 65 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.2) 2 | project(li_calib) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -msse4.2") 6 | set(CMAKE_BUILD_TYPE "RELEASE") 7 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -msse4.2 -mavx") 8 | 9 | find_package(OpenMP) 10 | if (OPENMP_FOUND) 11 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 12 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 13 | endif() 14 | 15 | find_package(catkin REQUIRED COMPONENTS 16 | roscpp 17 | std_msgs 18 | rosbag 19 | geometry_msgs 20 | nav_msgs 21 | velodyne_msgs 22 | ndt_omp 23 | tf 24 | pcl_ros 25 | ) 26 | 27 | find_package(Eigen3 REQUIRED) 28 | find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) 29 | 30 | set(PANGOLIN_DIR "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/build-pangolin") 31 | find_package(Pangolin REQUIRED) 32 | 33 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/Kontiki) 34 | 35 | # Set link libraries used by all binaries 36 | list(APPEND thirdparty_libraries 37 | ${Boost_LIBRARIES} 38 | ${catkin_LIBRARIES} 39 | ${Pangolin_LIBRARIES} 40 | Kontiki 41 | ) 42 | 43 | catkin_package( 44 | INCLUDE_DIRS include 45 | # LIBRARIES li_calibr_lib 46 | # CATKIN_DEPENDS geometry_msgs nav_msgs roscpp std_msgs velodyne_msgs 47 | # DEPENDS system_lib 48 | ) 49 | 50 | include_directories( include 51 | ${catkin_INCLUDE_DIRS} 52 | ${EIGEN3_INCLUDE_DIR} 53 | ${Boost_INCLUDE_DIRS} 54 | ${Pangolin_INCLUDE_DIRS} 55 | ) 56 | 57 | add_library(li_calib_lib 58 | src/core/trajectory_manager.cpp 59 | src/core/inertial_initializer.cpp 60 | src/core/lidar_odometry.cpp 61 | src/core/surfel_association.cpp 62 | ) 63 | target_link_libraries(li_calib_lib ${thirdparty_libraries}) 64 | 65 | add_executable(li_calib_gui 66 | test/li_calib_gui.cpp 67 | src/ui/calib_helper.cpp 68 | src/ui/calib_ui.cpp) 69 | target_link_libraries(li_calib_gui li_calib_lib ${thirdparty_libraries}) -------------------------------------------------------------------------------- /include/ui/calib_ui.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef CALIB_UI_H 22 | #define CALIB_UI_H 23 | 24 | #include 25 | #include 26 | 27 | using namespace licalib; 28 | 29 | struct TranslationVector { 30 | Eigen::Vector3d trans = Eigen::Vector3d(0,0,0); 31 | }; 32 | 33 | class CalibInterface : public CalibrHelper { 34 | public: 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | CalibInterface(ros::NodeHandle& nh); 37 | 38 | void initGui(); 39 | 40 | void renderingLoop(); 41 | 42 | void showCalibResult(); 43 | 44 | void resetModelView() { 45 | s_cam_.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0,40,0,0,0,pangolin::AxisNegY)); 46 | } 47 | 48 | private: 49 | static constexpr int UI_WIDTH = 300; 50 | 51 | pangolin::View *pointcloud_view_display_; 52 | pangolin::OpenGlRenderState s_cam_; 53 | 54 | std::vector pangolin_colors_; 55 | 56 | pangolin::Var show_surfel_map_; 57 | pangolin::Var show_all_association_points_; 58 | pangolin::Var optimize_time_offset_; 59 | pangolin::Var show_lidar_frame_; 60 | 61 | pangolin::Var show_p_IinL_; 62 | pangolin::Var show_q_ItoL_; 63 | pangolin::Var show_gravity_; 64 | pangolin::Var show_gyro_bias_; 65 | pangolin::Var show_acce_bias_; 66 | pangolin::Var show_time_offset_; 67 | }; 68 | 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /include/utils/ceres_callbacks.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef _CERES_CALLBACKS_ 22 | #define _CERES_CALLBACKS_ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | namespace licalib { 30 | 31 | class CheckStateCallback : public ceres::IterationCallback { 32 | public: 33 | CheckStateCallback() : iteration_(0u) {} 34 | 35 | ~CheckStateCallback() {} 36 | 37 | void addCheckState(const std::string& description, size_t block_size, 38 | double* param_block) { 39 | parameter_block_descr.push_back(description); 40 | parameter_block_sizes.push_back(block_size); 41 | parameter_blocks.push_back(param_block); 42 | } 43 | 44 | ceres::CallbackReturnType operator()( 45 | const ceres::IterationSummary& summary) { 46 | std::cout << "Iteration: " << iteration_ << std::endl; 47 | for (size_t i = 0; i < parameter_block_descr.size(); ++i) { 48 | std::cout << parameter_block_descr.at(i) << " "; 49 | for (size_t k = 0; k < parameter_block_sizes.at(i); ++k) 50 | std::cout << parameter_blocks.at(i)[k] << " "; 51 | std::cout << std::endl; 52 | } 53 | 54 | ++iteration_; 55 | return ceres::SOLVER_CONTINUE; 56 | } 57 | 58 | private: 59 | 60 | std::vector parameter_block_descr; 61 | std::vector parameter_block_sizes; 62 | std::vector parameter_blocks; 63 | 64 | // Count iterations locally 65 | size_t iteration_; 66 | }; 67 | 68 | } 69 | 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /include/ui/calib_helper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef CALIB_HELPER_H 22 | #define CALIB_HELPER_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | //the following are UBUNTU/LINUX ONLY terminal color codes. 31 | #define RESET "\033[0m" 32 | #define BLACK "\033[30m" /* Black */ 33 | #define RED "\033[31m" /* Red */ 34 | #define GREEN "\033[32m" /* Green */ 35 | #define YELLOW "\033[33m" /* Yellow */ 36 | #define BLUE "\033[34m" /* Blue */ 37 | #define MAGENTA "\033[35m" /* Magenta */ 38 | #define CYAN "\033[36m" /* Cyan */ 39 | #define WHITE "\033[37m" /* White */ 40 | 41 | namespace licalib { 42 | 43 | class CalibrHelper { 44 | public: 45 | 46 | enum CalibStep { 47 | Error = 0, 48 | Start, 49 | InitializationDone, 50 | DataAssociationDone, 51 | BatchOptimizationDone, 52 | RefineDone 53 | }; 54 | 55 | explicit CalibrHelper(ros::NodeHandle& nh); 56 | 57 | bool createCacheFolder(const std::string& bag_path); 58 | 59 | void Initialization(); 60 | 61 | void DataAssociation(); 62 | 63 | void BatchOptimization(); 64 | 65 | void Refinement(); 66 | 67 | void saveCalibResult(const std::string& calib_result_file) const; 68 | 69 | void saveMap() const; 70 | 71 | protected: 72 | 73 | void Mapping(bool relocalization = false); 74 | 75 | /// dataset 76 | std::string cache_path_; 77 | std::string topic_imu_; 78 | std::string bag_path_; 79 | 80 | /// optimization 81 | CalibStep calib_step_; 82 | int iteration_step_; 83 | bool opt_time_offset_; 84 | 85 | /// lidar odometry 86 | double map_time_; 87 | double ndt_resolution_; 88 | double scan4map_time_; 89 | 90 | /// data association 91 | double associated_radius_; 92 | double plane_lambda_; 93 | 94 | std::shared_ptr dataset_reader_; 95 | InertialInitializer::Ptr rotation_initializer_; 96 | TrajectoryManager::Ptr traj_manager_; 97 | LiDAROdometry::Ptr lidar_odom_; 98 | SurfelAssociation::Ptr surfel_association_; 99 | 100 | ScanUndistortion::Ptr scan_undistortion_; 101 | }; 102 | 103 | } 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /include/utils/pcl_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef PCL_UTILS_H 22 | #define PCL_UTILS_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace licalib { 32 | 33 | typedef pcl::PointXYZI VPoint; 34 | typedef pcl::PointCloud VPointCloud; 35 | 36 | typedef pcl::PointXYZRGB colorPointT; 37 | typedef pcl::PointCloud colorPointCloudT; 38 | 39 | struct PointXYZIT { 40 | PCL_ADD_POINT4D 41 | float intensity; 42 | double timestamp; 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 44 | } EIGEN_ALIGN16; 45 | 46 | inline void downsampleCloud(pcl::PointCloud::Ptr in_cloud, 47 | pcl::PointCloud::Ptr out_cloud, 48 | float in_leaf_size) { 49 | pcl::VoxelGrid sor; 50 | sor.setInputCloud(in_cloud); 51 | sor.setLeafSize((float)in_leaf_size, (float)in_leaf_size, (float)in_leaf_size); 52 | sor.filter(*out_cloud); 53 | } 54 | 55 | }; 56 | 57 | POINT_CLOUD_REGISTER_POINT_STRUCT(licalib::PointXYZIT, 58 | (float, x, x) 59 | (float, y, y) 60 | (float, z, z) 61 | (float, intensity, intensity) 62 | (double, timestamp, timestamp)) 63 | 64 | typedef licalib::PointXYZIT TPoint; 65 | typedef pcl::PointCloud TPointCloud; 66 | 67 | inline void TPointCloud2VPointCloud(TPointCloud::Ptr input_pc, 68 | licalib::VPointCloud::Ptr output_pc) { 69 | output_pc->header = input_pc->header; 70 | output_pc->height = input_pc->height; 71 | output_pc->width = input_pc->width; 72 | output_pc->is_dense = input_pc->is_dense; 73 | output_pc->resize(output_pc->width * output_pc->height); 74 | for(int h = 0; h < input_pc->height; h++) { 75 | for(int w = 0; w < input_pc->width; w++) { 76 | licalib::VPoint point; 77 | point.x = input_pc->at(w,h).x; 78 | point.y = input_pc->at(w,h).y; 79 | point.z = input_pc->at(w,h).z; 80 | point.intensity = input_pc->at(w,h).intensity; 81 | output_pc->at(w,h) = point; 82 | } 83 | } 84 | } 85 | 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/core/inertial_initializer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #include 22 | #include 23 | 24 | namespace licalib { 25 | 26 | bool InertialInitializer::EstimateRotation( 27 | TrajectoryManager::Ptr traj_manager, 28 | const Eigen::aligned_vector& odom_data) { 29 | 30 | int flags = kontiki::trajectories::EvalOrientation; 31 | std::shared_ptr p_traj 32 | = traj_manager->getTrajectory(); 33 | 34 | Eigen::aligned_vector A_vec; 35 | for (size_t j = 1; j < odom_data.size(); ++j) { 36 | size_t i = j - 1; 37 | double ti = odom_data.at(i).timestamp; 38 | double tj = odom_data.at(j).timestamp; 39 | if (tj >= p_traj->MaxTime()) 40 | break; 41 | auto result_i = p_traj->Evaluate(ti, flags); 42 | auto result_j = p_traj->Evaluate(tj, flags); 43 | Eigen::Quaterniond delta_qij_imu = result_i->orientation.conjugate() 44 | * result_j->orientation; 45 | 46 | Eigen::Matrix3d R_Si_toS0 = odom_data.at(i).pose.topLeftCorner<3,3>(); 47 | Eigen::Matrix3d R_Sj_toS0 = odom_data.at(j).pose.topLeftCorner<3,3>(); 48 | Eigen::Matrix3d delta_ij_sensor = R_Si_toS0.transpose() * R_Sj_toS0; 49 | Eigen::Quaterniond delta_qij_sensor(delta_ij_sensor); 50 | 51 | Eigen::AngleAxisd R_vector1(delta_qij_sensor.toRotationMatrix()); 52 | Eigen::AngleAxisd R_vector2(delta_qij_imu.toRotationMatrix()); 53 | double delta_angle = 180 / M_PI * std::fabs(R_vector1.angle() - R_vector2.angle()); 54 | double huber = delta_angle > 1.0 ? 1.0/delta_angle : 1.0; 55 | 56 | Eigen::Matrix4d lq_mat = mathutils::LeftQuatMatrix(delta_qij_sensor); 57 | Eigen::Matrix4d rq_mat = mathutils::RightQuatMatrix(delta_qij_imu); 58 | A_vec.push_back(huber * (lq_mat - rq_mat)); 59 | } 60 | size_t valid_size = A_vec.size(); 61 | if (valid_size < 15) { 62 | return false; 63 | } 64 | Eigen::MatrixXd A(valid_size * 4, 4); 65 | for (size_t i = 0; i < valid_size; ++i) 66 | A.block<4, 4>(i * 4, 0) = A_vec.at(i); 67 | 68 | Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); 69 | 70 | Eigen::Matrix x = svd.matrixV().col(3); 71 | Eigen::Quaterniond q_ItoS_est(x); 72 | Eigen::Vector4d cov = svd.singularValues(); 73 | 74 | if (cov(2) > 0.25) { 75 | q_ItoS_est_ = q_ItoS_est; 76 | rotaion_initialized_ = true; 77 | return true; 78 | } else { 79 | return false; 80 | } 81 | } 82 | 83 | } 84 | -------------------------------------------------------------------------------- /include/utils/eigen_utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Definition of the standard containers with Eigen allocator. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | namespace Eigen { 50 | 51 | template 52 | using aligned_vector = std::vector>; 53 | 54 | template 55 | using aligned_deque = std::deque>; 56 | 57 | template 58 | using aligned_map = std::map, 59 | Eigen::aligned_allocator>>; 60 | 61 | template 62 | using aligned_unordered_map = 63 | std::unordered_map, std::equal_to, 64 | Eigen::aligned_allocator>>; 65 | 66 | /** sorts vectors from large to small 67 | * vec: vector to be sorted 68 | * sorted_vec: sorted results 69 | * ind: the position of each element in the sort result in the original vector 70 | * https://www.programmersought.com/article/343692646/ 71 | */ 72 | inline void sort_vec(const Eigen::Vector3d& vec, 73 | Eigen::Vector3d& sorted_vec, 74 | Eigen::Vector3i& ind) { 75 | ind = Eigen::Vector3i::LinSpaced(vec.size(), 0, vec.size()-1);//[0 1 2] 76 | auto rule=[vec](int i, int j)->bool{ 77 | return vec(i)>vec(j); 78 | }; // regular expression, as a predicate of sort 79 | 80 | std::sort(ind.data(), ind.data()+ind.size(), rule); 81 | 82 | // The data member function returns a pointer to the first element of VectorXd, 83 | // similar to begin() 84 | for (int i=0;i. 20 | */ 21 | #ifndef CALIBR_LIDAR_ODOMETRY_H 22 | #define CALIBR_LIDAR_ODOMETRY_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace licalib { 30 | 31 | class LiDAROdometry { 32 | 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | typedef std::shared_ptr Ptr; 36 | 37 | struct OdomData { 38 | double timestamp; 39 | Eigen::Matrix4d pose; // cur scan to first scan 40 | }; 41 | 42 | explicit LiDAROdometry(double ndtResolution = 0.5); 43 | 44 | static pclomp::NormalDistributionsTransform::Ptr ndtInit( 45 | double ndt_resolution); 46 | 47 | void feedScan(double timestamp, 48 | VPointCloud::Ptr cur_scan, 49 | Eigen::Matrix4d pose_predict = Eigen::Matrix4d::Identity(), 50 | const bool update_map = true); 51 | 52 | void clearOdomData(); 53 | 54 | void setTargetMap(VPointCloud::Ptr map_cloud_in); 55 | 56 | void saveTargetMap(const std::string& path) const { 57 | std::cout << "Save NDT target map to " << path 58 | << "; size: " << map_cloud_->size() << std::endl; 59 | pcl::io::savePCDFileASCII(path, *map_cloud_); 60 | } 61 | 62 | const VPointCloud::Ptr getTargetMap(){ 63 | return map_cloud_; 64 | } 65 | 66 | const pclomp::NormalDistributionsTransform::Ptr& getNDTPtr() const { 67 | return ndt_omp_; 68 | } 69 | 70 | const Eigen::aligned_vector &get_odom_data() const { 71 | return odom_data_; 72 | } 73 | 74 | private: 75 | 76 | void registration(const VPointCloud::Ptr& cur_scan, 77 | const Eigen::Matrix4d& pose_predict, 78 | Eigen::Matrix4d& pose_out, 79 | VPointCloud::Ptr scan_in_target); 80 | 81 | void updateKeyScan(const VPointCloud::Ptr& cur_scan, const OdomData& odom_data); 82 | 83 | bool checkKeyScan(const OdomData& odomdata); 84 | 85 | // Normalize angle to be between [-180, 180] 86 | static inline double normalize_angle(double ang_degree) { 87 | if(ang_degree > 180) 88 | ang_degree -= 360; 89 | 90 | if(ang_degree < -180) 91 | ang_degree += 360; 92 | return ang_degree; 93 | } 94 | 95 | private: 96 | 97 | VPointCloud::Ptr map_cloud_; 98 | 99 | pclomp::NormalDistributionsTransform::Ptr ndt_omp_; 100 | 101 | std::vector key_frame_index_; 102 | Eigen::aligned_vector odom_data_; 103 | }; 104 | 105 | 106 | } 107 | 108 | 109 | #endif // CALIBR_LIDAR_ODOMETRY_H 110 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | li_calib 4 | 0.0.0 5 | The calibr package 6 | 7 | 8 | 9 | 10 | ha 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | nav_msgs 54 | roscpp 55 | std_msgs 56 | velodyne_msgs 57 | rosbag 58 | tf 59 | ndt_omp 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | geometry_msgs 74 | nav_msgs 75 | roscpp 76 | std_msgs 77 | velodyne_msgs 78 | rosbag 79 | tf 80 | ndt_omp 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /include/core/surfel_association.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef SURFELASSOCIATION_HPP 22 | #define SURFELASSOCIATION_HPP 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace licalib { 30 | 31 | class SurfelAssociation { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | typedef std::shared_ptr Ptr; 35 | 36 | struct SurfelPoint { 37 | double timestamp; 38 | Eigen::Vector3d point; // raw data 39 | Eigen::Vector3d point_in_map; 40 | size_t plane_id; 41 | }; 42 | 43 | struct SurfelPlane { 44 | Eigen::Vector4d p4; 45 | Eigen::Vector3d Pi; // Closest Point Paramization 46 | Eigen::Vector3d boxMin; 47 | Eigen::Vector3d boxMax; 48 | VPointCloud cloud; 49 | VPointCloud cloud_inlier; 50 | }; 51 | 52 | explicit SurfelAssociation(double associated_radius = 0.05, 53 | double plane_lambda = 0.7) 54 | : associated_radius_(associated_radius), 55 | p_lambda_(plane_lambda), 56 | map_timestamp_(0) { 57 | initColorList(); 58 | } 59 | 60 | void setSurfelMap(const pclomp::NormalDistributionsTransform::Ptr& ndtPtr, 61 | double timestamp = 0); 62 | 63 | void setPlaneLambda(double lambda) { 64 | p_lambda_ = lambda; 65 | } 66 | 67 | void getAssociation(const VPointCloud::Ptr& scan_inM, 68 | const TPointCloud::Ptr& scan_raw, 69 | size_t selected_num_per_ring = 2); 70 | 71 | void randomDownSample(int num_points_max = 5); 72 | 73 | void averageDownSmaple(int num_points_max = 5); 74 | 75 | void averageTimeDownSmaple(int step = 10); 76 | 77 | const Eigen::aligned_vector& get_surfel_planes() const { 78 | return surfel_planes_; 79 | } 80 | 81 | const Eigen::aligned_vector& get_surfel_points() const { 82 | return spoint_downsampled_; 83 | } 84 | 85 | double get_maptime() const { 86 | return map_timestamp_; 87 | } 88 | 89 | void saveSurfelsMap(std::string& path) const { 90 | std::cout << "Save surfel map to " << path 91 | << "; size: " << surfels_map_.size() << std::endl; 92 | pcl::io::savePCDFileASCII(path, surfels_map_); 93 | } 94 | 95 | private: 96 | void initColorList(); 97 | 98 | void clearSurfelMap(); 99 | 100 | static int checkPlaneType(const Eigen::Vector3d& eigen_value, 101 | const Eigen::Matrix3d& eigen_vector, 102 | const double& p_lambda); 103 | 104 | static bool fitPlane(const VPointCloud::Ptr& cloud, 105 | Eigen::Vector4d &coeffs, 106 | VPointCloud::Ptr cloud_inliers); 107 | 108 | static double point2PlaneDistance(Eigen::Vector3d &pt, 109 | Eigen::Vector4d &plane_coeff); 110 | 111 | void associateScanToSurfel(const size_t& surfel_idx, 112 | const VPointCloud::Ptr& scan, 113 | const double& radius, 114 | std::vector> &ring_masks) const; 115 | 116 | private: 117 | 118 | boost::circular_buffer color_list_; // for visualization 119 | 120 | double associated_radius_; 121 | double p_lambda_; 122 | double map_timestamp_; 123 | 124 | Eigen::aligned_vector surfel_planes_; 125 | colorPointCloudT surfels_map_; 126 | 127 | // associated results 128 | Eigen::aligned_vector> spoint_per_surfel_; 129 | Eigen::aligned_vector spoints_all_; 130 | 131 | Eigen::aligned_vector spoint_downsampled_; 132 | }; 133 | 134 | } 135 | 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LI-Calib 2 | 3 | ## Overview 4 | 5 | **LI-Calib** is a toolkit for calibrating the 6DoF rigid transformation and the time offset between a 3D LiDAR and an IMU. It's based on continuous-time batch optimization. IMU-based cost and LiDAR point-to-surfel distance are minimized jointly, which renders the calibration problem well-constrained in general scenarios. 6 | 7 | ## **Prerequisites** 8 | 9 | - [ROS](http://wiki.ros.org/ROS/Installation) (tested with Kinetic and Melodic) 10 | 11 | ```shell 12 | sudo apt-get install ros-melodic-pcl-ros ros-melodic-velodyne-msgs 13 | ``` 14 | 15 | - [Ceres](http://ceres-solver.org/installation.html) (tested with version 1.14.0) 16 | 17 | - [Kontiki](https://github.com/APRIL-ZJU/Kontiki) (Continuous-Time Toolkit) 18 | - Pangolin (for visualization and user interface) 19 | - [ndt_omp](https://github.com/APRIL-ZJU/ndt_omp) 20 | 21 | Note that **Kontiki** and **Pangolin** are included in the *thirdparty* folder. 22 | 23 | ## Install 24 | 25 | Clone the source code for the project and build it. 26 | 27 | ```shell 28 | # init ROS workspace 29 | mkdir -p ~/catkin_li_calib/src 30 | cd ~/catkin_li_calib/src 31 | catkin_init_workspace 32 | 33 | # Clone the source code for the project and build it. 34 | git clone https://github.com/APRIL-ZJU/lidar_IMU_calib 35 | 36 | # ndt_omp 37 | wstool init 38 | wstool merge lidar_IMU_calib/depend_pack.rosinstall 39 | wstool update 40 | # Pangolin 41 | cd lidar_imu_calib_beta 42 | ./build_submodules.sh 43 | ## build 44 | cd ../.. 45 | catkin_make 46 | source ./devel/setup.bash 47 | ``` 48 | 49 | ## Examples 50 | 51 | Currently the LI-Calib toolkit only support `VLP-16` but it is easy to expanded for other LiDARs. 52 | 53 | Run the calibration: 54 | 55 | ```shell 56 | ./src/lidar_IMU_calib/calib.sh 57 | ``` 58 | 59 | The options in `calib.sh` the have the following meaning: 60 | 61 | - `bag_path` path to the dataset. 62 | - `imu_topic` IMU topic. 63 | - `bag_start` the relative start time of the rosbag [s]. 64 | - `bag_durr` the duration for data association [s]. 65 | - `scan4map` the duration for NDT mapping [s]. 66 | - `timeOffsetPadding` maximum range in which the timeoffset may change during estimation [s]. 67 | - `ndtResolution` resolution for NDT [m]. 68 | 69 | UI 70 | 71 | Following the step: 72 | 73 | 1. `Initialization` 74 | 75 | 2. `DataAssociation` 76 | 77 | (The users are encouraged to toggle the `show_lidar_frame` for checking the odometry result. ) 78 | 79 | 3. `BatchOptimization` 80 | 81 | 4. `Refinement` 82 | 83 | 6. `Refinement` 84 | 85 | 7. ... 86 | 87 | 8. (you cloud try to optimize the time offset by choose `optimize_time_offset` then run `Refinement`) 88 | 89 | 9. `SaveMap` 90 | 91 | All the cache results are saved in the location of the dataset. 92 | 93 | **Note that the toolkit is implemented with only one thread, it would response slowly while processing data. Please be patient** 94 | 95 | ## Dataset 96 | 97 | 3imu 98 | 99 | Dataset for evaluating LI_Calib are available at [here](https://drive.google.com/drive/folders/1kYLVLMlwchBsjAoNqnrwq2N2Ow5na4VD?usp=sharing). 100 | 101 | We utilize an MCU (stm32f1) to simulate the synchronization Pulse Per Second (PPS) signal. The LiDAR's timestamps are synchronizing to UTC, and each IMU captures the rising edge of the PPS signal and outputs the latest data with a sync signal. Considering the jitter of the internal clock of MCU, the external synchronization method has some error (within a few microseconds). 102 | 103 | Each rosbag contains 7 topics: 104 | 105 | ``` 106 | /imu1/data : sensor_msgs/Imu 107 | /imu1/data_sync : sensor_msgs/Imu 108 | /imu2/data : sensor_msgs/Imu 109 | /imu2/data_sync : sensor_msgs/Imu 110 | /imu3/data : sensor_msgs/Imu 111 | /imu3/data_sync : sensor_msgs/Imu 112 | /velodyne_packets : velodyne_msgs/VelodyneScan 113 | ``` 114 | 115 | `/imu*/data` are raw data and the timestamps are coincide with the received time. 116 | 117 | `/imu*/data_sync` are the sync data, so do `/velodyne_packets` . 118 | 119 | ## Credits 120 | 121 | This code was developed by the [APRIL Lab](https://github.com/APRIL-ZJU) in Zhejiang University. 122 | 123 | For researchers that have leveraged or compared to this work, please cite the following: 124 | 125 | Jiajun Lv, Jinhong Xu, Kewei Hu, Yong Liu, Xingxing Zuo. Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation. IROS 2020. [[arxiv](https://arxiv.org/pdf/2007.14759.pdf)] 126 | 127 | ## License 128 | 129 | The code is provided under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt). 130 | -------------------------------------------------------------------------------- /src/core/lidar_odometry.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #include 22 | #include 23 | #include 24 | 25 | namespace licalib { 26 | 27 | LiDAROdometry::LiDAROdometry(double ndt_resolution) 28 | : map_cloud_(new VPointCloud()) { 29 | ndt_omp_ = ndtInit(ndt_resolution); 30 | } 31 | 32 | pclomp::NormalDistributionsTransform::Ptr 33 | LiDAROdometry::ndtInit(double ndt_resolution) { 34 | auto ndt_omp = pclomp::NormalDistributionsTransform::Ptr( 35 | new pclomp::NormalDistributionsTransform()); 36 | ndt_omp->setResolution(ndt_resolution); 37 | ndt_omp->setNumThreads(4); 38 | ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7); 39 | ndt_omp->setTransformationEpsilon(1e-3); 40 | ndt_omp->setStepSize(0.01); 41 | ndt_omp->setMaximumIterations(50); 42 | return ndt_omp; 43 | } 44 | 45 | void LiDAROdometry::feedScan(double timestamp, 46 | VPointCloud::Ptr cur_scan, 47 | Eigen::Matrix4d pose_predict, 48 | const bool update_map) { 49 | OdomData odom_cur; 50 | odom_cur.timestamp = timestamp; 51 | odom_cur.pose = Eigen::Matrix4d::Identity(); 52 | 53 | VPointCloud::Ptr scan_in_target(new VPointCloud()); 54 | if (map_cloud_->empty()) { 55 | scan_in_target = cur_scan; 56 | } else { 57 | Eigen::Matrix4d T_LtoM_predict = odom_data_.back().pose * pose_predict; 58 | registration(cur_scan, T_LtoM_predict, odom_cur.pose, scan_in_target); 59 | } 60 | odom_data_.push_back(odom_cur); 61 | 62 | if (update_map) { 63 | updateKeyScan(cur_scan, odom_cur); 64 | } 65 | } 66 | 67 | void LiDAROdometry::registration(const VPointCloud::Ptr& cur_scan, 68 | const Eigen::Matrix4d& pose_predict, 69 | Eigen::Matrix4d& pose_out, 70 | VPointCloud::Ptr scan_in_target) { 71 | VPointCloud::Ptr p_filtered_cloud(new VPointCloud()); 72 | downsampleCloud(cur_scan, p_filtered_cloud, 0.5); 73 | 74 | ndt_omp_->setInputSource(p_filtered_cloud); 75 | ndt_omp_->align(*scan_in_target, pose_predict.cast()); 76 | 77 | pose_out = ndt_omp_->getFinalTransformation().cast(); 78 | } 79 | 80 | void LiDAROdometry::updateKeyScan(const VPointCloud::Ptr& cur_scan, 81 | const OdomData& odom_data) { 82 | if (checkKeyScan(odom_data)) { 83 | 84 | VPointCloud::Ptr filtered_cloud(new VPointCloud()); 85 | downsampleCloud(cur_scan, filtered_cloud, 0.1); 86 | 87 | VPointCloud::Ptr scan_in_target(new VPointCloud()); 88 | pcl::transformPointCloud(*filtered_cloud, *scan_in_target, odom_data.pose); 89 | 90 | *map_cloud_ += *scan_in_target; 91 | ndt_omp_->setInputTarget(map_cloud_); 92 | key_frame_index_.push_back(odom_data_.size()); 93 | } 94 | } 95 | 96 | bool LiDAROdometry::checkKeyScan(const OdomData& odom_data) { 97 | static Eigen::Vector3d position_last(0,0,0); 98 | static Eigen::Vector3d ypr_last(0,0,0); 99 | 100 | Eigen::Vector3d position_now = odom_data.pose.block<3,1>(0,3); 101 | double dist = (position_now - position_last).norm(); 102 | 103 | const Eigen::Matrix3d rotation (odom_data.pose.block<3,3> (0,0)); 104 | Eigen::Vector3d ypr = mathutils::R2ypr(rotation); 105 | Eigen::Vector3d delta_angle = ypr - ypr_last; 106 | for (size_t i = 0; i < 3; i++) 107 | delta_angle(i) = normalize_angle(delta_angle(i)); 108 | delta_angle = delta_angle.cwiseAbs(); 109 | 110 | if (key_frame_index_.size() == 0 || dist > 0.2 111 | || delta_angle(0) > 5.0 || delta_angle(1) > 5.0 || delta_angle(2) > 5.0) { 112 | position_last = position_now; 113 | ypr_last = ypr; 114 | return true; 115 | } 116 | return false; 117 | } 118 | 119 | void LiDAROdometry::setTargetMap(VPointCloud::Ptr map_cloud_in) { 120 | map_cloud_->clear(); 121 | pcl::copyPointCloud(*map_cloud_in, *map_cloud_); 122 | ndt_omp_->setInputTarget(map_cloud_); 123 | } 124 | 125 | void LiDAROdometry::clearOdomData() { 126 | key_frame_index_.clear(); 127 | odom_data_.clear(); 128 | } 129 | 130 | 131 | } 132 | 133 | 134 | -------------------------------------------------------------------------------- /include/core/calibration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef CALIBRATION_HPP 22 | #define CALIBRATION_HPP 23 | 24 | #define _USE_MATH_DEFINES 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace licalib { 32 | 33 | class CalibParamManager 34 | { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | typedef std::shared_ptr Ptr; 39 | 40 | CalibParamManager() : 41 | p_LinI(Eigen::Vector3d(0,0,0)) , 42 | q_LtoI(Eigen::Quaterniond::Identity()), 43 | gravity(Eigen::Vector3d(0, 0, -9.8)), 44 | time_offset(0), 45 | gyro_bias(Eigen::Vector3d(0,0,0)), 46 | acce_bias(Eigen::Vector3d(0,0,0)) { 47 | 48 | double gyroscope_noise_density = 1.745e-4; 49 | double accelerometer_noise_density = 5.88e-4; 50 | double imu_rate = 400.0; 51 | double lidar_noise = 0.02; 52 | 53 | double gyro_discrete = gyroscope_noise_density * std::sqrt(imu_rate); 54 | double acce_discrete = accelerometer_noise_density * std::sqrt(imu_rate); 55 | 56 | global_opt_gyro_weight = 1.0 / std::pow(gyro_discrete, 2); // 8.21e4 57 | global_opt_acce_weight = 1.0 / std::pow(acce_discrete, 2); // 7.23e3 58 | global_opt_lidar_weight = 1.0 / std::pow(lidar_noise, 2); // 2.5e3 59 | 60 | // fine-tuned parameter 61 | global_opt_gyro_weight = 28.0; 62 | global_opt_acce_weight = 18.5; 63 | global_opt_lidar_weight = 10.0; 64 | } 65 | 66 | void set_q_LtoI(Eigen::Quaterniond q) { 67 | q_LtoI = q; 68 | } 69 | 70 | void set_p_LinI(Eigen::Vector3d p) { 71 | p_LinI = p; 72 | } 73 | 74 | void set_gravity(Eigen::Vector3d g) { 75 | gravity = g; 76 | } 77 | 78 | void set_time_offset(double t) { 79 | time_offset = t; 80 | } 81 | 82 | void set_gyro_bias(Eigen::Vector3d gb) { 83 | gyro_bias = gb; 84 | } 85 | 86 | void set_acce_bias(Eigen::Vector3d ab) { 87 | acce_bias = ab; 88 | } 89 | 90 | void showStates() const { 91 | Eigen::Vector3d euler_LtoI = q_LtoI.toRotationMatrix().eulerAngles(0,1,2); 92 | euler_LtoI = euler_LtoI * 180 / M_PI; 93 | 94 | Eigen::Quaterniond q_ItoL = q_LtoI.inverse(); 95 | Eigen::Vector3d p_IinL = q_ItoL * (-p_LinI); 96 | Eigen::Vector3d euler_ItoL = q_ItoL.toRotationMatrix().eulerAngles(0,1,2); 97 | euler_ItoL = euler_ItoL * 180 / M_PI; 98 | 99 | std::cout << "P_LinI : " << p_LinI.transpose() << std::endl; 100 | std::cout << "euler_LtoI : " << euler_LtoI.transpose() << std::endl; 101 | std::cout << "P_IinL : " << p_IinL.transpose() << std::endl; 102 | std::cout << "euler_ItoL : " << euler_ItoL.transpose() << std::endl; 103 | std::cout << "time offset : " << time_offset << std::endl; 104 | std::cout << "gravity : " << gravity.transpose() << std::endl; 105 | std::cout << "acce bias : " << acce_bias.transpose() << std::endl; 106 | std::cout << "gyro bias : " << gyro_bias.transpose() << std::endl; 107 | } 108 | 109 | void save_result(const std::string& filename, const std::string& info) const { 110 | Eigen::Quaterniond q_ItoL = q_LtoI.inverse(); 111 | Eigen::Vector3d p_IinL = q_ItoL * (-p_LinI); 112 | 113 | std::ofstream outfile; 114 | outfile.open(filename, std::ios::app); 115 | outfile << info << "," 116 | << p_IinL(0) << "," << p_IinL(1) << "," << p_IinL(2) << "," 117 | << q_ItoL.x() << "," << q_ItoL.y() << "," << q_ItoL.z() << "," << q_ItoL.w() << "," 118 | << time_offset << "," << gravity(0) << "," << gravity(1) << "," << gravity(2) << "," 119 | << gyro_bias(0) << "," << gyro_bias(1) << "," <. 20 | */ 21 | #ifndef TRAJECTORY_MANAGER_H 22 | #define TRAJECTORY_MANAGER_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace licalib { 44 | class TrajectoryManager { 45 | using IMUSensor = kontiki::sensors::ConstantBiasImu; 46 | using LiDARSensor = kontiki::sensors::VLP16LiDAR; 47 | 48 | using SO3TrajEstimator = kontiki::TrajectoryEstimator; 49 | using R3TrajEstimator = kontiki::TrajectoryEstimator; 50 | using SplitTrajEstimator = kontiki::TrajectoryEstimator; 51 | 52 | using GyroMeasurement = kontiki::measurements::GyroscopeMeasurement; 53 | using AccelMeasurement = kontiki::measurements::AccelerometerMeasurement; 54 | using SurfMeasurement = kontiki::measurements::LiDARSurfelPoint; 55 | 56 | using OrientationMeasurement = kontiki::measurements::OrientationMeasurement; 57 | using PositionMeasurement = kontiki::measurements::PositionMeasurement; 58 | 59 | public: 60 | typedef std::shared_ptr Ptr; 61 | using Result = std::unique_ptr>; 62 | 63 | explicit TrajectoryManager(double start_time, double end_time, 64 | double knot_distance = 0.02, 65 | double time_offset_padding = 0) 66 | : time_offset_padding_(time_offset_padding), 67 | map_time_(0), 68 | imu_(std::make_shared()), 69 | lidar_(std::make_shared()), 70 | calib_param_manager(std::make_shared()) { 71 | assert(knot_distance > 0 && "knot_distance should be lager than 0"); 72 | 73 | double traj_start_time = start_time - time_offset_padding; 74 | double traj_end_time = end_time + time_offset_padding; 75 | traj_ = std::make_shared 76 | (knot_distance, knot_distance, traj_start_time, traj_start_time); 77 | initialTrajTo(traj_end_time); 78 | } 79 | 80 | void initialTrajTo(double max_time); 81 | 82 | void feedIMUData(const IO::IMUData& data); 83 | 84 | void initialSO3TrajWithGyro(); 85 | 86 | void trajInitFromSurfel(SurfelAssociation::Ptr surfels_association, 87 | bool opt_time_offset_ = false); 88 | 89 | bool evaluateIMUPose(double imu_time, int flags, Result& result) const; 90 | 91 | bool evaluateLidarPose(double lidar_time, Eigen::Quaterniond& q_LtoG, 92 | Eigen::Vector3d& p_LinG) const; 93 | 94 | bool evaluateLidarRelativeRotation(double lidar_time1, double lidar_time2, 95 | Eigen::Quaterniond& q_L2toL1) const; 96 | 97 | CalibParamManager::Ptr getCalibParamManager() const { 98 | return calib_param_manager; 99 | } 100 | 101 | double get_map_time() const { 102 | return map_time_; 103 | } 104 | 105 | /// Access the trajectory 106 | std::shared_ptr getTrajectory() const { 107 | return traj_; 108 | } 109 | 110 | private: 111 | template 112 | void addGyroscopeMeasurements( 113 | std::shared_ptr> estimator); 114 | 115 | template 116 | void addAccelerometerMeasurement( 117 | std::shared_ptr> estimator); 118 | 119 | template 120 | void addSurfMeasurement( 121 | std::shared_ptr> estimator, 122 | const SurfelAssociation::Ptr surfel_association); 123 | 124 | template 125 | void addCallback( 126 | std::shared_ptr> estimator); 127 | 128 | void printErrorStatistics(const std::string& intro, bool show_gyro = true, 129 | bool show_accel = true, bool show_lidar = true) const; 130 | 131 | double map_time_; 132 | double time_offset_padding_; 133 | std::shared_ptr traj_; 134 | std::shared_ptr imu_; 135 | std::shared_ptr lidar_; 136 | 137 | CalibParamManager::Ptr calib_param_manager; 138 | 139 | std::vector imu_data_; 140 | 141 | Eigen::aligned_vector closest_point_vec_; 142 | 143 | std::vector< std::shared_ptr> gyro_list_; 144 | std::vector< std::shared_ptr> accel_list_; 145 | std::vector< std::shared_ptr> surfelpoint_list_; 146 | }; 147 | } 148 | 149 | #endif 150 | -------------------------------------------------------------------------------- /include/core/scan_undistortion.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace licalib { 28 | 29 | class ScanUndistortion { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | typedef std::shared_ptr Ptr; 33 | 34 | explicit ScanUndistortion(TrajectoryManager::Ptr traj_manager, 35 | std::shared_ptr dataset) 36 | : traj_manager_(std::move(traj_manager)), 37 | dataset_reader_(std::move(dataset)) { 38 | } 39 | 40 | void undistortScan(bool correct_position = false) { 41 | scan_data_.clear(); 42 | 43 | for (const TPointCloud& scan_raw: dataset_reader_->get_scan_data()) { 44 | Eigen::Quaterniond q_L0_to_G; 45 | Eigen::Vector3d p_L0_in_G; 46 | double scan_timestamp = pcl_conversions::fromPCL(scan_raw.header.stamp).toSec(); 47 | if (!traj_manager_->evaluateLidarPose(scan_timestamp, q_L0_to_G, p_L0_in_G)) { 48 | std::cout << "[ScanUndistortion] : pass " << scan_timestamp << std::endl; 49 | continue; 50 | } 51 | 52 | VPointCloud::Ptr scan_in_target(new VPointCloud); 53 | undistort(q_L0_to_G.conjugate(), p_L0_in_G, scan_raw, 54 | scan_in_target, correct_position); 55 | scan_data_.insert({scan_in_target->header.stamp, scan_in_target}); 56 | } 57 | } 58 | 59 | void undistortScanInMap(bool correct_position = true) { 60 | scan_data_in_map_.clear(); 61 | map_cloud_ = VPointCloud::Ptr(new VPointCloud); 62 | Eigen::Quaterniond q_L0_to_G; 63 | Eigen::Vector3d p_L0_in_G; 64 | double map_start_time = traj_manager_->get_map_time(); 65 | traj_manager_->evaluateLidarPose(map_start_time, q_L0_to_G, p_L0_in_G); 66 | 67 | for (const TPointCloud& scan_raw: dataset_reader_->get_scan_data()) { 68 | VPointCloud::Ptr scan_in_target(new VPointCloud); 69 | undistort(q_L0_to_G.conjugate(), p_L0_in_G, scan_raw, 70 | scan_in_target, correct_position); 71 | scan_data_in_map_.insert({scan_in_target->header.stamp, scan_in_target}); 72 | *map_cloud_ += *scan_in_target; 73 | } 74 | } 75 | 76 | void undistortScanInMap(const Eigen::aligned_vector& odom_data) { 77 | scan_data_in_map_.clear(); 78 | map_cloud_ = VPointCloud::Ptr(new VPointCloud); 79 | 80 | for (size_t idx = 0; idx < dataset_reader_->get_scan_data().size(); idx++){ 81 | auto scan_raw = dataset_reader_->get_scan_data().at(idx); 82 | auto iter = scan_data_.find(scan_raw.header.stamp); 83 | if (iter == scan_data_.end()) { 84 | continue; 85 | } 86 | VPointCloud::Ptr scan_inMap = VPointCloud::Ptr(new VPointCloud); 87 | pcl::transformPointCloud(*(iter->second), *scan_inMap, odom_data.at(idx).pose); 88 | scan_data_in_map_.insert({scan_raw.header.stamp, scan_inMap}); 89 | *map_cloud_ += *scan_inMap; 90 | } 91 | } 92 | 93 | const std::map &get_scan_data() const { 94 | return scan_data_; 95 | } 96 | 97 | const std::map &get_scan_data_in_map() const { 98 | return scan_data_in_map_; 99 | } 100 | 101 | const VPointCloud::Ptr& get_map_cloud() const { 102 | return map_cloud_; 103 | } 104 | 105 | private: 106 | 107 | void undistort(const Eigen::Quaterniond& q_G_to_target, 108 | const Eigen::Vector3d& p_target_in_G, 109 | const TPointCloud& scan_raw, 110 | const VPointCloud::Ptr& scan_in_target, 111 | bool correct_position = false) const { 112 | scan_in_target->header = scan_raw.header; 113 | scan_in_target->height = scan_raw.height; 114 | scan_in_target->width = scan_raw.width; 115 | scan_in_target->resize(scan_raw.height * scan_raw.width); 116 | scan_in_target->is_dense = false; 117 | 118 | VPoint NanPoint; 119 | NanPoint.x = NAN; NanPoint.y = NAN; NanPoint.z = NAN; 120 | for (int h = 0; h < scan_raw.height; h++) { 121 | for (int w = 0; w < scan_raw.width; w++) { 122 | VPoint vpoint; 123 | if (pcl_isnan(scan_raw.at(w,h).x)) { 124 | vpoint = NanPoint; 125 | scan_in_target->at(w,h) = vpoint; 126 | continue; 127 | } 128 | double point_timestamp = scan_raw.at(w,h).timestamp; 129 | Eigen::Quaterniond q_Lk_to_G; 130 | Eigen::Vector3d p_Lk_in_G; 131 | if (!traj_manager_->evaluateLidarPose(point_timestamp, q_Lk_to_G, p_Lk_in_G)) { 132 | continue; 133 | } 134 | Eigen::Quaterniond q_LktoL0 = q_G_to_target * q_Lk_to_G; 135 | Eigen::Vector3d p_Lk(scan_raw.at(w,h).x, scan_raw.at(w,h).y, scan_raw.at(w,h).z); 136 | 137 | Eigen::Vector3d point_out; 138 | if (!correct_position) { 139 | point_out = q_LktoL0 * p_Lk; 140 | } else { 141 | point_out = q_LktoL0 * p_Lk + q_G_to_target * (p_Lk_in_G - p_target_in_G); 142 | } 143 | 144 | vpoint.x = point_out(0); 145 | vpoint.y = point_out(1); 146 | vpoint.z = point_out(2); 147 | vpoint.intensity = scan_raw.at(w,h).intensity; 148 | scan_in_target->at(w,h) = vpoint; 149 | } 150 | } 151 | } 152 | 153 | TrajectoryManager::Ptr traj_manager_; 154 | std::shared_ptr dataset_reader_; 155 | 156 | std::map scan_data_; 157 | std::map scan_data_in_map_; 158 | VPointCloud::Ptr map_cloud_; 159 | }; 160 | 161 | } -------------------------------------------------------------------------------- /src/ui/calib_ui.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #include 22 | 23 | const int SCAN_LENGTH = 300; 24 | 25 | std::ostream& operator << (std::ostream& out, const TranslationVector& t) { 26 | out<<"=["<> (std::istream& in, TranslationVector& t) { 31 | return in; 32 | } 33 | 34 | CalibInterface::CalibInterface(ros::NodeHandle& nh) : 35 | CalibrHelper(nh), 36 | show_surfel_map_("ui.show_surfel_map", true, false, true), 37 | show_all_association_points_("ui.show_all_associated_points", false, false, true), 38 | optimize_time_offset_("ui.optimize_time_offset", false, false, true), 39 | show_lidar_frame_("ui.show_lidar_frame", 1, 1, SCAN_LENGTH), 40 | show_p_IinL_("ui.position", TranslationVector()), 41 | show_q_ItoL_("ui.rpy", TranslationVector()), 42 | show_gravity_("ui.g", TranslationVector()), 43 | show_gyro_bias_("ui.GB", TranslationVector()), 44 | show_acce_bias_("ui.AB", TranslationVector()), 45 | show_time_offset_("ui.offset", 0.00, -0.1, 0.1) { 46 | 47 | bool show_ui = true; 48 | nh.param("show_ui", show_ui, true); 49 | 50 | if (show_ui) { 51 | initGui(); 52 | pangolin::ColourWheel cw; 53 | for (int i = 0; i < 200; i++) { 54 | pangolin_colors_.emplace_back(cw.GetUniqueColour()); 55 | } 56 | } else { 57 | Initialization(); 58 | 59 | DataAssociation(); 60 | 61 | BatchOptimization(); 62 | 63 | for (size_t iter = 0; iter < 7; iter++) 64 | Refinement(); 65 | 66 | opt_time_offset_ = true; 67 | Refinement(); 68 | 69 | saveMap(); 70 | std::cout << "Calibration finished." << std::endl; 71 | } 72 | } 73 | 74 | 75 | void CalibInterface::initGui() { 76 | pangolin::CreateWindowAndBind("Main", 1600, 1000); 77 | glEnable(GL_DEPTH_TEST); 78 | glDepthFunc(GL_LEQUAL); 79 | 80 | s_cam_.SetProjectionMatrix(pangolin::ProjectionMatrix(1600, 1000, 2000, 2000, 81 | 800, 500, 0.1, 1000)); 82 | s_cam_.SetModelViewMatrix(pangolin::ModelViewLookAt(0, 0, 40, 0, 0, 0, 83 | pangolin::AxisNegY)); 84 | pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, 85 | pangolin::Attach::Pix(UI_WIDTH)); 86 | pointcloud_view_display_ = &pangolin::CreateDisplay() 87 | .SetBounds(0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) 88 | .SetHandler(new pangolin::Handler3D(s_cam_)); 89 | 90 | pangolin::Var> initialization( 91 | "ui.Initialization", std::bind(&CalibInterface::Initialization, this)); 92 | 93 | pangolin::Var> data_association( 94 | "ui.DataAssociation", std::bind(&CalibInterface::DataAssociation, this)); 95 | 96 | pangolin::Var> batch_optimization( 97 | "ui.BatchOptimization", std::bind(&CalibInterface::BatchOptimization, this)); 98 | 99 | pangolin::Var> refinement( 100 | "ui.Refinement", std::bind(&CalibInterface::Refinement, this)); 101 | 102 | pangolin::Var> save_map( 103 | "ui.SaveMap", std::bind(&CalibInterface::saveMap, this)); 104 | 105 | /// short-cut 106 | pangolin::RegisterKeyPressCallback('r', 107 | [this](){resetModelView();}); 108 | 109 | pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'a', 110 | [&](){if(show_lidar_frame_ != 1) 111 | show_lidar_frame_ = show_lidar_frame_ - 1;}); 112 | 113 | pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'd', 114 | [&](){if(show_lidar_frame_ != SCAN_LENGTH) 115 | show_lidar_frame_ = show_lidar_frame_ + 1;}); 116 | } 117 | 118 | 119 | void CalibInterface::renderingLoop() { 120 | while (!pangolin::ShouldQuit() && ros::ok()) { 121 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 122 | pointcloud_view_display_->Activate(s_cam_); 123 | glClearColor(0, 0, 0, 1.0f); 124 | 125 | if (optimize_time_offset_) { 126 | opt_time_offset_ = true; 127 | } else { 128 | opt_time_offset_ = false; 129 | } 130 | 131 | // show map cloud 132 | if (!show_surfel_map_) { 133 | glPointSize(1); 134 | glBegin(GL_POINTS); 135 | if (calib_step_ == RefineDone) { 136 | for (VPoint p: scan_undistortion_->get_map_cloud()->points) { 137 | glColor3f(0, 1, 1); 138 | glVertex3d(p.x, p.y, p.z); 139 | } 140 | } else if (calib_step_ >= DataAssociationDone) { 141 | for (const VPoint& p: lidar_odom_->getTargetMap()->points) { 142 | glColor3f(0, 1, 1); 143 | glVertex3d(p.x, p.y, p.z); 144 | } 145 | } 146 | glEnd(); 147 | } 148 | else if (calib_step_ >= DataAssociationDone) { 149 | // show surfel map 150 | for (size_t i = 0; i < surfel_association_->get_surfel_planes().size(); i++) { 151 | glPointSize(1); 152 | glBegin(GL_POINTS); 153 | pangolin::Colour colour = pangolin_colors_[i % 200]; 154 | for (VPoint p: surfel_association_->get_surfel_planes().at(i).cloud_inlier) { 155 | glColor3f(colour.red, colour.green, colour.blue); 156 | glVertex3d(p.x, p.y, p.z); 157 | } 158 | glEnd(); 159 | } 160 | // show scan data 161 | if (!show_all_association_points_) { 162 | double scale = (double)dataset_reader_->get_scan_data().size() / SCAN_LENGTH; 163 | size_t frame_id = scale * show_lidar_frame_; 164 | if (frame_id < dataset_reader_->get_scan_data().size()) { 165 | pcl::uint64_t t = dataset_reader_->get_scan_data().at(frame_id).header.stamp; 166 | auto iter = scan_undistortion_->get_scan_data_in_map().find(t); 167 | if (iter != scan_undistortion_->get_scan_data_in_map().end()) { 168 | glPointSize(1); 169 | glBegin(GL_POINTS); 170 | for (VPoint p: iter->second->points) { 171 | glColor3f(1, 1, 1); 172 | glVertex3d(p.x, p.y, p.z); 173 | } 174 | glEnd(); 175 | } 176 | } 177 | } 178 | } 179 | 180 | if (show_all_association_points_) { 181 | glPointSize(5); 182 | glBegin(GL_POINTS); 183 | for (auto const &v : surfel_association_->get_surfel_points()) { 184 | glColor3f(1, 1, 1); 185 | glVertex3d(v.point_in_map(0), v.point_in_map(1), v.point_in_map(2)); 186 | } 187 | glEnd(); 188 | } 189 | 190 | if (calib_step_ >= BatchOptimizationDone) { 191 | showCalibResult(); 192 | } 193 | 194 | pangolin::FinishFrame(); 195 | usleep(10); // sleep 10 ms 196 | } 197 | } 198 | 199 | 200 | void CalibInterface::showCalibResult() { 201 | CalibParamManager::Ptr v = traj_manager_->getCalibParamManager(); 202 | 203 | Eigen::Vector3d euler_ItoL = (v->q_LtoI.conjugate()).toRotationMatrix().eulerAngles(0,1,2); 204 | euler_ItoL = euler_ItoL * 180 / M_PI; 205 | 206 | TranslationVector g, gb, ab, tra, q; 207 | tra.trans = v->q_LtoI.inverse() * (-v->p_LinI); 208 | g.trans = v->gravity; 209 | q.trans = euler_ItoL; 210 | gb.trans = v->gyro_bias; 211 | ab.trans = v->acce_bias; 212 | 213 | show_p_IinL_ = tra; 214 | show_q_ItoL_ = q; 215 | show_gravity_ = g; 216 | show_acce_bias_ = ab; 217 | show_gyro_bias_ = gb; 218 | show_time_offset_ = v->time_offset; 219 | } -------------------------------------------------------------------------------- /include/utils/dataset_reader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef DATASET_READER_H 22 | #define DATASET_READER_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace licalib { 39 | namespace IO { 40 | 41 | 42 | struct IMUData { 43 | double timestamp; 44 | Eigen::Matrix gyro; 45 | Eigen::Matrix accel; 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | }; 48 | 49 | struct PoseData { 50 | double timestamp; 51 | Eigen::Vector3d p; 52 | Eigen::Quaterniond q; 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 54 | }; 55 | 56 | 57 | enum LidarModelType { 58 | VLP_16, 59 | VLP_16_SIMU, 60 | }; 61 | 62 | class LioDataset { 63 | private: 64 | std::shared_ptr data_; 65 | 66 | std::shared_ptr bag_; 67 | 68 | Eigen::aligned_vector imu_data_; 69 | 70 | Eigen::aligned_vector scan_data_; 71 | std::vector scan_timestamps_; 72 | 73 | double start_time_; 74 | double end_time_; 75 | 76 | VelodyneCorrection::Ptr p_LidarConvert_; 77 | 78 | LidarModelType lidar_model_; 79 | 80 | public: 81 | LioDataset(LidarModelType lidar_model) : lidar_model_(lidar_model) { 82 | 83 | } 84 | 85 | void init() { 86 | switch (lidar_model_) { 87 | case LidarModelType::VLP_16: 88 | p_LidarConvert_ = VelodyneCorrection::Ptr( 89 | new VelodyneCorrection(VelodyneCorrection::ModelType::VLP_16)); 90 | break; 91 | case LidarModelType::VLP_16_SIMU: 92 | p_LidarConvert_ = VelodyneCorrection::Ptr( 93 | new VelodyneCorrection(VelodyneCorrection::ModelType::VLP_16)); 94 | break; 95 | default: 96 | std::cout << "LiDAR model " << lidar_model_ 97 | << " not support yet." << std::endl; 98 | } 99 | } 100 | 101 | bool read(const std::string path, 102 | const std::string imu_topic, 103 | const std::string lidar_topic, 104 | const double bag_start = -1.0, 105 | const double bag_durr = -1.0) { 106 | 107 | data_.reset(new LioDataset(lidar_model_)); 108 | data_->bag_.reset(new rosbag::Bag); 109 | data_->bag_->open(path, rosbag::bagmode::Read); 110 | 111 | init(); 112 | 113 | rosbag::View view; 114 | { 115 | std::vector topics; 116 | topics.push_back(imu_topic); 117 | topics.push_back(lidar_topic); 118 | 119 | rosbag::View view_full; 120 | view_full.addQuery(*data_->bag_); 121 | ros::Time time_init = view_full.getBeginTime(); 122 | time_init += ros::Duration(bag_start); 123 | ros::Time time_finish = (bag_durr < 0)? 124 | view_full.getEndTime() : time_init + ros::Duration(bag_durr); 125 | view.addQuery(*data_->bag_, rosbag::TopicQuery(topics), time_init, time_finish); 126 | } 127 | 128 | for (rosbag::MessageInstance const m : view) { 129 | const std::string &topic = m.getTopic(); 130 | 131 | if (lidar_topic == topic) { 132 | TPointCloud pointcloud; 133 | double timestamp = 0; 134 | 135 | if (m.getDataType() == std::string("velodyne_msgs/VelodyneScan")) { 136 | velodyne_msgs::VelodyneScan::ConstPtr vlp_msg = 137 | m.instantiate(); 138 | timestamp = vlp_msg->header.stamp.toSec(); 139 | p_LidarConvert_->unpack_scan(vlp_msg, pointcloud); 140 | } 141 | if (m.getDataType() == std::string("sensor_msgs/PointCloud2")) { 142 | sensor_msgs::PointCloud2::ConstPtr scan_msg = 143 | m.instantiate(); 144 | timestamp = scan_msg->header.stamp.toSec(); 145 | p_LidarConvert_->unpack_scan(scan_msg, pointcloud); 146 | } 147 | 148 | data_->scan_data_.emplace_back(pointcloud); 149 | data_->scan_timestamps_.emplace_back(timestamp); 150 | } 151 | 152 | if (imu_topic == topic) { 153 | sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 154 | double time = imu_msg->header.stamp.toSec(); 155 | 156 | data_->imu_data_.emplace_back(); 157 | data_->imu_data_.back().timestamp = time; 158 | data_->imu_data_.back().accel = Eigen::Vector3d( 159 | imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, 160 | imu_msg->linear_acceleration.z); 161 | data_->imu_data_.back().gyro = Eigen::Vector3d( 162 | imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, 163 | imu_msg->angular_velocity.z); 164 | } 165 | } 166 | 167 | std::cout << lidar_topic << ": " << data_->scan_data_.size() << std::endl; 168 | std::cout << imu_topic << ": " << data_->imu_data_.size() << std::endl; 169 | } 170 | 171 | 172 | /// Select the overlap of the dataset 173 | /// ----------------------- ...... -----------------------------> t 174 | /// | | | | | | 175 | /// scan_0 IMU_0 scan_1 scan_N-1 IMU_N scan_N 176 | /// 177 | /// selected data [scan_0, scan_N-1],[IMU_0, IMU_N] 178 | /// time [scan_0.t, scan_N.t) 179 | void adjustDataset() { 180 | assert(imu_data.size() > 0 && "No IMU data. Check your bag and imu topic"); 181 | assert(scan_data.size() > 0 && "No scan data. Check your bag and lidar topic"); 182 | 183 | assert(scan_timestamps.front() < imu_data.back().timestamp 184 | && scan_timestamps.back() > imu_data.front().timestamp 185 | && "Unvalid dataset. Check your dataset.. "); 186 | 187 | if (scan_timestamps_.front() > imu_data_.front().timestamp) { 188 | start_time_ = scan_timestamps_.front(); 189 | while (imu_data_.front().timestamp < start_time_) 190 | imu_data_.erase(imu_data_.begin()); 191 | 192 | } else { 193 | while ((*std::next(scan_timestamps_.begin())) < start_time_) { 194 | scan_data_.erase(scan_data_.begin()); 195 | scan_timestamps_.erase(scan_timestamps_.begin()); 196 | } 197 | start_time_ = scan_timestamps_.front(); 198 | } 199 | 200 | end_time_ = std::min(scan_timestamps_.back(), imu_data_.back().timestamp); 201 | 202 | while (imu_data_.back().timestamp > end_time_) 203 | imu_data_.pop_back(); 204 | 205 | while (scan_timestamps_.back() >= end_time_) { 206 | scan_data_.pop_back(); 207 | scan_timestamps_.pop_back(); 208 | } 209 | //std::cout<<"after adjust --> imu size : "<< imu_data_.size() << std::endl; 210 | } 211 | 212 | void reset() { data_.reset(); } 213 | 214 | std::shared_ptr get_data() const { 215 | // return std::dynamic_pointer_cast(data); 216 | return data_; 217 | } 218 | 219 | double get_start_time() const { 220 | return start_time_; 221 | } 222 | 223 | double get_end_time() const { 224 | return end_time_; 225 | } 226 | 227 | const std::vector &get_scan_timestamps() const { 228 | return scan_timestamps_; 229 | } 230 | 231 | const Eigen::aligned_vector &get_imu_data() const { 232 | return imu_data_; 233 | } 234 | const Eigen::aligned_vector &get_scan_data() const { 235 | return scan_data_; 236 | } 237 | 238 | }; 239 | 240 | } 241 | } 242 | 243 | #endif // DATASET_READER_H 244 | -------------------------------------------------------------------------------- /src/core/surfel_association.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include "omp.h" 28 | 29 | 30 | namespace licalib { 31 | 32 | void SurfelAssociation::initColorList() { 33 | color_list_.set_capacity(6); 34 | color_list_.push_back(0xFF0000); 35 | color_list_.push_back(0xFF00FF); 36 | color_list_.push_back(0x436EEE); 37 | color_list_.push_back(0xBF3EFF); 38 | color_list_.push_back(0xB4EEB4); 39 | color_list_.push_back(0xFFE7BA); 40 | } 41 | 42 | void SurfelAssociation::clearSurfelMap() { 43 | surfel_planes_.clear(); 44 | spoint_per_surfel_.clear(); 45 | spoint_downsampled_.clear(); 46 | surfels_map_.clear(); 47 | spoints_all_.clear(); 48 | } 49 | 50 | void SurfelAssociation::setSurfelMap( 51 | const pclomp::NormalDistributionsTransform::Ptr& ndtPtr, 52 | double timestamp) { 53 | clearSurfelMap(); 54 | map_timestamp_ = timestamp; 55 | 56 | //mCellSize = ndtPtr->getTargetCells().getLeafSize()(0); 57 | 58 | // check each cell 59 | Eigen::Vector3i counter(0,0,0); 60 | for (const auto &v : ndtPtr->getTargetCells().getLeaves()) { 61 | auto leaf = v.second; 62 | 63 | if (leaf.nr_points < 10) 64 | continue; 65 | int plane_type = checkPlaneType(leaf.getEvals(), leaf.getEvecs(), p_lambda_); 66 | if (plane_type < 0) 67 | continue; 68 | 69 | Eigen::Vector4d surfCoeff; 70 | VPointCloud::Ptr cloud_inliers = VPointCloud::Ptr(new VPointCloud); 71 | if (!fitPlane(leaf.pointList_.makeShared(), surfCoeff, cloud_inliers)) 72 | continue; 73 | 74 | counter(plane_type) += 1; 75 | SurfelPlane surfplane; 76 | surfplane.cloud = leaf.pointList_; 77 | surfplane.cloud_inlier = *cloud_inliers; 78 | surfplane.p4 = surfCoeff; 79 | surfplane.Pi = -surfCoeff(3) * surfCoeff.head<3>(); 80 | 81 | VPoint min, max; 82 | pcl::getMinMax3D(surfplane.cloud, min, max); 83 | surfplane.boxMin = Eigen::Vector3d(min.x, min.y, min.z); 84 | surfplane.boxMax = Eigen::Vector3d(max.x, max.y, max.z); 85 | 86 | surfel_planes_.push_back(surfplane); 87 | } 88 | 89 | spoint_per_surfel_.resize(surfel_planes_.size()); 90 | 91 | std::cout << "Plane type :" << counter.transpose() 92 | << "; Plane number: " << surfel_planes_.size() << std::endl; 93 | 94 | surfels_map_.clear(); 95 | { 96 | int idx = 0; 97 | for (const auto &v : surfel_planes_) { 98 | colorPointCloudT cloud_rgb; 99 | pcl::copyPointCloud(v.cloud_inlier, cloud_rgb); 100 | 101 | size_t colorType = (idx++) % color_list_.size(); 102 | for (auto & p : cloud_rgb) { 103 | p.rgba = color_list_[colorType]; 104 | } 105 | surfels_map_ += cloud_rgb; 106 | } 107 | } 108 | } 109 | 110 | 111 | void SurfelAssociation::getAssociation(const VPointCloud::Ptr& scan_inM, 112 | const TPointCloud::Ptr& scan_raw, 113 | size_t selected_num_per_ring) { 114 | const size_t width = scan_raw->width; 115 | const size_t height = scan_raw->height; 116 | 117 | int associatedFlag[width * height]; 118 | for (unsigned int i = 0; i < width * height; i++) { 119 | associatedFlag[i] = -1; 120 | } 121 | 122 | #pragma omp parallel for num_threads(omp_get_max_threads()) 123 | for (int plane_id = 0; plane_id < surfel_planes_.size(); plane_id++) { 124 | std::vector> ring_masks; 125 | associateScanToSurfel(plane_id, scan_inM, associated_radius_, ring_masks); 126 | 127 | for (int h = 0; h < height; h++) { 128 | if (ring_masks.at(h).size() < selected_num_per_ring*2) 129 | continue; 130 | int step = ring_masks.at(h).size() / (selected_num_per_ring + 1); 131 | step = std::max(step, 1); 132 | for (int selected = 0; selected < selected_num_per_ring; selected++) { 133 | int w = ring_masks.at(h).at(step * (selected+1) - 1); 134 | associatedFlag[h*width + w] = plane_id; 135 | } 136 | } 137 | } 138 | 139 | // in chronological order 140 | SurfelPoint spoint; 141 | for (int w = 0; w < width; w++) { 142 | for (int h = 0; h < height; h++) { 143 | if (associatedFlag[ h * width + w] == -1 144 | || 0 == scan_raw->at(w,h).timestamp) 145 | continue; 146 | spoint.point = Eigen::Vector3d(scan_raw->at(w,h).x, 147 | scan_raw->at(w,h).y, 148 | scan_raw->at(w,h).z); 149 | spoint.point_in_map = Eigen::Vector3d(scan_inM->at(w,h).x, 150 | scan_inM->at(w,h).y, 151 | scan_inM->at(w,h).z); 152 | spoint.plane_id = associatedFlag[h*width+w]; 153 | spoint.timestamp = scan_raw->at(w,h).timestamp; 154 | 155 | spoint_per_surfel_.at(spoint.plane_id).push_back(spoint); 156 | spoints_all_.push_back(spoint); 157 | } 158 | } 159 | } 160 | 161 | 162 | void SurfelAssociation::randomDownSample(int num_points_max) { 163 | for (const auto & v : spoint_per_surfel_) { 164 | if (v.size() < 20) 165 | continue; 166 | for(int i = 0; i < num_points_max; i++) { 167 | int random_index = rand() / (RAND_MAX) * v.size(); 168 | spoint_downsampled_.push_back(v.at(random_index)); 169 | } 170 | } 171 | } 172 | 173 | 174 | void SurfelAssociation::averageDownSmaple(int num_points_max) { 175 | for (const auto & v : spoint_per_surfel_) { 176 | if (v.size() < 20) 177 | continue; 178 | int d_step = v.size() / num_points_max; 179 | int step = d_step > 1 ? d_step : 1; 180 | for (int i = 0; i < v.size(); i+=step) { 181 | spoint_downsampled_.push_back(v.at(i)); 182 | } 183 | } 184 | } 185 | void SurfelAssociation::averageTimeDownSmaple(int step) { 186 | for (size_t idx = 0; idx < spoints_all_.size(); idx+= step) { 187 | spoint_downsampled_.push_back(spoints_all_.at(idx)); 188 | } 189 | } 190 | 191 | int SurfelAssociation::checkPlaneType(const Eigen::Vector3d& eigen_value, 192 | const Eigen::Matrix3d& eigen_vector, 193 | const double& p_lambda) { 194 | Eigen::Vector3d sorted_vec; 195 | Eigen::Vector3i ind; 196 | Eigen::sort_vec(eigen_value, sorted_vec, ind); 197 | 198 | double p = 2*(sorted_vec[1] - sorted_vec[2]) / 199 | (sorted_vec[2] + sorted_vec[1] + sorted_vec[0]); 200 | 201 | if (p < p_lambda) { 202 | return -1; 203 | } 204 | 205 | int min_idx = ind[2]; 206 | Eigen::Vector3d plane_normal = eigen_vector.block<3,1>(0, min_idx); 207 | plane_normal = plane_normal.array().abs(); 208 | 209 | Eigen::sort_vec(plane_normal, sorted_vec, ind); 210 | return ind[2]; 211 | } 212 | 213 | bool SurfelAssociation::fitPlane(const VPointCloud::Ptr& cloud, 214 | Eigen::Vector4d &coeffs, 215 | VPointCloud::Ptr cloud_inliers) { 216 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 217 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 218 | pcl::SACSegmentation seg; /// Create the segmentation object 219 | // Optional 220 | seg.setOptimizeCoefficients (true); 221 | // Mandatory 222 | seg.setModelType (pcl::SACMODEL_PLANE); 223 | seg.setMethodType (pcl::SAC_RANSAC); 224 | seg.setDistanceThreshold (0.05); 225 | 226 | seg.setInputCloud (cloud); 227 | seg.segment (*inliers, *coefficients); 228 | 229 | if (inliers->indices.size () < 20) { 230 | return false; 231 | } 232 | 233 | for(int i = 0; i < 4; i++) { 234 | coeffs(i) = coefficients->values[i]; 235 | } 236 | 237 | pcl::copyPointCloud (*cloud, *inliers, *cloud_inliers); 238 | return true; 239 | } 240 | 241 | double SurfelAssociation::point2PlaneDistance(Eigen::Vector3d &pt, 242 | Eigen::Vector4d &plane_coeff) { 243 | Eigen::Vector3d normal = plane_coeff.head<3>(); 244 | double dist = pt.dot(normal) + plane_coeff(3); 245 | dist = dist > 0 ? dist : -dist; 246 | 247 | return dist; 248 | } 249 | 250 | void SurfelAssociation::associateScanToSurfel( 251 | const size_t& surfel_idx, 252 | const VPointCloud::Ptr& scan, 253 | const double& radius, 254 | std::vector> &ring_masks) const { 255 | 256 | Eigen::Vector3d box_min = surfel_planes_.at(surfel_idx).boxMin; 257 | Eigen::Vector3d box_max = surfel_planes_.at(surfel_idx).boxMax; 258 | Eigen::Vector4d plane_coeffs = surfel_planes_.at(surfel_idx).p4; 259 | 260 | for (int j = 0 ; j < scan->height; j++) { 261 | std::vector mask_per_ring; 262 | for (int i=0; i< scan->width; i++) { 263 | if (!pcl_isnan(scan->at(i,j).x) && 264 | scan->at(i,j).x > box_min[0] && scan->at(i,j).x < box_max[0] && 265 | scan->at(i,j).y > box_min[1] && scan->at(i,j).y < box_max[1] && 266 | scan->at(i,j).z > box_min[2] && scan->at(i,j).z < box_max[2]) { 267 | 268 | Eigen::Vector3d point(scan->at(i,j).x, scan->at(i,j).y, scan->at(i,j).z); 269 | if (point2PlaneDistance(point, plane_coeffs) <= radius) { 270 | mask_per_ring.push_back(i); 271 | } 272 | } 273 | } // end of one colmun (ring) 274 | ring_masks.push_back(mask_per_ring); 275 | } 276 | } 277 | 278 | } 279 | -------------------------------------------------------------------------------- /include/utils/math_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of LIO-mapping. 3 | * 4 | * Copyright (C) 2019 Haoyang Ye , 5 | * Robotics and Multiperception Lab (RAM-LAB ), 6 | * The Hong Kong University of Science and Technology 7 | * 8 | * For more information please see 9 | * or . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * LIO-mapping is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * LIO-mapping is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with LIO-mapping. If not, see . 25 | */ 26 | 27 | // 28 | // Created by hyye on 3/16/18. 29 | // 30 | 31 | #ifndef LIO_MATH_UTILS_H_ 32 | #define LIO_MATH_UTILS_H_ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace mathutils { 42 | 43 | template 44 | inline T RadToDeg(T rad) { 45 | return rad * 180.0 / M_PI; 46 | } 47 | 48 | template 49 | inline T NormalizeRad(T rad) { 50 | rad = fmod(rad + M_PI, 2 * M_PI); 51 | if (rad < 0) { 52 | rad += 2 * M_PI; 53 | } 54 | return rad - M_PI; 55 | } 56 | 57 | template 58 | inline T DegToRad(T deg) { 59 | return deg / 180.0 * M_PI; 60 | } 61 | 62 | template 63 | inline T NormalizeDeg(T deg) { 64 | deg = fmod(deg + 180.0, 360.0); 65 | if (deg < 0) { 66 | deg += 360.0; 67 | } 68 | return deg - 180.0; 69 | } 70 | 71 | inline bool RadLt(double a, double b) { 72 | return NormalizeRad(a - b) < 0; 73 | } 74 | 75 | inline bool RadGt(double a, double b) { 76 | return NormalizeRad(a - b) > 0; 77 | } 78 | 79 | template 80 | inline PointT ScalePoint(const PointT &p, float scale) { 81 | PointT p_o = p; 82 | p_o.x *= scale; 83 | p_o.y *= scale; 84 | p_o.z *= scale; 85 | return p_o; 86 | } 87 | 88 | template 89 | inline float CalcSquaredDiff(const PointT &a, const PointT &b) { 90 | float diff_x = a.x - b.x; 91 | float diff_y = a.y - b.y; 92 | float diff_z = a.z - b.z; 93 | 94 | return diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; 95 | } 96 | 97 | template 98 | inline float CalcSquaredDiff(const PointT &a, const PointT &b, const float &wb) { 99 | float diff_x = a.x - b.x * wb; 100 | float diff_y = a.y - b.y * wb; 101 | float diff_z = a.z - b.z * wb; 102 | 103 | return diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; 104 | } 105 | 106 | template 107 | inline float CalcPointDistance(const PointT &p) { 108 | return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); 109 | } 110 | 111 | template 112 | inline float CalcSquaredPointDistance(const PointT &p) { 113 | return p.x * p.x + p.y * p.y + p.z * p.z; 114 | } 115 | 116 | ///< Matrix calculations 117 | 118 | static const Eigen::Matrix3d I3x3 = Eigen::Matrix3d::Identity(); 119 | 120 | template 121 | inline Eigen::Quaternion DeltaQ(const Eigen::MatrixBase &theta) { 122 | typedef typename Derived::Scalar Scalar_t; 123 | 124 | Eigen::Quaternion dq; 125 | Eigen::Matrix half_theta = theta; 126 | half_theta /= static_cast(2.0); 127 | dq.w() = static_cast(1.0); 128 | dq.x() = half_theta.x(); 129 | dq.y() = half_theta.y(); 130 | dq.z() = half_theta.z(); 131 | return dq; 132 | } 133 | 134 | template 135 | inline Eigen::Matrix SkewSymmetric(const Eigen::MatrixBase &v3d) { 136 | Eigen::Matrix m; 137 | m << typename Derived::Scalar(0), -v3d.z(), v3d.y(), 138 | v3d.z(), typename Derived::Scalar(0), -v3d.x(), 139 | -v3d.y(), v3d.x(), typename Derived::Scalar(0); 140 | return m; 141 | } 142 | 143 | template 144 | inline Eigen::Matrix LeftQuatMatrix(const Eigen::QuaternionBase &q) { 145 | Eigen::Matrix m; 146 | Eigen::Matrix vq = q.vec(); 147 | typename Derived::Scalar q4 = q.w(); 148 | m.block(0, 0, 3, 3) << q4 * I3x3 + SkewSymmetric(vq); 149 | m.block(3, 0, 1, 3) << -vq.transpose(); 150 | m.block(0, 3, 3, 1) << vq; 151 | m(3, 3) = q4; 152 | return m; 153 | } 154 | 155 | template 156 | inline Eigen::Matrix RightQuatMatrix(const Eigen::QuaternionBase &p) { 157 | Eigen::Matrix m; 158 | Eigen::Matrix vp = p.vec(); 159 | typename Derived::Scalar p4 = p.w(); 160 | m.block(0, 0, 3, 3) << p4 * I3x3 - SkewSymmetric(vp); 161 | m.block(3, 0, 1, 3) << -vp.transpose(); 162 | m.block(0, 3, 3, 1) << vp; 163 | m(3, 3) = p4; 164 | return m; 165 | } 166 | 167 | template 168 | inline Eigen::Matrix LeftQuatMatrix(const Eigen::Matrix &q) { 169 | Eigen::Matrix m; 170 | Eigen::Matrix vq{q.x(), q.y(), q.z()}; 171 | T q4 = q.w(); 172 | m.block(0, 0, 3, 3) << q4 * I3x3 + SkewSymmetric(vq); 173 | m.block(3, 0, 1, 3) << -vq.transpose(); 174 | m.block(0, 3, 3, 1) << vq; 175 | m(3, 3) = q4; 176 | return m; 177 | } 178 | 179 | template 180 | inline Eigen::Matrix RightQuatMatrix(const Eigen::Matrix &p) { 181 | Eigen::Matrix m; 182 | Eigen::Matrix vp{p.x(), p.y(), p.z()}; 183 | T p4 = p.w(); 184 | m.block(0, 0, 3, 3) << p4 * I3x3 - SkewSymmetric(vp); 185 | m.block(3, 0, 1, 3) << -vp.transpose(); 186 | m.block(0, 3, 3, 1) << vp; 187 | m(3, 3) = p4; 188 | return m; 189 | } 190 | 191 | // adapted from VINS-mono 192 | inline Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 193 | { 194 | Eigen::Vector3d n = R.col(0); 195 | Eigen::Vector3d o = R.col(1); 196 | Eigen::Vector3d a = R.col(2); 197 | 198 | Eigen::Vector3d ypr(3); 199 | double y = atan2(n(1), n(0)); 200 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 201 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 202 | ypr(0) = y; 203 | ypr(1) = p; 204 | ypr(2) = r; 205 | 206 | return ypr / M_PI * 180.0; 207 | } 208 | 209 | template 210 | inline Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 211 | { 212 | typedef typename Derived::Scalar Scalar_t; 213 | 214 | Scalar_t y = ypr(0) / 180.0 * M_PI; 215 | Scalar_t p = ypr(1) / 180.0 * M_PI; 216 | Scalar_t r = ypr(2) / 180.0 * M_PI; 217 | 218 | Eigen::Matrix Rz; 219 | Rz << cos(y), -sin(y), 0, 220 | sin(y), cos(y), 0, 221 | 0, 0, 1; 222 | 223 | Eigen::Matrix Ry; 224 | Ry << cos(p), 0., sin(p), 225 | 0., 1., 0., 226 | -sin(p), 0., cos(p); 227 | 228 | Eigen::Matrix Rx; 229 | Rx << 1., 0., 0., 230 | 0., cos(r), -sin(r), 231 | 0., sin(r), cos(r); 232 | 233 | return Rz * Ry * Rx; 234 | } 235 | 236 | inline Eigen::Quaterniond g2R(Eigen::Vector3d gravity) { 237 | 238 | // Get z axis, which alines with -g (z_in_G=0,0,1) 239 | Eigen::Vector3d z_axis = -gravity/gravity.norm(); 240 | 241 | // Create an x_axis 242 | Eigen::Vector3d e_1(1,0,0); 243 | 244 | // Make x_axis perpendicular to z 245 | Eigen::Vector3d x_axis = e_1-z_axis*z_axis.transpose()*e_1; 246 | x_axis= x_axis/x_axis.norm(); 247 | 248 | // Get z from the cross product of these two 249 | Eigen::Matrix y_axis = SkewSymmetric(z_axis)*x_axis; 250 | 251 | // From these axes get rotation 252 | Eigen::Matrix Ro; 253 | Ro.block(0,0,3,1) = x_axis; 254 | Ro.block(0,1,3,1) = y_axis; 255 | Ro.block(0,2,3,1) = z_axis; 256 | 257 | Eigen::Quaterniond q0(Ro); 258 | q0.normalize(); 259 | return q0; 260 | } 261 | 262 | 263 | inline Eigen::Vector3f CrossProduct(Eigen::Vector3f a, Eigen::Vector3f b) 264 | { 265 | Eigen::Vector3f c; 266 | 267 | c[0] = a[1] * b[2] - a[2] * b[1]; 268 | c[1] = a[2] * b[0] - a[0] * b[2]; 269 | c[2] = a[0] * b[1] - a[1] * b[0]; 270 | 271 | return c; 272 | } 273 | 274 | inline float DotProduct(Eigen::Vector3f a, Eigen::Vector3f b) 275 | { 276 | float result; 277 | result = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; 278 | return result; 279 | } 280 | 281 | inline float Normalize(Eigen::Vector3f v) 282 | { 283 | float result; 284 | result = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); 285 | return result; 286 | } 287 | 288 | inline Eigen::Matrix3f RotationMatrix(float angle, Eigen::Vector3f u) 289 | { 290 | float norm = Normalize(u); 291 | Eigen::Matrix3f rotatinMatrix; 292 | 293 | u(0) = u(0) / norm; 294 | u(1) = u(1) / norm; 295 | u(2) = u(2) / norm; 296 | 297 | rotatinMatrix(0, 0) = cos(angle) + u(0) * u(0) * (1 - cos(angle)); 298 | rotatinMatrix(0, 1) = u(0) * u(1) * (1 - cos(angle)) - u(2) * sin(angle); 299 | rotatinMatrix(0, 2) = u(1) * sin(angle) + u(0) * u(2) * (1 - cos(angle)); 300 | 301 | rotatinMatrix(1, 0) = u(2) * sin(angle) + u(0) * u(1) * (1 - cos(angle)); 302 | rotatinMatrix(1, 1) = cos(angle) + u(1) * u(1) * (1 - cos(angle)); 303 | rotatinMatrix(1, 2) = -u(0) * sin(angle) + u(1) * u(2) * (1 - cos(angle)); 304 | 305 | rotatinMatrix(2, 0) = -u(1) * sin(angle) + u(0) * u(2) * (1 - cos(angle)); 306 | rotatinMatrix(2, 1) = u(0) * sin(angle) + u(1) * u(2) * (1 - cos(angle)); 307 | rotatinMatrix(2, 2) = cos(angle) + u(2) * u(2) * (1 - cos(angle)); 308 | 309 | return rotatinMatrix; 310 | } 311 | 312 | inline Eigen::Matrix3f Calculation(Eigen::Vector3f vectorBefore, Eigen::Vector3f vectorAfter) 313 | { 314 | Eigen::Vector3f rotationAxis; 315 | float rotationAngle; 316 | Eigen::Matrix3f rotationMatrix; 317 | rotationAxis = CrossProduct(vectorBefore, vectorAfter); 318 | rotationAngle = acos(DotProduct(vectorBefore, vectorAfter) / Normalize(vectorBefore) / Normalize(vectorAfter)); 319 | rotationMatrix = RotationMatrix(rotationAngle, rotationAxis); 320 | return rotationMatrix; 321 | } 322 | 323 | 324 | 325 | } // namespance mathutils 326 | 327 | #endif //LIO_MATH_UTILS_H_ 328 | -------------------------------------------------------------------------------- /src/core/trajectory_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace licalib { 29 | using namespace kontiki::trajectories; 30 | 31 | void TrajectoryManager::initialTrajTo(double max_time) { 32 | Eigen::Quaterniond q0 = Eigen::Quaterniond::Identity(); 33 | Eigen::Vector3d p0(0,0,0); 34 | traj_->R3Spline()->ExtendTo (max_time, p0); 35 | traj_->SO3Spline()->ExtendTo(max_time, q0); 36 | } 37 | 38 | void TrajectoryManager::feedIMUData(const IO::IMUData& data) { 39 | imu_data_.emplace_back(data); 40 | } 41 | 42 | void TrajectoryManager::initialSO3TrajWithGyro() { 43 | assert(imu_data_.size() > 0 && 44 | "[initialSO3TrajWithGyro]: There's NO imu data for initialization."); 45 | std::shared_ptr estimator_SO3; 46 | estimator_SO3 = std::make_shared(traj_->SO3Spline()); 47 | 48 | addGyroscopeMeasurements(estimator_SO3); 49 | 50 | /// fix the initial pose of trajectory 51 | double weight_t0 = calib_param_manager->global_opt_gyro_weight; 52 | double t0 = traj_->SO3Spline()->MinTime(); 53 | //Eigen::Quaterniond q0 = Eigen::Quaterniond::Identity(); 54 | Eigen::AngleAxisd rotation_vector(0.0001, Eigen::Vector3d(0,0,1)); 55 | Eigen::Quaterniond q0 = Eigen::Quaterniond (rotation_vector.matrix()); 56 | auto m_q0 = std::make_shared(t0, q0, weight_t0); 57 | estimator_SO3->AddMeasurement(m_q0); 58 | 59 | ceres::Solver::Summary summary = estimator_SO3->Solve(30, false); 60 | std::cout << summary.BriefReport() << std::endl; 61 | } 62 | 63 | void TrajectoryManager::trajInitFromSurfel( 64 | SurfelAssociation::Ptr surfels_association, 65 | bool opt_time_offset_) { 66 | lidar_->set_relative_orientation(calib_param_manager->q_LtoI); 67 | lidar_->set_relative_position(calib_param_manager->p_LinI); 68 | lidar_->LockRelativeOrientation(false); 69 | lidar_->LockRelativePosition(false); 70 | if (opt_time_offset_ && time_offset_padding_ > 0) { 71 | lidar_->LockTimeOffset(false); 72 | lidar_->set_max_time_offset(time_offset_padding_); 73 | } 74 | else { 75 | lidar_->LockTimeOffset(true); 76 | } 77 | imu_->LockGyroscopeBias(false); 78 | imu_->LockAccelerometerBias(false); 79 | 80 | std::shared_ptr estimator_split; 81 | estimator_split = std::make_shared(traj_); 82 | 83 | // add constraints 84 | addGyroscopeMeasurements(estimator_split); 85 | addAccelerometerMeasurement(estimator_split); 86 | addSurfMeasurement(estimator_split, surfels_association); 87 | 88 | // addCallback(estimator_split); 89 | 90 | //printErrorStatistics("Before optimization"); 91 | ceres::Solver::Summary summary = estimator_split->Solve(30, false); 92 | std::cout << summary.BriefReport() << std::endl; 93 | printErrorStatistics("After optimization"); 94 | 95 | calib_param_manager->set_p_LinI(lidar_->relative_position()); 96 | calib_param_manager->set_q_LtoI(lidar_->relative_orientation()); 97 | calib_param_manager->set_time_offset(lidar_->time_offset()); 98 | calib_param_manager->set_gravity(imu_->refined_gravity()); 99 | calib_param_manager->set_gyro_bias(imu_->gyroscope_bias()); 100 | calib_param_manager->set_acce_bias(imu_->accelerometer_bias()); 101 | calib_param_manager->showStates(); 102 | } 103 | 104 | bool TrajectoryManager::evaluateIMUPose(double imu_time, int flags, 105 | Result &result) const { 106 | if (traj_->MinTime() > imu_time || traj_->MaxTime() <= imu_time) 107 | return false; 108 | result = traj_->Evaluate(imu_time, flags); 109 | return true; 110 | } 111 | 112 | bool TrajectoryManager::evaluateLidarPose(double lidar_time, 113 | Eigen::Quaterniond &q_LtoG, 114 | Eigen::Vector3d &p_LinG) const { 115 | double traj_time = lidar_time + lidar_->time_offset(); 116 | if (traj_->MinTime() > traj_time || traj_->MaxTime() <= traj_time) 117 | return false; 118 | Result result = traj_->Evaluate( traj_time, EvalOrientation | EvalPosition); 119 | q_LtoG = result->orientation * calib_param_manager->q_LtoI; 120 | p_LinG = result->orientation * calib_param_manager->p_LinI + result->position; 121 | return true; 122 | } 123 | 124 | bool TrajectoryManager::evaluateLidarRelativeRotation(double lidar_time1, 125 | double lidar_time2, Eigen::Quaterniond &q_L2toL1) const { 126 | assert(lidar_time1 <= lidar_time2 127 | && "[evaluateRelativeRotation] : lidar_time1 > lidar_time2"); 128 | 129 | double traj_time1 = lidar_time1 + lidar_->time_offset(); 130 | double traj_time2 = lidar_time2 + lidar_->time_offset(); 131 | 132 | if (traj_->MinTime() > traj_time1 || traj_->MaxTime() <= traj_time2) 133 | return false; 134 | 135 | Result result1 = traj_->Evaluate(traj_time1, EvalOrientation); 136 | Result result2 = traj_->Evaluate(traj_time2, EvalOrientation); 137 | Eigen::Quaterniond q_I2toI1 = result1->orientation.conjugate()*result2->orientation; 138 | 139 | q_L2toL1 = calib_param_manager->q_LtoI.conjugate() * q_I2toI1 * calib_param_manager->q_LtoI; 140 | return true; 141 | } 142 | 143 | template 144 | void TrajectoryManager::addGyroscopeMeasurements( 145 | std::shared_ptr> estimator) { 146 | gyro_list_.clear(); 147 | 148 | double weight = calib_param_manager->global_opt_gyro_weight; 149 | const double min_time = estimator->trajectory()->MinTime(); 150 | const double max_time = estimator->trajectory()->MaxTime(); 151 | 152 | for (const auto &v : imu_data_) { 153 | if ( min_time > v.timestamp || max_time <= v.timestamp) { 154 | continue; 155 | } 156 | auto mg = std::make_shared(imu_, v.timestamp, v.gyro, weight); 157 | gyro_list_.push_back(mg); 158 | estimator->template AddMeasurement(mg); 159 | } 160 | } 161 | 162 | template 163 | void TrajectoryManager::addAccelerometerMeasurement( 164 | std::shared_ptr> estimator) { 165 | accel_list_.clear(); 166 | 167 | const double weight = calib_param_manager->global_opt_acce_weight; 168 | const double min_time = estimator->trajectory()->MinTime(); 169 | const double max_time = estimator->trajectory()->MaxTime(); 170 | 171 | for (auto const &v : imu_data_) { 172 | if ( min_time > v.timestamp || max_time <= v.timestamp) { 173 | continue; 174 | } 175 | auto ma = std::make_shared(imu_, v.timestamp, v.accel, weight); 176 | accel_list_.push_back(ma); 177 | estimator->template AddMeasurement(ma); 178 | } 179 | } 180 | 181 | template 182 | void TrajectoryManager::addSurfMeasurement( 183 | std::shared_ptr> estimator, 184 | const SurfelAssociation::Ptr surfel_association) { 185 | const double weight = calib_param_manager->global_opt_lidar_weight; 186 | surfelpoint_list_.clear(); 187 | closest_point_vec_.clear(); 188 | for (auto const& v: surfel_association->get_surfel_planes()) { 189 | closest_point_vec_.push_back(v.Pi); 190 | } 191 | 192 | map_time_ = surfel_association->get_maptime(); 193 | for (auto const &spoint : surfel_association->get_surfel_points()) { 194 | double time = spoint.timestamp; 195 | size_t plane_id = spoint.plane_id; 196 | 197 | auto msp = std::make_shared (lidar_, spoint.point, 198 | closest_point_vec_.at(plane_id).data(), time, map_time_, 5.0, weight); 199 | surfelpoint_list_.push_back(msp); 200 | estimator->template AddMeasurement(msp); 201 | } 202 | } 203 | 204 | template 205 | void TrajectoryManager::addCallback( 206 | std::shared_ptr> estimator) { 207 | // Add callback for debug 208 | std::unique_ptr cb = std::make_unique(); 209 | cb->addCheckState("q_LtoI :", 4, lidar_->relative_orientation().coeffs().data()); 210 | cb->addCheckState("p_LinI :", 3, lidar_->relative_position().data()); 211 | cb->addCheckState("time_offset:", 1, &lidar_->time_offset()); 212 | cb->addCheckState("g_roll :", 1, &imu_->gravity_orientation_roll()); 213 | cb->addCheckState("g_pitch :", 1, &imu_->gravity_orientation_pitch()); 214 | estimator->AddCallback(std::move(cb), true); 215 | } 216 | 217 | void TrajectoryManager::printErrorStatistics(const std::string& intro, bool show_gyro, 218 | bool show_accel, bool show_lidar) const { 219 | std::cout << "\n============== " << intro << " ================" << std::endl; 220 | 221 | if (show_gyro && !gyro_list_.empty()) { 222 | Eigen::Vector3d error_sum; 223 | for(auto const& m : gyro_list_) { 224 | error_sum += m->ErrorRaw (*traj_).cwiseAbs(); 225 | } 226 | std::cout << "[Gyro] Error size, average: " << gyro_list_.size() 227 | << "; " << (error_sum/gyro_list_.size()).transpose() << std::endl; 228 | } 229 | 230 | if (show_accel && !accel_list_.empty()) { 231 | Eigen::Vector3d error_sum; 232 | for(auto const& m : accel_list_) { 233 | error_sum += m->ErrorRaw (*traj_).cwiseAbs(); 234 | } 235 | std::cout << "[Accel] Error size, average: " << accel_list_.size() 236 | << "; " << (error_sum/accel_list_.size()).transpose() << std::endl; 237 | } 238 | 239 | if (show_lidar && !surfelpoint_list_.empty()) { 240 | Eigen::Matrix error_sum; 241 | for (auto const &m : surfelpoint_list_) { 242 | error_sum += m->point2plane(*traj_).cwiseAbs(); 243 | } 244 | std::cout << "[LiDAR] Error size, average: " << surfelpoint_list_.size() 245 | << "; " << (error_sum/surfelpoint_list_.size()).transpose() << std::endl; 246 | } 247 | 248 | std::cout << std::endl; 249 | } 250 | 251 | } -------------------------------------------------------------------------------- /src/ui/calib_helper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace licalib { 30 | 31 | CalibrHelper::CalibrHelper(ros::NodeHandle& nh) 32 | : calib_step_(Start), 33 | iteration_step_(0), 34 | opt_time_offset_(false), 35 | plane_lambda_(0.6), 36 | ndt_resolution_(0.5), 37 | associated_radius_(0.05) { 38 | std::string topic_lidar; 39 | double bag_start, bag_durr; 40 | double scan4map; 41 | double knot_distance; 42 | double time_offset_padding; 43 | 44 | nh.param("path_bag", bag_path_, "V1_01_easy.bag"); 45 | nh.param("topic_imu", topic_imu_, "/imu0"); 46 | nh.param("topic_lidar", topic_lidar, "/velodyne_packets"); 47 | nh.param("bag_start", bag_start, 0); 48 | nh.param("bag_durr", bag_durr, -1); 49 | nh.param("scan4map", scan4map, 15); 50 | nh.param("ndtResolution", ndt_resolution_, 0.5); 51 | nh.param("time_offset_padding", time_offset_padding, 0.015); 52 | nh.param("knot_distance", knot_distance, 0.02); 53 | 54 | if (!createCacheFolder(bag_path_)) { 55 | calib_step_ = Error; 56 | } 57 | 58 | { 59 | std::string lidar_model; 60 | nh.param("LidarModel", lidar_model, "VLP_16"); 61 | IO::LidarModelType lidar_model_type = IO::LidarModelType::VLP_16; 62 | if (lidar_model == "VLP_16") { 63 | lidar_model_type = IO::LidarModelType::VLP_16; 64 | } else { 65 | calib_step_ = Error; 66 | ROS_WARN("LiDAR model %s not support yet.", lidar_model.c_str()); 67 | } 68 | /// read dataset 69 | std::cout << "\nLoad dataset from " << bag_path_ << std::endl; 70 | IO::LioDataset lio_dataset_temp(lidar_model_type); 71 | lio_dataset_temp.read(bag_path_, topic_imu_, topic_lidar, bag_start, bag_durr); 72 | dataset_reader_ = lio_dataset_temp.get_data(); 73 | dataset_reader_->adjustDataset(); 74 | } 75 | 76 | map_time_ = dataset_reader_->get_start_time(); 77 | scan4map_time_ = map_time_ + scan4map; 78 | double end_time = dataset_reader_->get_end_time(); 79 | 80 | traj_manager_ = std::make_shared( 81 | map_time_, end_time, knot_distance, time_offset_padding); 82 | 83 | scan_undistortion_ = std::make_shared( 84 | traj_manager_, dataset_reader_); 85 | 86 | lidar_odom_ = std::make_shared(ndt_resolution_); 87 | 88 | rotation_initializer_ = std::make_shared(); 89 | 90 | surfel_association_ = std::make_shared( 91 | associated_radius_, plane_lambda_); 92 | } 93 | 94 | bool CalibrHelper::createCacheFolder(const std::string& bag_path) { 95 | boost::filesystem::path p(bag_path); 96 | if (p.extension() != ".bag") { 97 | return false; 98 | } 99 | cache_path_ = p.parent_path().string() + "/" + p.stem().string(); 100 | boost::filesystem::create_directory(cache_path_); 101 | return true; 102 | } 103 | 104 | void CalibrHelper::Initialization() { 105 | if (Start != calib_step_) { 106 | ROS_WARN("[Initialization] Need status: Start."); 107 | return; 108 | } 109 | for (const auto& imu_data: dataset_reader_->get_imu_data()) { 110 | traj_manager_->feedIMUData(imu_data); 111 | } 112 | traj_manager_->initialSO3TrajWithGyro(); 113 | 114 | for(const TPointCloud& raw_scan: dataset_reader_->get_scan_data()) { 115 | VPointCloud::Ptr cloud(new VPointCloud); 116 | TPointCloud2VPointCloud(raw_scan.makeShared(), cloud); 117 | double scan_timestamp = pcl_conversions::fromPCL(raw_scan.header.stamp).toSec(); 118 | 119 | lidar_odom_->feedScan(scan_timestamp, cloud); 120 | 121 | if (lidar_odom_->get_odom_data().size() < 30 122 | || (lidar_odom_->get_odom_data().size() % 10 != 0)) 123 | continue; 124 | if (rotation_initializer_->EstimateRotation(traj_manager_, 125 | lidar_odom_->get_odom_data())) { 126 | Eigen::Quaterniond qItoLidar = rotation_initializer_->getQ_ItoS(); 127 | traj_manager_->getCalibParamManager()->set_q_LtoI(qItoLidar.conjugate()); 128 | 129 | Eigen::Vector3d euler_ItoL = qItoLidar.toRotationMatrix().eulerAngles(0,1,2); 130 | std::cout << "[Initialization] Done. Euler_ItoL initial degree: " 131 | << (euler_ItoL*180.0/M_PI).transpose() << std::endl; 132 | calib_step_ = InitializationDone; 133 | break; 134 | } 135 | } 136 | if (calib_step_ != InitializationDone) 137 | ROS_WARN("[Initialization] fails."); 138 | } 139 | 140 | void CalibrHelper::DataAssociation() { 141 | std::cout << "[Association] start ...." << std::endl; 142 | TicToc timer; 143 | timer.tic(); 144 | 145 | /// set surfel pap 146 | if (InitializationDone == calib_step_ ) { 147 | Mapping(); 148 | scan_undistortion_->undistortScanInMap(lidar_odom_->get_odom_data()); 149 | 150 | surfel_association_->setSurfelMap(lidar_odom_->getNDTPtr(), map_time_); 151 | } else if (BatchOptimizationDone == calib_step_ || RefineDone == calib_step_) { 152 | scan_undistortion_->undistortScanInMap(); 153 | 154 | plane_lambda_ = 0.7; 155 | surfel_association_->setPlaneLambda(plane_lambda_); 156 | auto ndt_omp = LiDAROdometry::ndtInit(ndt_resolution_); 157 | ndt_omp->setInputTarget(scan_undistortion_->get_map_cloud()); 158 | surfel_association_->setSurfelMap(ndt_omp, map_time_); 159 | } else { 160 | ROS_WARN("[DataAssociation] Please follow the step."); 161 | return; 162 | } 163 | 164 | /// get association 165 | for (auto const &scan_raw : dataset_reader_->get_scan_data()) { 166 | auto iter = scan_undistortion_->get_scan_data_in_map().find( 167 | scan_raw.header.stamp); 168 | if (iter == scan_undistortion_->get_scan_data_in_map().end()) { 169 | continue; 170 | } 171 | surfel_association_->getAssociation(iter->second, scan_raw.makeShared(), 2); 172 | } 173 | surfel_association_->averageTimeDownSmaple(); 174 | std::cout << "Surfel point number: " 175 | << surfel_association_->get_surfel_points().size() << std::endl; 176 | std::cout<get_surfel_points().size() > 10){ 179 | calib_step_ = DataAssociationDone; 180 | } else { 181 | ROS_WARN("[DataAssociation] fails."); 182 | } 183 | } 184 | 185 | void CalibrHelper::BatchOptimization() { 186 | if (DataAssociationDone != calib_step_) { 187 | ROS_WARN("[BatchOptimization] Need status: DataAssociationDone."); 188 | return; 189 | } 190 | std::cout << "\n================ Iteration " << iteration_step_ << " ==================\n"; 191 | 192 | TicToc timer; 193 | timer.tic(); 194 | traj_manager_->trajInitFromSurfel(surfel_association_, opt_time_offset_); 195 | 196 | calib_step_ = BatchOptimizationDone; 197 | saveCalibResult(cache_path_ + "/calib_result.csv"); 198 | std::cout< calib_step_) { 203 | ROS_WARN("[Refinement] Need status: BatchOptimizationDone."); 204 | return; 205 | } 206 | iteration_step_++; 207 | std::cout << "\n================ Iteration " << iteration_step_ << " ==================\n"; 208 | 209 | DataAssociation(); 210 | if (DataAssociationDone != calib_step_) { 211 | ROS_WARN("[Refinement] Need status: DataAssociationDone."); 212 | return; 213 | } 214 | TicToc timer; 215 | timer.tic(); 216 | 217 | traj_manager_->trajInitFromSurfel(surfel_association_, opt_time_offset_); 218 | calib_step_ = RefineDone; 219 | saveCalibResult(cache_path_ + "/calib_result.csv"); 220 | 221 | std::cout<clearOdomData(); 228 | update_map = false; 229 | } else { 230 | scan_undistortion_->undistortScan(); 231 | lidar_odom_ = std::make_shared(ndt_resolution_); 232 | } 233 | 234 | double last_scan_t = 0; 235 | for (const auto& scan_raw: dataset_reader_->get_scan_data()) { 236 | double scan_t = pcl_conversions::fromPCL(scan_raw.header.stamp).toSec(); 237 | if (scan_t > scan4map_time_) 238 | update_map = false; 239 | auto iter = scan_undistortion_->get_scan_data().find(scan_raw.header.stamp); 240 | if (iter != scan_undistortion_->get_scan_data().end()) { 241 | Eigen::Matrix4d pose_predict = Eigen::Matrix4d::Identity(); 242 | Eigen::Quaterniond q_L2toL1 = Eigen::Quaterniond::Identity(); 243 | if (last_scan_t > 0 && 244 | traj_manager_->evaluateLidarRelativeRotation(last_scan_t, scan_t, q_L2toL1)) { 245 | pose_predict.block<3,3>(0,0) = q_L2toL1.toRotationMatrix(); 246 | } 247 | lidar_odom_->feedScan(scan_t, iter->second, pose_predict, update_map); 248 | last_scan_t = scan_t; 249 | } 250 | } 251 | } 252 | 253 | 254 | void CalibrHelper::saveCalibResult(const std::string& calib_result_file) const { 255 | if (!boost::filesystem::exists(calib_result_file)) { 256 | std::ofstream outfile; 257 | outfile.open(calib_result_file, std::ios::app); 258 | outfile << "bag_path" << "," 259 | << "imu_topic" << "," << "map_time" << "," << "iteration_step" << "," 260 | << "p_IinL.x" << "," << "p_IinL.y" << "," << "p_IinL.z" << "," 261 | << "q_ItoL.x" << "," << "q_ItoL.y" << "," << "q_ItoL" << "," 262 | << "q_ItoL.w" << "," 263 | << "time_offset" << "," 264 | << "gravity.x" << "," << "gravity.y" << "," << "gravity.z" << "," 265 | << "gyro_bias.x" << "," << "gyro_bias.y" << "," <<"gyro_bias.z" << "," 266 | << "acce_bias.z" << "," << "acce_bias.y" << "," <<"acce_bias.z" << "\n"; 267 | outfile.close(); 268 | } 269 | 270 | std::stringstream ss; 271 | ss << bag_path_; 272 | ss << "," << topic_imu_; 273 | ss << "," << map_time_; 274 | ss << "," << iteration_step_; 275 | std::string info; 276 | ss >> info; 277 | 278 | traj_manager_->getCalibParamManager()->save_result(calib_result_file, info); 279 | } 280 | 281 | void CalibrHelper::saveMap() const { 282 | if (calib_step_ <= Start) 283 | return; 284 | std::string NDT_target_map_path = cache_path_ + "/NDT_target_map.pcd"; 285 | lidar_odom_->saveTargetMap(NDT_target_map_path); 286 | 287 | std::string surfel_map_path = cache_path_ + "/surfel_map.pcd"; 288 | surfel_association_->saveSurfelsMap(surfel_map_path); 289 | 290 | if (RefineDone == calib_step_) { 291 | std::string refined_map_path = cache_path_ + "/refined_map.pcd"; 292 | std::cout << "Save refined map to " << refined_map_path << "; size: " 293 | << scan_undistortion_->get_map_cloud()->size() << std::endl; 294 | pcl::io::savePCDFileASCII(refined_map_path, *scan_undistortion_->get_map_cloud()); 295 | } 296 | } 297 | 298 | } 299 | -------------------------------------------------------------------------------- /include/utils/vlp_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LI_Calib: An Open Platform for LiDAR-IMU Calibration 3 | * Copyright (C) 2020 Jiajun Lv 4 | * Copyright (C) 2020 Kewei Hu 5 | * Copyright (C) 2020 Jinhong Xu 6 | * Copyright (C) 2020 LI_Calib Contributors 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | #ifndef VELODYNE_CORRECTION_HPP 22 | #define VELODYNE_CORRECTION_HPP 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | 36 | namespace licalib { 37 | 38 | class VelodyneCorrection { 39 | public: 40 | typedef std::shared_ptr Ptr; 41 | 42 | enum ModelType { 43 | VLP_16, 44 | HDL_32E // not support yet 45 | }; 46 | 47 | VelodyneCorrection(ModelType modelType = VLP_16) : m_modelType(modelType) { 48 | setParameters(m_modelType); 49 | } 50 | 51 | void unpack_scan(const velodyne_msgs::VelodyneScan::ConstPtr &lidarMsg, 52 | TPointCloud &outPointCloud) const { 53 | outPointCloud.clear(); 54 | outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header); 55 | if (m_modelType == ModelType::VLP_16) { 56 | outPointCloud.height = 16; 57 | outPointCloud.width = 24*(int)lidarMsg->packets.size(); 58 | outPointCloud.is_dense = false; 59 | outPointCloud.resize(outPointCloud.height * outPointCloud.width); 60 | } 61 | 62 | int block_counter = 0; 63 | 64 | double scan_timestamp = lidarMsg->header.stamp.toSec(); 65 | 66 | for (size_t i = 0; i < lidarMsg->packets.size(); ++i) { 67 | float azimuth; 68 | float azimuth_diff; 69 | float last_azimuth_diff=0; 70 | float azimuth_corrected_f; 71 | int azimuth_corrected; 72 | float x, y, z; 73 | 74 | const raw_packet_t *raw = (const raw_packet_t *) &lidarMsg->packets[i].data[0]; 75 | 76 | for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_counter++) { 77 | // Calculate difference between current and next block's azimuth angle. 78 | azimuth = (float)(raw->blocks[block].rotation); 79 | 80 | if (block < (BLOCKS_PER_PACKET-1)){ 81 | azimuth_diff = (float)((36000 + raw->blocks[block+1].rotation - raw->blocks[block].rotation)%36000); 82 | last_azimuth_diff = azimuth_diff; 83 | } 84 | else { 85 | azimuth_diff = last_azimuth_diff; 86 | } 87 | 88 | for (int firing=0, k=0; firing < FIRINGS_PER_BLOCK; firing++) { 89 | for (int dsr=0; dsr < SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) { 90 | 91 | /** Position Calculation */ 92 | union two_bytes tmp; 93 | tmp.bytes[0] = raw->blocks[block].data[k]; 94 | tmp.bytes[1] = raw->blocks[block].data[k+1]; 95 | 96 | /** correct for the laser rotation as a function of timing during the firings **/ 97 | azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*DSR_TOFFSET) + (firing*FIRING_TOFFSET)) / BLOCK_TDURATION); 98 | azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000; 99 | 100 | /*condition added to avoid calculating points which are not 101 | in the interesting defined area (min_angle < area < max_angle)*/ 102 | if ((azimuth_corrected >= m_config.min_angle 103 | && azimuth_corrected <= m_config.max_angle 104 | && m_config.min_angle < m_config.max_angle) 105 | || (m_config.min_angle > m_config.max_angle 106 | && (azimuth_corrected <= m_config.max_angle 107 | || azimuth_corrected >= m_config.min_angle))) { 108 | // convert polar coordinates to Euclidean XYZ 109 | float distance = tmp.uint * DISTANCE_RESOLUTION; 110 | 111 | float cos_vert_angle = cos_vert_angle_[dsr]; 112 | float sin_vert_angle = sin_vert_angle_[dsr]; 113 | 114 | float cos_rot_angle = cos_rot_table_[azimuth_corrected]; 115 | float sin_rot_angle = sin_rot_table_[azimuth_corrected]; 116 | 117 | x = distance * cos_vert_angle * sin_rot_angle; 118 | y = distance * cos_vert_angle * cos_rot_angle; 119 | z = distance * sin_vert_angle; 120 | 121 | /** Use standard ROS coordinate system (right-hand rule) */ 122 | float x_coord = y; 123 | float y_coord = -x; 124 | float z_coord = z; 125 | 126 | float intensity = raw->blocks[block].data[k+2]; // 反射率 127 | double point_timestamp = scan_timestamp + getExactTime(scan_mapping_16[dsr], 2*block_counter+firing); 128 | 129 | TPoint point; 130 | point.timestamp = point_timestamp; 131 | if (pointInRange(distance)) { 132 | point.x = x_coord; 133 | point.y = y_coord; 134 | point.z = z_coord; 135 | point.intensity = intensity; 136 | } else { 137 | point.x = NAN; 138 | point.y = NAN; 139 | point.z = NAN; 140 | point.intensity = 0; 141 | } 142 | if(m_modelType == ModelType::VLP_16) 143 | outPointCloud.at(2*block_counter+firing, scan_mapping_16[dsr]) = point; 144 | } 145 | } 146 | } 147 | } 148 | } 149 | } 150 | 151 | 152 | void unpack_scan(const sensor_msgs::PointCloud2::ConstPtr &lidarMsg, 153 | TPointCloud &outPointCloud) const { 154 | VPointCloud temp_pc; 155 | pcl::fromROSMsg(*lidarMsg, temp_pc); 156 | 157 | outPointCloud.clear(); 158 | outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header); 159 | outPointCloud.height = temp_pc.height; 160 | outPointCloud.width = temp_pc.width; 161 | outPointCloud.is_dense = false; 162 | outPointCloud.resize(outPointCloud.height * outPointCloud.width); 163 | 164 | double timebase = lidarMsg->header.stamp.toSec(); 165 | for (int h = 0; h < temp_pc.height; h++) { 166 | for (int w = 0; w < temp_pc.width; w++) { 167 | TPoint point; 168 | point.x = temp_pc.at(w,h).x; 169 | point.y = temp_pc.at(w,h).y; 170 | point.z = temp_pc.at(w,h).z; 171 | point.intensity = temp_pc.at(w,h).intensity; 172 | point.timestamp = timebase + getExactTime(h,w); 173 | outPointCloud.at(w,h) = point; 174 | } 175 | } 176 | } 177 | 178 | 179 | inline double getExactTime(int dsr, int firing) const { 180 | return mVLP16TimeBlock[firing][dsr]; 181 | } 182 | 183 | private: 184 | void setParameters(ModelType modelType) { 185 | m_modelType = modelType; 186 | m_config.max_range = 150; 187 | m_config.min_range = 0.6; 188 | m_config.min_angle = 0; 189 | m_config.max_angle = 36000; 190 | // Set up cached values for sin and cos of all the possible headings 191 | for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { 192 | float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); 193 | cos_rot_table_[rot_index] = cosf(rotation); 194 | sin_rot_table_[rot_index] = sinf(rotation); 195 | } 196 | 197 | if (modelType == VLP_16) { 198 | FIRINGS_PER_BLOCK = 2; 199 | SCANS_PER_FIRING = 16; 200 | BLOCK_TDURATION = 110.592f; // [µs] 201 | DSR_TOFFSET = 2.304f; // [µs] 202 | FIRING_TOFFSET = 55.296f; // [µs] 203 | PACKET_TIME = (BLOCKS_PER_PACKET*2*FIRING_TOFFSET); 204 | 205 | float vert_correction[16] = { 206 | -0.2617993877991494, 207 | 0.017453292519943295, 208 | -0.22689280275926285, 209 | 0.05235987755982989, 210 | -0.19198621771937624, 211 | 0.08726646259971647, 212 | -0.15707963267948966, 213 | 0.12217304763960307, 214 | -0.12217304763960307, 215 | 0.15707963267948966, 216 | -0.08726646259971647, 217 | 0.19198621771937624, 218 | -0.05235987755982989, 219 | 0.22689280275926285, 220 | -0.017453292519943295, 221 | 0.2617993877991494 222 | }; 223 | for(int i = 0; i < 16; i++) { 224 | cos_vert_angle_[i] = std::cos(vert_correction[i]); 225 | sin_vert_angle_[i] = std::sin(vert_correction[i]); 226 | } 227 | scan_mapping_16[0]=15; 228 | scan_mapping_16[1]=7; 229 | scan_mapping_16[2]=14; 230 | scan_mapping_16[3]=6; 231 | scan_mapping_16[4]=13; 232 | scan_mapping_16[5]=5; 233 | scan_mapping_16[6]=12; 234 | scan_mapping_16[7]=4; 235 | scan_mapping_16[8]=11; 236 | scan_mapping_16[9]=3; 237 | scan_mapping_16[10]=10; 238 | scan_mapping_16[11]=2; 239 | scan_mapping_16[12]=9; 240 | scan_mapping_16[13]=1; 241 | scan_mapping_16[14]=8; 242 | scan_mapping_16[15]=0; 243 | 244 | for(unsigned int w = 0; w < 1824; w++) { 245 | for(unsigned int h = 0; h < 16; h++) { 246 | mVLP16TimeBlock[w][h] = h * 2.304 * 1e-6 + w * 55.296 * 1e-6; /// VLP_16 16*1824 247 | } 248 | } 249 | } 250 | } 251 | 252 | inline bool pointInRange(float range) const { 253 | return (range >= m_config.min_range 254 | && range <= m_config.max_range); 255 | } 256 | 257 | private: 258 | static const int RAW_SCAN_SIZE = 3; 259 | static const int SCANS_PER_BLOCK = 32; 260 | static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); 261 | constexpr static const float ROTATION_RESOLUTION = 0.01f; 262 | static const uint16_t ROTATION_MAX_UNITS = 36000u; 263 | constexpr static const float DISTANCE_RESOLUTION = 0.002f; 264 | 265 | /** @todo make this work for both big and little-endian machines */ 266 | static const uint16_t UPPER_BANK = 0xeeff; 267 | static const uint16_t LOWER_BANK = 0xddff; 268 | 269 | static const int BLOCKS_PER_PACKET = 12; 270 | static const int PACKET_STATUS_SIZE = 2; 271 | 272 | int FIRINGS_PER_BLOCK; 273 | int SCANS_PER_FIRING; 274 | float BLOCK_TDURATION; 275 | float DSR_TOFFSET; 276 | float FIRING_TOFFSET; 277 | float PACKET_TIME ; 278 | 279 | float sin_rot_table_[ROTATION_MAX_UNITS]; 280 | float cos_rot_table_[ROTATION_MAX_UNITS]; 281 | float cos_vert_angle_[32]; 282 | float sin_vert_angle_[32]; 283 | int scan_mapping_16[16]; 284 | int scan_mapping_32[32]; 285 | 286 | typedef struct raw_block { 287 | uint16_t header; ///< UPPER_BANK or LOWER_BANK 288 | uint16_t rotation; ///< 0-35999, divide by 100 to get degrees 289 | uint8_t data[BLOCK_DATA_SIZE]; 290 | } raw_block_t; 291 | 292 | union two_bytes { 293 | uint16_t uint; 294 | uint8_t bytes[2]; 295 | }; 296 | 297 | union four_bytes { 298 | uint32_t uint32; 299 | float_t float32; 300 | }; 301 | 302 | typedef struct raw_packet { 303 | raw_block_t blocks[BLOCKS_PER_PACKET]; 304 | uint32_t revolution; 305 | uint8_t status[PACKET_STATUS_SIZE]; 306 | } raw_packet_t; 307 | 308 | /** configuration parameters */ 309 | typedef struct { 310 | double max_range; ///< maximum range to publish 311 | double min_range; 312 | int min_angle; ///< minimum angle to publish 313 | int max_angle; ///< maximum angle to publish 314 | } Config; 315 | Config m_config; 316 | 317 | 318 | ModelType m_modelType; 319 | 320 | double mVLP16TimeBlock[1824][16]; 321 | }; 322 | 323 | } 324 | 325 | 326 | #endif 327 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | --------------------------------------------------------------------------------