├── README.md ├── SC-PGO ├── CMakeLists.txt ├── docker │ ├── Dockerfile │ ├── Makefile │ └── run.sh ├── include │ ├── aloam_velodyne │ │ ├── common.h │ │ └── tic_toc.h │ └── scancontext │ │ ├── KDTreeVectorOfVectorsAdaptor.h │ │ ├── Scancontext.cpp │ │ ├── Scancontext.h │ │ └── nanoflann.hpp ├── launch │ └── loop_closure.launch ├── package.xml ├── rviz_cfg │ └── aloam_velodyne.rviz ├── src │ ├── kittiHelper.cpp │ ├── laserMapping.cpp │ ├── laserOdometry.cpp │ ├── laserPosegraphOptimization.cpp │ ├── lidarFactor.hpp │ └── scanRegistration.cpp └── utils │ └── python │ ├── __pycache__ │ └── pypcdMyUtils.cpython-36.pyc │ ├── bone_table.npy │ ├── jet_table.npy │ ├── makeMergedMap.py │ └── pypcdMyUtils.py └── sr_lio ├── CMakeLists.txt ├── LICENSE ├── config ├── ulhk_CA.yaml └── urban_nav.yaml ├── include ├── cloudMap.h ├── cloudProcessing.h ├── eskfEstimator.h ├── lioOptimization.h ├── parameters.h ├── state.h └── utility.h ├── launch ├── lio_ulhk_CA.launch └── lio_urban_nav.launch ├── package.xml ├── rviz_cfg └── visualization.rviz ├── src ├── cloudMap.cpp ├── cloudProcessing.cpp ├── eskfEstimator.cpp ├── lioOptimization.cpp ├── optimize.cpp ├── parameters.cpp ├── state.cpp └── utility.cpp └── thirdLibrary └── tessil-src ├── CMakeLists.txt ├── LICENSE ├── README.md ├── appveyor.yml ├── cmake └── tsl-robin-mapConfig.cmake.in ├── doxygen.conf ├── include └── tsl │ ├── robin_growth_policy.h │ ├── robin_hash.h │ ├── robin_map.h │ └── robin_set.h ├── tests ├── CMakeLists.txt ├── custom_allocator_tests.cpp ├── main.cpp ├── policy_tests.cpp ├── robin_map_tests.cpp ├── robin_set_tests.cpp └── utils.h └── tsl-robin-map.natvis /README.md: -------------------------------------------------------------------------------- 1 | # Dynamic_LIO 2 | 3 | **Dynamic-LIO** is a LiDAR-inertial odometry for dynamic driving scenarios. The front-end odometry is designed based on the framework of [**SR-LIO**](https://github.com/ZikangYuan/sr_lio), and the back-end loop closure utilizes [**SC-A-LOAM**](https://github.com/gisbi-kim/SC-A-LOAM). 4 | 5 | ## Related Work 6 | 7 | [LiDAR-Inertial Odometry in Dynamic Driving Scenarios using Label Consistency Detection](https://arxiv.org/abs/2407.03590) 8 | 9 | Authors: [*Zikang Yuan*](https://scholar.google.com/citations?hl=zh-CN&user=acxdM9gAAAAJ), *Xiaoxiang Wang*, *Jingying Wu*, [*Junda Cheng*](https://scholar.google.com/citations?user=_G_Tu9EAAAAJ&hl=zh-CN) and [*Xin Yang*](https://scholar.google.com/citations?user=lsz8OOYAAAAJ&hl=zh-CN) 10 | 11 | [SR-LIO: LiDAR-Inertial Odometry with Sweep Reconstruction](https://ieeexplore.ieee.org/document/10802314) 12 | 13 | Authors: [*Zikang Yuan*](https://scholar.google.com/citations?hl=zh-CN&user=acxdM9gAAAAJ), [*Fengtian Lang*](https://scholar.google.com/citations?hl=zh-CN&user=zwgGSkEAAAAJ&view_op=list_works&gmla=ABEO0Yrl4-YPuowyntSYyCW760yxM5-IWkF8FGV4t9bs9qz1oWrqnlHmPdbt7LMcMDc04kl2puqRR4FaZvaCUONsX7MQhuAC6a--VS2pTsuwj-CyKgWp3iWDP2TS0I__Zui5da4), *Tianle Xu* and [*Xin Yang*](https://scholar.google.com/citations?user=lsz8OOYAAAAJ&hl=zh-CN) 14 | 15 | ## Installation 16 | 17 | ### 1. Requirements 18 | 19 | > GCC >= 7.5.0 20 | > 21 | > Cmake >= 3.16.0 22 | > 23 | > [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) >= 3.3.4 24 | > 25 | > [OpenCV](https://github.com/opencv/opencv) >= 3.3 26 | > 27 | > [PCL](https://pointclouds.org/downloads/) == 1.8 for Ubuntu 18.04, and == 1.10 for Ubuntu 20.04 28 | > 29 | > [GTSAM](https://github.com/borglab/gtsam/tree/4.0.3) == 4.0.3 for Ubuntu 20.04 30 | > 31 | > [ROS](http://wiki.ros.org/ROS/Installation) 32 | 33 | ##### Have Tested On: 34 | 35 | | OS | GCC | Cmake | Eigen3 | OpenCV | PCL | GTSAM | 36 | |:-:|:-:|:-:|:-:|:-:|:-:|:-:| 37 | | Ubuntu 20.04 | 9.4.0 | 3.16.3 | 3.3.7 | 4.2.0 | 1.10.0 | 4.0.3 | 38 | 39 | ### 2. Create ROS workspace 40 | 41 | ```bash 42 | mkdir -p ~/Dynamic-LIO/src 43 | cd Dynamic-LIO/src 44 | ``` 45 | 46 | ### 3. Clone the directory and build 47 | 48 | ```bash 49 | git clone https://github.com/ZikangYuan/dynamic_lio.git 50 | cd .. 51 | catkin_make 52 | ``` 53 | 54 | ## Run on Public Datasets 55 | 56 | Noted: 57 | 58 | A. The warning message "Failed to find match for field 'time'." doesn't matter. It can be ignored. 59 | 60 | B. **Please create a folder named "output" in "sr_lio" folder before running.** When **Dynamic-LIO** is running, the estimated pose of odometry is recorded in real time in the **pose.txt** located in the **output folder**, and the estimated pose with loop closure is recorded in the **final_pose.txt** located in the **output folder** after finishing a sequence. 61 | 62 | ### 1. Run on [*ULHK-CA*](https://github.com/weisongwen/UrbanLoco) 63 | 64 | Please go to the workspace of **Dynamic-LIO** and open two terminals, type the following command in terminal 1: 65 | 66 | ```bash 67 | cd Dynamic-LIO 68 | source devel/setup.bash 69 | roslaunch sr_lio lio_ulhk_CA.launch 70 | ``` 71 | 72 | and type the following command in terminal 2: 73 | 74 | ```bash 75 | cd Dynamic-LIO 76 | source devel/setup.bash 77 | roslaunch aloam_velodyne loop_closure.launch 78 | ``` 79 | 80 | Then open the terminal in the path of the bag file, and type: 81 | 82 | ```bash 83 | rosbag play SEQUENCE_NAME.bag --clock -d 1.0 84 | ``` 85 | 86 | ### 2. Run on [*Urban-Nav*](https://github.com/weisongwen/UrbanNavDataset) 87 | 88 | Please go to the workspace of **Dynamic-LIO** and open two terminals, type the following command in terminal 1: 89 | 90 | ```bash 91 | cd Dynamic-LIO 92 | source devel/setup.bash 93 | roslaunch sr_lio lio_urban_nav.launch 94 | ``` 95 | 96 | and type the following command in terminal 2: 97 | 98 | ```bash 99 | cd Dynamic-LIO 100 | source devel/setup.bash 101 | roslaunch aloam_velodyne loop_closure.launch 102 | ``` 103 | 104 | Then open the terminal in the path of the bag file, and type: 105 | 106 | ```bash 107 | rosbag play SEQUENCE_NAME.bag --clock -d 1.0 108 | ``` 109 | 110 | ## Citation 111 | 112 | If you use our work in your research project, please consider citing: 113 | 114 | ``` 115 | @article{yuan2024lidar, 116 | title={LiDAR-Inertial Odometry in Dynamic Driving Scenarios using Label Consistency Detection}, 117 | author={Yuan, Zikang and Wang, Xiaoxiang and Wu, jingying and Cheng, junda and Yang, Xin}, 118 | journal={arXiv preprint arXiv:2407.03590}, 119 | year={2024} 120 | } 121 | ``` 122 | 123 | ## Acknowledgments 124 | 125 | Thanks for [CT-ICP](https://github.com/jedeschaud/ct_icp), [Fast-LIO](https://github.com/hku-mars/FAST_LIO), [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM) and [Open-VINs](https://github.com/rpng/open_vins). 126 | -------------------------------------------------------------------------------- /SC-PGO/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aloam_velodyne) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | # set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | geometry_msgs 11 | nav_msgs 12 | sensor_msgs 13 | roscpp 14 | rospy 15 | rosbag 16 | std_msgs 17 | image_transport 18 | cv_bridge 19 | tf 20 | ) 21 | 22 | #find_package(Eigen3 REQUIRED) 23 | find_package(PCL REQUIRED) 24 | find_package(OpenCV REQUIRED) 25 | find_package(Ceres REQUIRED) 26 | 27 | find_package(OpenMP REQUIRED) 28 | find_package(GTSAM REQUIRED QUIET) 29 | 30 | include_directories( 31 | include 32 | ${catkin_INCLUDE_DIRS} 33 | ${PCL_INCLUDE_DIRS} 34 | ${CERES_INCLUDE_DIRS} 35 | ${OpenCV_INCLUDE_DIRS} 36 | ${GTSAM_INCLUDE_DIR} 37 | ) 38 | 39 | catkin_package( 40 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 41 | DEPENDS EIGEN3 PCL 42 | INCLUDE_DIRS include 43 | ) 44 | 45 | 46 | add_executable(ascanRegistration src/scanRegistration.cpp) 47 | target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 48 | 49 | add_executable(alaserOdometry src/laserOdometry.cpp) 50 | target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 51 | 52 | add_executable(alaserMapping src/laserMapping.cpp) 53 | target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 54 | 55 | add_executable(alaserPGO 56 | src/laserPosegraphOptimization.cpp 57 | include/scancontext/Scancontext.cpp 58 | ) 59 | target_compile_options(alaserPGO 60 | PRIVATE ${OpenMP_CXX_FLAGS} 61 | ) 62 | target_link_libraries(alaserPGO 63 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} 64 | ${OpenMP_CXX_FLAGS} 65 | gtsam 66 | ) 67 | 68 | add_executable(kittiHelper src/kittiHelper.cpp) 69 | target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /SC-PGO/docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-perception 2 | 3 | ENV CERES_VERSION="1.12.0" 4 | ENV PCL_VERSION="1.8.0" 5 | ENV CATKIN_WS=/root/catkin_ws 6 | 7 | # setup processors number used to compile library 8 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \ 9 | # Install dependencies 10 | apt-get update && apt-get install -y \ 11 | cmake \ 12 | libatlas-base-dev \ 13 | libeigen3-dev \ 14 | libgoogle-glog-dev \ 15 | libsuitesparse-dev \ 16 | python-catkin-tools \ 17 | ros-${ROS_DISTRO}-cv-bridge \ 18 | ros-${ROS_DISTRO}-image-transport \ 19 | ros-${ROS_DISTRO}-message-filters \ 20 | ros-${ROS_DISTRO}-tf && \ 21 | rm -rf /var/lib/apt/lists/* && \ 22 | # Build and install Ceres 23 | git clone https://ceres-solver.googlesource.com/ceres-solver && \ 24 | cd ceres-solver && \ 25 | git checkout tags/${CERES_VERSION} && \ 26 | mkdir build && cd build && \ 27 | cmake .. && \ 28 | make -j${USE_PROC} install && \ 29 | cd ../.. && \ 30 | rm -rf ./ceres-solver && \ 31 | # Build and install pcl 32 | git clone https://github.com/PointCloudLibrary/pcl.git && \ 33 | cd pcl && \ 34 | git checkout tags/pcl-${PCL_VERSION} && \ 35 | mkdir build && cd build && \ 36 | cmake .. && \ 37 | make -j${USE_PROC} install && \ 38 | cd ../.. && \ 39 | rm -rf ./pcl && \ 40 | # Setup catkin workspace 41 | mkdir -p $CATKIN_WS/src/A-LOAM/ 42 | 43 | # WORKDIR $CATKIN_WS/src 44 | 45 | # Copy A-LOAM 46 | COPY ./ $CATKIN_WS/src/A-LOAM/ 47 | # use the following line if you only have this dockerfile 48 | # RUN git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git 49 | 50 | # Build A-LOAM 51 | WORKDIR $CATKIN_WS 52 | ENV TERM xterm 53 | ENV PYTHONIOENCODING UTF-8 54 | RUN catkin config \ 55 | --extend /opt/ros/$ROS_DISTRO \ 56 | --cmake-args \ 57 | -DCMAKE_BUILD_TYPE=Release && \ 58 | catkin build && \ 59 | sed -i '/exec "$@"/i \ 60 | source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh 61 | -------------------------------------------------------------------------------- /SC-PGO/docker/Makefile: -------------------------------------------------------------------------------- 1 | all: help 2 | 3 | help: 4 | @echo "" 5 | @echo "-- Help Menu" 6 | @echo "" 7 | @echo " 1. make build - build all images" 8 | # @echo " 1. make pull - pull all images" 9 | @echo " 1. make clean - remove all images" 10 | @echo "" 11 | 12 | build: 13 | @docker build --tag ros:aloam -f ./Dockerfile .. 14 | 15 | clean: 16 | @docker rmi -f ros:aloam 17 | -------------------------------------------------------------------------------- /SC-PGO/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | trap : SIGTERM SIGINT 3 | 4 | function abspath() { 5 | # generate absolute path from relative path 6 | # $1 : relative filename 7 | # return : absolute path 8 | if [ -d "$1" ]; then 9 | # dir 10 | (cd "$1"; pwd) 11 | elif [ -f "$1" ]; then 12 | # file 13 | if [[ $1 = /* ]]; then 14 | echo "$1" 15 | elif [[ $1 == */* ]]; then 16 | echo "$(cd "${1%/*}"; pwd)/${1##*/}" 17 | else 18 | echo "$(pwd)/$1" 19 | fi 20 | fi 21 | } 22 | 23 | if [ "$#" -ne 1 ]; then 24 | echo "Usage: $0 LIDAR_SCAN_NUMBER" >&2 25 | exit 1 26 | fi 27 | 28 | roscore & 29 | ROSCORE_PID=$! 30 | sleep 1 31 | 32 | rviz -d ../rviz_cfg/aloam_velodyne.rviz & 33 | RVIZ_PID=$! 34 | 35 | A_LOAM_DIR=$(abspath "..") 36 | 37 | if [ "$1" -eq 16 ]; then 38 | docker run \ 39 | -it \ 40 | --rm \ 41 | --net=host \ 42 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ 43 | ros:aloam \ 44 | /bin/bash -c \ 45 | "cd /root/catkin_ws/; \ 46 | catkin config \ 47 | --cmake-args \ 48 | -DCMAKE_BUILD_TYPE=Release; \ 49 | catkin build; \ 50 | source devel/setup.bash; \ 51 | roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch rviz:=false" 52 | elif [ "$1" -eq "32" ]; then 53 | docker run \ 54 | -it \ 55 | --rm \ 56 | --net=host \ 57 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ 58 | ros:aloam \ 59 | /bin/bash -c \ 60 | "cd /root/catkin_ws/; \ 61 | catkin config \ 62 | --cmake-args \ 63 | -DCMAKE_BUILD_TYPE=Release; \ 64 | catkin build; \ 65 | source devel/setup.bash; \ 66 | roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch rviz:=false" 67 | elif [ "$1" -eq "64" ]; then 68 | docker run \ 69 | -it \ 70 | --rm \ 71 | --net=host \ 72 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ 73 | ros:aloam \ 74 | /bin/bash -c \ 75 | "cd /root/catkin_ws/; \ 76 | catkin config \ 77 | --cmake-args \ 78 | -DCMAKE_BUILD_TYPE=Release; \ 79 | catkin build; \ 80 | source devel/setup.bash; \ 81 | roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch rviz:=false" 82 | fi 83 | 84 | wait $ROSCORE_PID 85 | wait $RVIZ_PID 86 | 87 | if [[ $? -gt 128 ]] 88 | then 89 | kill $ROSCORE_PID 90 | kill $RVIZ_PID 91 | fi -------------------------------------------------------------------------------- /SC-PGO/include/aloam_velodyne/common.h: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #pragma once 38 | 39 | #include 40 | 41 | #include 42 | 43 | typedef pcl::PointXYZI PointType; 44 | 45 | inline double rad2deg(double radians) 46 | { 47 | return radians * 180.0 / M_PI; 48 | } 49 | 50 | inline double deg2rad(double degrees) 51 | { 52 | return degrees * M_PI / 180.0; 53 | } 54 | 55 | struct Pose6D { 56 | double x; 57 | double y; 58 | double z; 59 | double roll; 60 | double pitch; 61 | double yaw; 62 | }; -------------------------------------------------------------------------------- /SC-PGO/include/aloam_velodyne/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | 34 | class TicTocV2 35 | { 36 | public: 37 | TicTocV2() 38 | { 39 | tic(); 40 | } 41 | 42 | TicTocV2( bool _disp ) 43 | { 44 | disp_ = _disp; 45 | tic(); 46 | } 47 | 48 | void tic() 49 | { 50 | start = std::chrono::system_clock::now(); 51 | } 52 | 53 | void toc( std::string _about_task ) 54 | { 55 | end = std::chrono::system_clock::now(); 56 | std::chrono::duration elapsed_seconds = end - start; 57 | double elapsed_ms = elapsed_seconds.count() * 1000; 58 | 59 | if( disp_ ) 60 | { 61 | std::cout.precision(3); // 10 for sec, 3 for ms 62 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 63 | } 64 | } 65 | 66 | private: 67 | std::chrono::time_point start, end; 68 | bool disp_ = false; 69 | }; -------------------------------------------------------------------------------- /SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include "scancontext/nanoflann.hpp" 32 | 33 | #include 34 | 35 | // ===== This example shows how to use nanoflann with these types of containers: ======= 36 | //typedef std::vector > my_vector_of_vectors_t; 37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include 38 | // ===================================================================================== 39 | 40 | 41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. 42 | * The i'th vector represents a point in the state space. 43 | * 44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. 45 | * \tparam num_t The type of the point coordinates (typically, double or float). 46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) 48 | */ 49 | template 50 | struct KDTreeVectorOfVectorsAdaptor 51 | { 52 | typedef KDTreeVectorOfVectorsAdaptor self_t; 53 | typedef typename Distance::template traits::distance_t metric_t; 54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; 55 | 56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. 57 | 58 | /// Constructor: takes a const ref to the vector of vectors object with the data points 59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) 60 | { 61 | assert(mat.size() != 0 && mat[0].size() != 0); 62 | const size_t dims = mat[0].size(); 63 | if (DIM>0 && static_cast(dims) != DIM) 64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); 65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); 66 | index->buildIndex(); 67 | } 68 | 69 | ~KDTreeVectorOfVectorsAdaptor() { 70 | delete index; 71 | } 72 | 73 | const VectorOfVectorsType &m_data; 74 | 75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). 76 | * Note that this is a short-cut method for index->findNeighbors(). 77 | * The user can also call index->... methods as desired. 78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 79 | */ 80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const 81 | { 82 | nanoflann::KNNResultSet resultSet(num_closest); 83 | resultSet.init(out_indices, out_distances_sq); 84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 85 | } 86 | 87 | /** @name Interface expected by KDTreeSingleIndexAdaptor 88 | * @{ */ 89 | 90 | const self_t & derived() const { 91 | return *this; 92 | } 93 | self_t & derived() { 94 | return *this; 95 | } 96 | 97 | // Must return the number of data points 98 | inline size_t kdtree_get_point_count() const { 99 | return m_data.size(); 100 | } 101 | 102 | // Returns the dim'th component of the idx'th point in the class: 103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { 104 | return m_data[idx][dim]; 105 | } 106 | 107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 110 | template 111 | bool kdtree_get_bbox(BBOX & /*bb*/) const { 112 | return false; 113 | } 114 | 115 | /** @} */ 116 | 117 | }; // end of KDTreeVectorOfVectorsAdaptor 118 | -------------------------------------------------------------------------------- /SC-PGO/include/scancontext/Scancontext.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "scancontext/nanoflann.hpp" 26 | #include "scancontext/KDTreeVectorOfVectorsAdaptor.h" 27 | #include "aloam_velodyne/tic_toc.h" 28 | 29 | using namespace Eigen; 30 | using namespace nanoflann; 31 | 32 | using std::cout; 33 | using std::endl; 34 | using std::make_pair; 35 | 36 | using std::atan2; 37 | using std::cos; 38 | using std::sin; 39 | 40 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) 41 | using KeyMat = std::vector >; 42 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; 43 | 44 | 45 | // namespace SC2 46 | // { 47 | 48 | void coreImportTest ( void ); 49 | 50 | 51 | // sc param-independent helper functions 52 | float xy2theta( const float & _x, const float & _y ); 53 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ); 54 | std::vector eig2stdvec( MatrixXd _eigmat ); 55 | 56 | 57 | class SCManager 58 | { 59 | public: 60 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. 61 | 62 | Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); 63 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); 64 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); 65 | 66 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 67 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) 68 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) 69 | 70 | // User-side API 71 | void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); 72 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw 73 | 74 | // for ltslam 75 | // User-side API for multi-session 76 | void saveScancontextAndKeys( Eigen::MatrixXd _scd ); 77 | std::pair detectLoopClosureIDBetweenSession ( std::vector& curr_key, Eigen::MatrixXd& curr_desc); 78 | 79 | const Eigen::MatrixXd& getConstRefRecentSCD(void); 80 | 81 | public: 82 | // hyper parameters () 83 | const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. 84 | 85 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) 86 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) 87 | double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) 88 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); 89 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); 90 | 91 | // tree 92 | const int NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. 93 | const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper) 94 | 95 | // loop thres 96 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. 97 | // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) 98 | 99 | double SC_DIST_THRES = 0.2; // 0.1 0.2 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 100 | // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 101 | 102 | // config 103 | const int TREE_MAKING_PERIOD_ = 30; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N. 104 | int tree_making_period_conter = 0; 105 | 106 | // setter 107 | void setSCdistThres(double _new_thres); 108 | void setMaximumRadius(double _max_r); 109 | 110 | // data 111 | std::vector polarcontexts_timestamp_; // optional. 112 | std::vector polarcontexts_; 113 | std::vector polarcontext_invkeys_; 114 | std::vector polarcontext_vkeys_; 115 | 116 | KeyMat polarcontext_invkeys_mat_; 117 | KeyMat polarcontext_invkeys_to_search_; 118 | std::unique_ptr polarcontext_tree_; 119 | 120 | bool is_tree_batch_made = false; 121 | std::unique_ptr polarcontext_tree_batch_; 122 | 123 | }; // SCManager 124 | 125 | // } // namespace SC2 126 | -------------------------------------------------------------------------------- /SC-PGO/launch/loop_closure.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 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /SC-PGO/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aloam_velodyne 4 | 0.0.0 5 | 6 | 7 | This is an advanced implentation of LOAM. 8 | LOAM is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | qintong 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | rosbag 26 | sensor_msgs 27 | tf 28 | image_transport 29 | 30 | geometry_msgs 31 | nav_msgs 32 | sensor_msgs 33 | roscpp 34 | rospy 35 | std_msgs 36 | rosbag 37 | tf 38 | image_transport 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /SC-PGO/src/kittiHelper.cpp: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | std::vector read_lidar_data(const std::string lidar_data_path) 26 | { 27 | std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary); 28 | lidar_data_file.seekg(0, std::ios::end); 29 | const size_t num_elements = lidar_data_file.tellg() / sizeof(float); 30 | lidar_data_file.seekg(0, std::ios::beg); 31 | 32 | std::vector lidar_data_buffer(num_elements); 33 | lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), num_elements*sizeof(float)); 34 | return lidar_data_buffer; 35 | } 36 | 37 | int main(int argc, char** argv) 38 | { 39 | ros::init(argc, argv, "kitti_helper"); 40 | ros::NodeHandle n("~"); 41 | std::string dataset_folder, sequence_number, output_bag_file; 42 | n.getParam("dataset_folder", dataset_folder); 43 | n.getParam("sequence_number", sequence_number); 44 | std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n'; 45 | bool to_bag; 46 | n.getParam("to_bag", to_bag); 47 | if (to_bag) 48 | n.getParam("output_bag_file", output_bag_file); 49 | int publish_delay; 50 | n.getParam("publish_delay", publish_delay); 51 | publish_delay = publish_delay <= 0 ? 1 : publish_delay; 52 | 53 | ros::Publisher pub_laser_cloud = n.advertise("/velodyne_points", 2); 54 | 55 | image_transport::ImageTransport it(n); 56 | image_transport::Publisher pub_image_left = it.advertise("/image_left", 2); 57 | image_transport::Publisher pub_image_right = it.advertise("/image_right", 2); 58 | 59 | ros::Publisher pubOdomGT = n.advertise ("/odometry_gt", 5); 60 | nav_msgs::Odometry odomGT; 61 | odomGT.header.frame_id = "camera_init"; 62 | odomGT.child_frame_id = "/ground_truth"; 63 | 64 | ros::Publisher pubPathGT = n.advertise ("/path_gt", 5); 65 | nav_msgs::Path pathGT; 66 | pathGT.header.frame_id = "camera_init"; 67 | 68 | std::string timestamp_path = "sequences/" + sequence_number + "/times.txt"; 69 | std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in); 70 | 71 | std::string ground_truth_path = "results/" + sequence_number + ".txt"; 72 | std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in); 73 | 74 | rosbag::Bag bag_out; 75 | if (to_bag) 76 | bag_out.open(output_bag_file, rosbag::bagmode::Write); 77 | 78 | Eigen::Matrix3d R_transform; 79 | R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0; 80 | Eigen::Quaterniond q_transform(R_transform); 81 | 82 | std::string line; 83 | std::size_t line_num = 0; 84 | 85 | ros::Rate r(10.0 / publish_delay); 86 | while (std::getline(timestamp_file, line) && ros::ok()) 87 | { 88 | float timestamp = stof(line); 89 | std::stringstream left_image_path, right_image_path; 90 | left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png"; 91 | cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE); 92 | right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png"; 93 | cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE); 94 | 95 | std::getline(ground_truth_file, line); 96 | std::stringstream pose_stream(line); 97 | std::string s; 98 | Eigen::Matrix gt_pose; 99 | for (std::size_t i = 0; i < 3; ++i) 100 | { 101 | for (std::size_t j = 0; j < 4; ++j) 102 | { 103 | std::getline(pose_stream, s, ' '); 104 | gt_pose(i, j) = stof(s); 105 | } 106 | } 107 | 108 | Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>()); 109 | Eigen::Quaterniond q = q_transform * q_w_i; 110 | q.normalize(); 111 | Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>(); 112 | 113 | odomGT.header.stamp = ros::Time().fromSec(timestamp); 114 | odomGT.pose.pose.orientation.x = q.x(); 115 | odomGT.pose.pose.orientation.y = q.y(); 116 | odomGT.pose.pose.orientation.z = q.z(); 117 | odomGT.pose.pose.orientation.w = q.w(); 118 | odomGT.pose.pose.position.x = t(0); 119 | odomGT.pose.pose.position.y = t(1); 120 | odomGT.pose.pose.position.z = t(2); 121 | pubOdomGT.publish(odomGT); 122 | 123 | geometry_msgs::PoseStamped poseGT; 124 | poseGT.header = odomGT.header; 125 | poseGT.pose = odomGT.pose.pose; 126 | pathGT.header.stamp = odomGT.header.stamp; 127 | pathGT.poses.push_back(poseGT); 128 | pubPathGT.publish(pathGT); 129 | 130 | // read lidar point cloud 131 | std::stringstream lidar_data_path; 132 | lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" 133 | << std::setfill('0') << std::setw(6) << line_num << ".bin"; 134 | std::vector lidar_data = read_lidar_data(lidar_data_path.str()); 135 | std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n"; 136 | 137 | std::vector lidar_points; 138 | std::vector lidar_intensities; 139 | pcl::PointCloud laser_cloud; 140 | for (std::size_t i = 0; i < lidar_data.size(); i += 4) 141 | { 142 | lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]); 143 | lidar_intensities.push_back(lidar_data[i+3]); 144 | 145 | pcl::PointXYZI point; 146 | point.x = lidar_data[i]; 147 | point.y = lidar_data[i + 1]; 148 | point.z = lidar_data[i + 2]; 149 | point.intensity = lidar_data[i + 3]; 150 | laser_cloud.push_back(point); 151 | } 152 | 153 | sensor_msgs::PointCloud2 laser_cloud_msg; 154 | pcl::toROSMsg(laser_cloud, laser_cloud_msg); 155 | laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp); 156 | laser_cloud_msg.header.frame_id = "camera_init"; 157 | pub_laser_cloud.publish(laser_cloud_msg); 158 | 159 | sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg(); 160 | sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg(); 161 | pub_image_left.publish(image_left_msg); 162 | pub_image_right.publish(image_right_msg); 163 | 164 | if (to_bag) 165 | { 166 | bag_out.write("/image_left", ros::Time::now(), image_left_msg); 167 | bag_out.write("/image_right", ros::Time::now(), image_right_msg); 168 | bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg); 169 | bag_out.write("/path_gt", ros::Time::now(), pathGT); 170 | bag_out.write("/odometry_gt", ros::Time::now(), odomGT); 171 | } 172 | 173 | line_num ++; 174 | r.sleep(); 175 | } 176 | bag_out.close(); 177 | std::cout << "Done \n"; 178 | 179 | 180 | return 0; 181 | } -------------------------------------------------------------------------------- /SC-PGO/src/lidarFactor.hpp: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | struct LidarEdgeFactor 13 | { 14 | LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, 15 | Eigen::Vector3d last_point_b_, double s_) 16 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {} 17 | 18 | template 19 | bool operator()(const T *q, const T *t, T *residual) const 20 | { 21 | 22 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 23 | Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; 24 | Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; 25 | 26 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 27 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 28 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 29 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 30 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 31 | 32 | Eigen::Matrix lp; 33 | lp = q_last_curr * cp + t_last_curr; 34 | 35 | Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); 36 | Eigen::Matrix de = lpa - lpb; 37 | 38 | residual[0] = nu.x() / de.norm(); 39 | residual[1] = nu.y() / de.norm(); 40 | residual[2] = nu.z() / de.norm(); 41 | 42 | return true; 43 | } 44 | 45 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, 46 | const Eigen::Vector3d last_point_b_, const double s_) 47 | { 48 | return (new ceres::AutoDiffCostFunction< 49 | LidarEdgeFactor, 3, 4, 3>( 50 | new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); 51 | } 52 | 53 | Eigen::Vector3d curr_point, last_point_a, last_point_b; 54 | double s; 55 | }; 56 | 57 | struct LidarPlaneFactor 58 | { 59 | LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 60 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_) 61 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 62 | last_point_m(last_point_m_), s(s_) 63 | { 64 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 65 | ljm_norm.normalize(); 66 | } 67 | 68 | template 69 | bool operator()(const T *q, const T *t, T *residual) const 70 | { 71 | 72 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 73 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 74 | //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; 75 | //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; 76 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 77 | 78 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 79 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 80 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 81 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 82 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 83 | 84 | Eigen::Matrix lp; 85 | lp = q_last_curr * cp + t_last_curr; 86 | 87 | residual[0] = (lp - lpj).dot(ljm); 88 | 89 | return true; 90 | } 91 | 92 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 93 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 94 | const double s_) 95 | { 96 | return (new ceres::AutoDiffCostFunction< 97 | LidarPlaneFactor, 1, 4, 3>( 98 | new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_))); 99 | } 100 | 101 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 102 | Eigen::Vector3d ljm_norm; 103 | double s; 104 | }; 105 | 106 | struct LidarPlaneNormFactor 107 | { 108 | 109 | LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, 110 | double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), 111 | negative_OA_dot_norm(negative_OA_dot_norm_) {} 112 | 113 | template 114 | bool operator()(const T *q, const T *t, T *residual) const 115 | { 116 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 117 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 118 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 119 | Eigen::Matrix point_w; 120 | point_w = q_w_curr * cp + t_w_curr; 121 | 122 | Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); 123 | residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); 124 | return true; 125 | } 126 | 127 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, 128 | const double negative_OA_dot_norm_) 129 | { 130 | return (new ceres::AutoDiffCostFunction< 131 | LidarPlaneNormFactor, 1, 4, 3>( 132 | new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); 133 | } 134 | 135 | Eigen::Vector3d curr_point; 136 | Eigen::Vector3d plane_unit_norm; 137 | double negative_OA_dot_norm; 138 | }; 139 | 140 | 141 | struct LidarDistanceFactor 142 | { 143 | 144 | LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) 145 | : curr_point(curr_point_), closed_point(closed_point_){} 146 | 147 | template 148 | bool operator()(const T *q, const T *t, T *residual) const 149 | { 150 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 151 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 152 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 153 | Eigen::Matrix point_w; 154 | point_w = q_w_curr * cp + t_w_curr; 155 | 156 | 157 | residual[0] = point_w.x() - T(closed_point.x()); 158 | residual[1] = point_w.y() - T(closed_point.y()); 159 | residual[2] = point_w.z() - T(closed_point.z()); 160 | return true; 161 | } 162 | 163 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_) 164 | { 165 | return (new ceres::AutoDiffCostFunction< 166 | LidarDistanceFactor, 3, 4, 3>( 167 | new LidarDistanceFactor(curr_point_, closed_point_))); 168 | } 169 | 170 | Eigen::Vector3d curr_point; 171 | Eigen::Vector3d closed_point; 172 | }; -------------------------------------------------------------------------------- /SC-PGO/utils/python/__pycache__/pypcdMyUtils.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZikangYuan/dynamic_lio/4e91af4adc6e1f15a14c2c6f7c769272516a7062/SC-PGO/utils/python/__pycache__/pypcdMyUtils.cpython-36.pyc -------------------------------------------------------------------------------- /SC-PGO/utils/python/bone_table.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZikangYuan/dynamic_lio/4e91af4adc6e1f15a14c2c6f7c769272516a7062/SC-PGO/utils/python/bone_table.npy -------------------------------------------------------------------------------- /SC-PGO/utils/python/jet_table.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZikangYuan/dynamic_lio/4e91af4adc6e1f15a14c2c6f7c769272516a7062/SC-PGO/utils/python/jet_table.npy -------------------------------------------------------------------------------- /SC-PGO/utils/python/makeMergedMap.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | import copy 5 | from io import StringIO 6 | 7 | import pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git 8 | from pypcd import pypcd 9 | 10 | import numpy as np 11 | from numpy import linalg as LA 12 | 13 | import open3d as o3d 14 | 15 | from pypcdMyUtils import * 16 | 17 | jet_table = np.load('jet_table.npy') 18 | bone_table = np.load('bone_table.npy') 19 | 20 | color_table = jet_table 21 | color_table_len = color_table.shape[0] 22 | 23 | 24 | ########################## 25 | # User only consider this block 26 | ########################## 27 | 28 | data_dir = "/home/user/Documents/catkin2021/catkin_fastlio2/data/" # should end with / 29 | scan_idx_range_to_stack = [0, 200] # if you want a whole map, use [0, len(scan_files)] 30 | node_skip = 1 31 | 32 | num_points_in_a_scan = 150000 # for reservation (save faster) // e.g., use 150000 for 128 ray lidars, 100000 for 64 ray lidars, 30000 for 16 ray lidars, if error occured, use the larger value. 33 | 34 | is_live_vis = False # recommend to use false 35 | is_o3d_vis = True 36 | intensity_color_max = 200 37 | 38 | is_near_removal = True 39 | thres_near_removal = 2 # meter (to remove platform-myself structure ghost points) 40 | 41 | ########################## 42 | 43 | 44 | # 45 | scan_dir = data_dir + "Scans" 46 | scan_files = os.listdir(scan_dir) 47 | scan_files.sort() 48 | 49 | poses = [] 50 | f = open(data_dir+"optimized_poses.txt", 'r') 51 | while True: 52 | line = f.readline() 53 | if not line: break 54 | pose_SE3 = np.asarray([float(i) for i in line.split()]) 55 | pose_SE3 = np.vstack( (np.reshape(pose_SE3, (3, 4)), np.asarray([0,0,0,1])) ) 56 | poses.append(pose_SE3) 57 | f.close() 58 | 59 | 60 | # 61 | assert (scan_idx_range_to_stack[1] > scan_idx_range_to_stack[0]) 62 | print("Merging scans from", scan_idx_range_to_stack[0], "to", scan_idx_range_to_stack[1]) 63 | 64 | 65 | # 66 | if(is_live_vis): 67 | vis = o3d.visualization.Visualizer() 68 | vis.create_window('Map', visible = True) 69 | 70 | nodes_count = 0 71 | pcd_combined_for_vis = o3d.geometry.PointCloud() 72 | pcd_combined_for_save = None 73 | 74 | # The scans from 000000.pcd should be prepared if it is not used (because below code indexing is designed in a naive way) 75 | 76 | # manually reserve memory for fast write 77 | num_all_points_expected = int(num_points_in_a_scan * np.round((scan_idx_range_to_stack[1] - scan_idx_range_to_stack[0])/node_skip)) 78 | 79 | np_xyz_all = np.empty([num_all_points_expected, 3]) 80 | np_intensity_all = np.empty([num_all_points_expected, 1]) 81 | curr_count = 0 82 | 83 | for node_idx in range(len(scan_files)): 84 | if(node_idx < scan_idx_range_to_stack[0] or node_idx >= scan_idx_range_to_stack[1]): 85 | continue 86 | 87 | nodes_count = nodes_count + 1 88 | if( nodes_count % node_skip is not 0): 89 | if(node_idx is not scan_idx_range_to_stack[0]): # to ensure the vis init 90 | continue 91 | 92 | print("read keyframe scan idx", node_idx) 93 | 94 | scan_pose = poses[node_idx] 95 | 96 | scan_path = os.path.join(scan_dir, scan_files[node_idx]) 97 | scan_pcd = o3d.io.read_point_cloud(scan_path) 98 | scan_xyz_local = copy.deepcopy(np.asarray(scan_pcd.points)) 99 | 100 | scan_pypcd_with_intensity = pypcd.PointCloud.from_path(scan_path) 101 | scan_intensity = scan_pypcd_with_intensity.pc_data['intensity'] 102 | scan_intensity_colors_idx = np.round( (color_table_len-1) * np.minimum( 1, np.maximum(0, scan_intensity / intensity_color_max) ) ) 103 | scan_intensity_colors = color_table[scan_intensity_colors_idx.astype(int)] 104 | 105 | scan_pcd_global = scan_pcd.transform(scan_pose) # global coord, note that this is not deepcopy 106 | scan_pcd_global.colors = o3d.utility.Vector3dVector(scan_intensity_colors) 107 | scan_xyz = np.asarray(scan_pcd_global.points) 108 | 109 | scan_intensity = np.expand_dims(scan_intensity, axis=1) 110 | scan_ranges = LA.norm(scan_xyz_local, axis=1) 111 | 112 | if(is_near_removal): 113 | eff_idxes = np.where (scan_ranges > thres_near_removal) 114 | scan_xyz = scan_xyz[eff_idxes[0], :] 115 | scan_intensity = scan_intensity[eff_idxes[0], :] 116 | 117 | scan_pcd_global = scan_pcd_global.select_by_index(eff_idxes[0]) 118 | 119 | if(is_o3d_vis): 120 | pcd_combined_for_vis += scan_pcd_global # open3d pointcloud class append is fast 121 | 122 | if is_live_vis: 123 | if(node_idx is scan_idx_range_to_stack[0]): # to ensure the vis init 124 | vis.add_geometry(pcd_combined_for_vis) 125 | 126 | vis.update_geometry(pcd_combined_for_vis) 127 | vis.poll_events() 128 | vis.update_renderer() 129 | 130 | # save 131 | np_xyz_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_xyz 132 | np_intensity_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_intensity 133 | 134 | curr_count = curr_count + scan_xyz.shape[0] 135 | print(curr_count) 136 | 137 | # 138 | if(is_o3d_vis): 139 | print("draw the merged map.") 140 | o3d.visualization.draw_geometries([pcd_combined_for_vis]) 141 | 142 | 143 | # save ply having intensity 144 | np_xyz_all = np_xyz_all[0:curr_count, :] 145 | np_intensity_all = np_intensity_all[0:curr_count, :] 146 | 147 | np_xyzi_all = np.hstack( (np_xyz_all, np_intensity_all) ) 148 | xyzi = make_xyzi_point_cloud(np_xyzi_all) 149 | 150 | map_name = data_dir + "map_" + str(scan_idx_range_to_stack[0]) + "_to_" + str(scan_idx_range_to_stack[1]) + "_with_intensity.pcd" 151 | xyzi.save_pcd(map_name, compression='binary_compressed') 152 | print("intensity map is save (path:", map_name, ")") 153 | 154 | # save rgb colored points 155 | # map_name = data_dir + "map_" + str(scan_idx_range_to_stack[0]) + "_to_" + str(scan_idx_range_to_stack[1]) + ".pcd" 156 | # o3d.io.write_point_cloud(map_name, pcd_combined_for_vis) 157 | # print("the map is save (path:", map_name, ")") 158 | 159 | 160 | -------------------------------------------------------------------------------- /SC-PGO/utils/python/pypcdMyUtils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git 3 | from pypcd import pypcd 4 | 5 | def make_xyzi_point_cloud(xyzl, label_type='f'): 6 | """ Make XYZL point cloud from numpy array. 7 | TODO i labels? 8 | """ 9 | md = {'version': .7, 10 | 'fields': ['x', 'y', 'z', 'intensity'], 11 | 'count': [1, 1, 1, 1], 12 | 'width': len(xyzl), 13 | 'height': 1, 14 | 'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 15 | 'points': len(xyzl), 16 | 'data': 'ASCII'} 17 | if label_type.lower() == 'f': 18 | md['size'] = [4, 4, 4, 4] 19 | md['type'] = ['F', 'F', 'F', 'F'] 20 | elif label_type.lower() == 'u': 21 | md['size'] = [4, 4, 4, 1] 22 | md['type'] = ['F', 'F', 'F', 'U'] 23 | else: 24 | raise ValueError('label type must be F or U') 25 | # TODO use .view() 26 | xyzl = xyzl.astype(np.float32) 27 | dt = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32), 28 | ('intensity', np.float32)]) 29 | pc_data = np.rec.fromarrays([xyzl[:, 0], xyzl[:, 1], xyzl[:, 2], 30 | xyzl[:, 3]], dtype=dt) 31 | pc = pypcd.PointCloud(md, pc_data) 32 | return pc 33 | -------------------------------------------------------------------------------- /sr_lio/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sr_lio) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | eigen_conversions 8 | geometry_msgs 9 | nav_msgs 10 | pcl_conversions 11 | pcl_ros 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | tf 17 | ) 18 | 19 | include(FetchContent) 20 | 21 | FetchContent_Declare( 22 | tessil 23 | SOURCE_DIR ${PROJECT_SOURCE_DIR}/thirdLibrary/tessil-src) 24 | 25 | if (NOT tessil_POPULATED) 26 | set(BUILD_TESTING OFF) 27 | FetchContent_Populate(tessil) 28 | 29 | add_library(robin_map INTERFACE) 30 | add_library(tsl::robin_map ALIAS robin_map) 31 | 32 | target_include_directories(robin_map INTERFACE 33 | "$") 34 | 35 | list(APPEND headers "${tessil_SOURCE_DIR}/include/tsl/robin_growth_policy.h" 36 | "${tessil_SOURCE_DIR}/include/tsl/robin_hash.h" 37 | "${tessil_SOURCE_DIR}/include/tsl/robin_map.h" 38 | "${tessil_SOURCE_DIR}/include/tsl/robin_set.h") 39 | target_sources(robin_map INTERFACE "$") 40 | 41 | if (MSVC) 42 | target_sources(robin_map INTERFACE 43 | "$") 44 | endif () 45 | endif () 46 | 47 | find_package(Eigen3 REQUIRED) 48 | find_package(PCL 1.7 REQUIRED) 49 | find_package(Ceres REQUIRED) 50 | find_package(OpenCV REQUIRED) 51 | 52 | catkin_package( 53 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 54 | DEPENDS EIGEN3 PCL 55 | INCLUDE_DIRS 56 | ) 57 | 58 | include_directories( 59 | ${catkin_INCLUDE_DIRS} 60 | ${EIGEN3_INCLUDE_DIR} 61 | ${PCL_INCLUDE_DIRS} 62 | ${CERES_INCLUDE_DIRS} 63 | ${OpenCV_INCLUDE_DIRS} 64 | robin_map 65 | include 66 | ) 67 | 68 | add_executable(lio_optimization 69 | src/lioOptimization.cpp 70 | src/optimize.cpp 71 | src/cloudMap.cpp 72 | src/cloudProcessing.cpp 73 | src/state.cpp 74 | src/eskfEstimator.cpp 75 | src/utility.cpp 76 | src/parameters.cpp) 77 | target_link_libraries(lio_optimization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} robin_map) 78 | -------------------------------------------------------------------------------- /sr_lio/config/ulhk_CA.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lidar_topic: "/rslidar_points" 3 | imu_topic: "/imu_raw" 4 | point_filter_num: 1 5 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 6 | gravity_acc: [ 0.0, 0.0, 9.8] 7 | 8 | lidar_parameter: 9 | lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for robosense 10 | N_SCANS: 32 11 | Horizon_SCANS: 1800 12 | ang_res_x: 0.2 13 | ang_res_y: 1.33 14 | ang_bottom: 30.67 15 | ground_scan_id: 31 16 | SCAN_RATE: 10 # only need to be set for velodyne, unit: Hz, 17 | time_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 18 | blind: 2 19 | fov_degree: 180 20 | det_range: 100.0 21 | 22 | imu_parameter: 23 | acc_cov: 0.1 24 | gyr_cov: 0.1 25 | b_acc_cov: 0.01 26 | b_gyr_cov: 0.01 27 | time_diff_enable: false 28 | 29 | extrinsic_parameter: 30 | extrinsic_enable: false # true: enable the online estimation of IMU-LiDAR extrinsic, 31 | extrinsic_t: [ 0.0, 0.0, -7.62e-02] 32 | extrinsic_R: [ 1, 0, 0, 33 | 0, 1, 0, 34 | 0, 0, 1] 35 | 36 | odometry_options: 37 | voxel_size: 0.5 # The voxel size for the grid sampling of the new frame (before keypoints extraction) 38 | sample_voxel_size: 1.5 # The size of a voxel for the selection of `keypoints` by grid sampling 39 | max_distance: 500.0 # The threshold of the distance to suppress voxels from the map 40 | max_num_points_in_voxel: 20 # The maximum number of points per voxel of the map 41 | init_num_frames: 20 42 | min_distance_points: 0.15 43 | distance_error_threshold: 100.0 # The motion of the sensor between two frames which is considered erroneous (stops the odometry) 44 | motion_compensation: CONSTANT_VELOCITY # The profile of the motion compensation (IMU, CONSTANT_VELOCITY) 45 | initialization: INIT_IMU # [INIT_IMU, INIT_CONSTANT_VELOCITY] 46 | 47 | icp_options: 48 | size_voxel_map: 1.0 # The voxel size of in the voxel map 49 | num_iters_icp: 5 # The number of iterations of the ICP 50 | min_number_neighbors: 20 # The minimum number of neighbor points to define a valid neighborhood 51 | voxel_neighborhood: 1 52 | max_number_neighbors: 20 53 | max_dist_to_plane_ct_icp: 0.3 54 | threshold_orientation_norm: 0.1 # Threshold on orientation changes (in degrees) for early termination of the ICP 55 | threshold_translation_norm: 0.01 # Threshold on distance changes (in m) for early termination of the ICP 56 | debug_print: false 57 | num_closest_neighbors: 1 58 | min_num_residuals: 200 # The minimum number of residuals for a valid ICP problem 59 | max_num_residuals: 600 # The maximum number of residuals considered (if more keypoints exist, residuals are randomly sampled) 60 | -------------------------------------------------------------------------------- /sr_lio/config/urban_nav.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lidar_topic: "/velodyne_points" 3 | imu_topic: "/imu/data" 4 | point_filter_num: 4 5 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 6 | gravity_acc: [ 0.0, 0.0, 9.8] 7 | 8 | lidar_parameter: 9 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | N_SCANS: 32 11 | Horizon_SCANS: 1800 12 | ang_res_x: 0.2 13 | ang_res_y: 1.33 14 | ang_bottom: 30.67 15 | ground_scan_id: 31 16 | SCAN_RATE: 10 # only need to be set for velodyne, unit: Hz, 17 | time_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 18 | blind: 2 19 | fov_degree: 180 20 | det_range: 100.0 21 | 22 | imu_parameter: 23 | acc_cov: 0.1 24 | gyr_cov: 0.1 25 | b_acc_cov: 0.01 26 | b_gyr_cov: 0.01 27 | time_diff_enable: false 28 | 29 | extrinsic_parameter: 30 | extrinsic_enable: false # true: enable the online estimation of IMU-LiDAR extrinsic, 31 | extrinsic_t: [ 0.0, 0.0, 0.28] 32 | extrinsic_R: [ 1, 0, 0, 33 | 0, 1, 0, 34 | 0, 0, 1] 35 | 36 | odometry_options: 37 | voxel_size: 0.5 # The voxel size for the grid sampling of the new frame (before keypoints extraction) 38 | sample_voxel_size: 1.5 # The size of a voxel for the selection of `keypoints` by grid sampling 39 | max_distance: 100.0 # The threshold of the distance to suppress voxels from the map 40 | max_num_points_in_voxel: 20 # The maximum number of points per voxel of the map 41 | init_num_frames: 20 42 | min_distance_points: 0.15 43 | distance_error_threshold: 100.0 # The motion of the sensor between two frames which is considered erroneous (stops the odometry) 44 | motion_compensation: CONSTANT_VELOCITY # The profile of the motion compensation (IMU, CONSTANT_VELOCITY) 45 | initialization: INIT_IMU # [INIT_IMU, INIT_CONSTANT_VELOCITY] 46 | 47 | icp_options: 48 | size_voxel_map: 1.0 # The voxel size of in the voxel map 49 | num_iters_icp: 5 # The number of iterations of the ICP 50 | min_number_neighbors: 20 # The minimum number of neighbor points to define a valid neighborhood 51 | voxel_neighborhood: 1 52 | max_number_neighbors: 20 53 | max_dist_to_plane_ct_icp: 0.3 54 | threshold_orientation_norm: 0.1 # Threshold on orientation changes (in degrees) for early termination of the ICP 55 | threshold_translation_norm: 0.01 # Threshold on distance changes (in m) for early termination of the ICP 56 | debug_print: false 57 | num_closest_neighbors: 1 58 | min_num_residuals: 200 # The minimum number of residuals for a valid ICP problem 59 | max_num_residuals: 600 # The maximum number of residuals considered (if more keypoints exist, residuals are randomly sampled) 60 | -------------------------------------------------------------------------------- /sr_lio/include/cloudMap.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // eigen 11 | #include 12 | #include 13 | 14 | // robin_map 15 | #include 16 | 17 | struct point3D { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | Eigen::Vector3d raw_point; 21 | Eigen::Vector3d point; 22 | Eigen::Vector3d imu_point; 23 | double alpha_time = 0.0; 24 | double relative_time = 0.0; 25 | double timestamp = 0.0; 26 | int index_frame = -1; 27 | 28 | // 2024.04.19 yzk 29 | int label = 0; 30 | double range = -1.0; 31 | bool is_dynamic = false; 32 | bool is_undecided = false; 33 | int num_decided = 0; 34 | int num_out_view = 0; 35 | // 2024.04.19 yzk 36 | 37 | point3D() = default; 38 | }; 39 | 40 | // 2024.04.19 yzk 41 | class globalPoint 42 | { 43 | private: 44 | Eigen::Vector3f position; 45 | 46 | short label; 47 | bool is_dynamic; 48 | 49 | public: 50 | 51 | globalPoint(); 52 | 53 | void setPosition(Eigen::Vector3d &position_); 54 | 55 | void setLabel(int label_); 56 | 57 | void setDynamic(bool is_dynamic_); 58 | 59 | Eigen::Vector3d getPosition(); 60 | 61 | int getLabel(); 62 | 63 | bool isDynamic(); 64 | }; 65 | // 2024.04.19 yzk 66 | 67 | // 2023.08.07 yzk 68 | struct planeParam { 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | Eigen::Vector3d raw_point; 72 | Eigen::Vector3d norm_vector; 73 | Eigen::Matrix jacobians; 74 | double norm_offset; 75 | double distance = 0.0; 76 | double weight = 1.0; 77 | 78 | planeParam() = default; 79 | }; 80 | // 2023.08.07 yzk 81 | 82 | // 2023.08.12 yzk 83 | struct imuState { 84 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 85 | 86 | double timestamp; 87 | 88 | Eigen::Vector3d un_acc; 89 | Eigen::Vector3d un_gyr; 90 | Eigen::Vector3d trans; 91 | Eigen::Quaterniond quat; 92 | Eigen::Vector3d vel; 93 | 94 | imuState() = default; 95 | }; 96 | // 2023.08.12 yzk 97 | 98 | // 2024.04.19 yzk 99 | struct voxel { 100 | 101 | voxel() = default; 102 | 103 | voxel(short x, short y, short z) : x(x), y(y), z(z) {} 104 | 105 | bool operator==(const voxel &vox) const { return x == vox.x && y == vox.y && z == vox.z; } 106 | 107 | inline bool operator<(const voxel &vox) const { 108 | return x < vox.x || (x == vox.x && y < vox.y) || (x == vox.x && y == vox.y && z < vox.z); 109 | } 110 | 111 | inline static voxel coordinates(globalPoint &point, double voxel_size) { 112 | return {short(point.getPosition().x() / voxel_size), 113 | short(point.getPosition().y() / voxel_size), 114 | short(point.getPosition().z() / voxel_size)}; 115 | } 116 | 117 | short x; 118 | short y; 119 | short z; 120 | }; 121 | 122 | struct voxelBlock { 123 | 124 | explicit voxelBlock(int num_points_ = 20) : num_points(num_points_) { points.reserve(num_points_); } 125 | 126 | std::vector points; 127 | 128 | bool IsFull() const { return num_points == points.size(); } 129 | 130 | void AddPoint(const globalPoint &point) { 131 | assert(num_points > points.size()); 132 | points.push_back(point); 133 | } 134 | 135 | inline int NumPoints() const { return points.size(); } 136 | 137 | inline int Capacity() { return num_points; } 138 | 139 | private: 140 | int num_points; 141 | }; 142 | // 2024.04.19 yzk 143 | 144 | typedef tsl::robin_map voxelHashMap; 145 | 146 | namespace std { 147 | 148 | template<> struct hash { 149 | std::size_t operator()(const voxel &vox) const 150 | { 151 | const size_t kP1 = 73856093; 152 | const size_t kP2 = 19349669; 153 | const size_t kP3 = 83492791; 154 | return vox.x * kP1 + vox.y * kP2 + vox.z * kP3; 155 | } 156 | }; 157 | } -------------------------------------------------------------------------------- /sr_lio/include/cloudProcessing.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | #include 5 | 6 | // ros 7 | #include 8 | #include 9 | 10 | // eigen 11 | #include 12 | 13 | // pcl 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // opencv 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "cloudMap.h" 28 | 29 | enum LID_TYPE{AVIA = 1, VELO = 2, OUST = 3, ROBO = 4}; 30 | enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; 31 | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; 32 | enum Surround{Prev, Next}; 33 | enum Edge_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; 34 | 35 | struct extraElement 36 | { 37 | double range; 38 | double distance; 39 | double angle[2]; 40 | double intersect; 41 | Edge_jump edge_jump[2]; 42 | Feature feature_type; 43 | 44 | extraElement() 45 | { 46 | range = 0; 47 | edge_jump[Prev] = Nr_nor; 48 | edge_jump[Next] = Nr_nor; 49 | feature_type = Nor; 50 | intersect = 2; 51 | } 52 | }; 53 | 54 | namespace velodyne_ros { 55 | struct EIGEN_ALIGN16 Point { 56 | PCL_ADD_POINT4D; 57 | float intensity; 58 | float time; 59 | uint16_t ring; 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | }; 62 | } 63 | 64 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 65 | (float, x, x) 66 | (float, y, y) 67 | (float, z, z) 68 | (float, intensity, intensity) 69 | (float, time, time) 70 | (uint16_t, ring, ring) 71 | ) 72 | 73 | namespace robosense_ros 74 | { 75 | struct EIGEN_ALIGN16 Point { 76 | PCL_ADD_POINT4D; 77 | uint8_t intensity; 78 | uint16_t ring = 0; 79 | double timestamp = 0; 80 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 81 | }; 82 | } 83 | 84 | POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, 85 | (float, x, x) 86 | (float, y, y) 87 | (float, z, z) 88 | (uint8_t, intensity, intensity) 89 | (uint16_t, ring, ring) 90 | (double, timestamp, timestamp) 91 | ) 92 | 93 | namespace ouster_ros 94 | { 95 | struct EIGEN_ALIGN16 Point 96 | { 97 | PCL_ADD_POINT4D; 98 | float intensity; 99 | uint32_t t; 100 | uint16_t reflectivity; 101 | uint8_t ring; 102 | uint16_t ambient; 103 | uint32_t range; 104 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 105 | }; 106 | } 107 | 108 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 109 | (float, x, x) 110 | (float, y, y) 111 | (float, z, z) 112 | (float, intensity, intensity) 113 | (uint32_t, t, t) 114 | (uint16_t, reflectivity, reflectivity) 115 | (uint8_t, ring, ring) 116 | (uint16_t, ambient, ambient) 117 | (uint32_t, range, range) 118 | ) 119 | 120 | class cloudProcessing 121 | { 122 | private: 123 | 124 | int lidar_type; 125 | int N_SCANS; 126 | 127 | int Horizon_SCANS; 128 | float ang_res_x; 129 | float ang_res_y; 130 | float ang_bottom; 131 | int ground_scan_id; 132 | 133 | float sensor_mount_angle; 134 | float segment_theta; 135 | int segment_valid_point_num; 136 | int segment_valid_line_num; 137 | float segment_alpha_x; 138 | float segment_alpha_y; 139 | 140 | int SCAN_RATE; 141 | int time_unit; 142 | double time_unit_scale; 143 | double blind; 144 | 145 | Eigen::Matrix3d R_imu_lidar; 146 | Eigen::Vector3d t_imu_lidar; 147 | 148 | bool given_offset_time; 149 | 150 | bool given_ring; 151 | 152 | cv::Mat range_mat; 153 | cv::Mat label_mat; 154 | cv::Mat ground_mat; 155 | int label_count; 156 | 157 | point3D nan_point; 158 | std::vector full_cloud; 159 | 160 | std::vector> neighbor_iterator; 161 | 162 | uint16_t *all_pushed_id_x; 163 | uint16_t *all_pushed_id_y; 164 | 165 | uint16_t *queue_id_x; 166 | uint16_t *queue_id_y; 167 | 168 | int point_filter_num; 169 | 170 | int sweep_id; 171 | 172 | double delta_cut_time; 173 | 174 | std::vector> scan_cloud; 175 | std::vector> v_extra_elem; 176 | 177 | // function 178 | void ousterHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, double &dt_offset); 179 | void velodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, double &dt_offset); 180 | void robosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, double &dt_offset); 181 | 182 | double plane_parameters[4] = {-20,500, 80,-70}; 183 | 184 | void resetParameters(); 185 | 186 | void groundRemoval(); 187 | 188 | void cloudSegmentation(std::vector &v_cloud_out); 189 | 190 | void labelComponents(int row, int col); 191 | 192 | public: 193 | 194 | cloudProcessing(); 195 | 196 | void setLidarType(int para); 197 | void setNumScans(int para); 198 | 199 | void setHorizonScans(int para); 200 | void setAngResX(float para); 201 | void setAngResY(float para); 202 | void setAngBottom(float para); 203 | void setGroundScanInd(int para); 204 | 205 | void setScanRate(int para); 206 | void setTimeUnit(int para); 207 | void setBlind(double para); 208 | 209 | void setExtrinR(Eigen::Matrix3d &R); 210 | void setExtrinT(Eigen::Vector3d &t); 211 | 212 | void setUseFeature(bool para); 213 | void setPointFilterNum(int para); 214 | 215 | int getLidarType() {return lidar_type;} 216 | 217 | bool isPointTimeEnable() {return given_offset_time;} 218 | 219 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector &v_cloud_out, double &dt_offset); 220 | 221 | void initialization(); 222 | }; -------------------------------------------------------------------------------- /sr_lio/include/eskfEstimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // eigen 11 | #include 12 | 13 | // ceres 14 | #include 15 | 16 | // utility 17 | #include "utility.h" 18 | 19 | class eskfEstimator 20 | { 21 | private: 22 | double dt; 23 | Eigen::Vector3d acc_0, gyr_0; 24 | Eigen::Vector3d acc_1, gyr_1; 25 | 26 | Eigen::Vector3d acc_cov, gyr_cov; 27 | Eigen::Vector3d acc_cov_scale, gyr_cov_scale; 28 | Eigen::Vector3d b_acc_cov, b_gyr_cov; 29 | 30 | Eigen::Vector3d p; 31 | Eigen::Quaterniond q; 32 | Eigen::Vector3d v; 33 | Eigen::Vector3d ba; 34 | Eigen::Vector3d bg; 35 | Eigen::Vector3d g; 36 | 37 | Eigen::Matrix noise; 38 | Eigen::Matrix delta_state; 39 | Eigen::Matrix covariance; 40 | 41 | Eigen::Matrix lxly; 42 | 43 | Eigen::Vector3d mean_gyr, mean_acc; 44 | 45 | bool is_first_imu_meas; 46 | double time_first_imu; 47 | int num_init_meas; 48 | 49 | void initialization(const std::vector>> &imu_meas); 50 | 51 | void initializeNoise(); 52 | 53 | void midPointIntegration(Eigen::Vector3d &result_p, Eigen::Quaterniond &result_q, Eigen::Vector3d &result_v); 54 | 55 | void updateAndReset(); 56 | 57 | void projectCovariance(); 58 | 59 | public: 60 | eskfEstimator(); 61 | 62 | void tryInit(const std::vector>> &imu_meas); 63 | 64 | void setAccCov(double para); 65 | 66 | void setGyrCov(double para); 67 | 68 | void setBiasAccCov(double para); 69 | 70 | void setBiasGyrCov(double para); 71 | 72 | void initializeImuData(const Eigen::Vector3d &acc_0_, const Eigen::Vector3d &gyr_0_); 73 | 74 | void setTranslation(const Eigen::Vector3d &p_); 75 | 76 | void setRotation(const Eigen::Quaterniond &q_); 77 | 78 | void setVelocity(const Eigen::Vector3d &v_); 79 | 80 | void setBa(const Eigen::Vector3d &ba_); 81 | 82 | void setBg(const Eigen::Vector3d &bg_); 83 | 84 | void setGravity(const Eigen::Vector3d &g_); 85 | 86 | void setCovariance(const Eigen::Matrix &covariance_); 87 | 88 | Eigen::Matrix getCovariance(); 89 | 90 | Eigen::Vector3d getTranslation(); 91 | 92 | Eigen::Quaterniond getRotation(); 93 | 94 | Eigen::Vector3d getVelocity(); 95 | 96 | Eigen::Vector3d getBa(); 97 | 98 | Eigen::Vector3d getBg(); 99 | 100 | Eigen::Vector3d getGravity(); 101 | 102 | Eigen::Vector3d getLastAcc(); 103 | 104 | Eigen::Vector3d getLastGyr(); 105 | 106 | void predict(double dt_, const Eigen::Vector3d &acc_1_, const Eigen::Vector3d &gyr_1_); 107 | 108 | void observe(const Eigen::Matrix &d_x_); 109 | 110 | void observePose(const Eigen::Vector3d &translation, const Eigen::Quaterniond &rotation, double trans_noise = 0.001, double ang_noise = 0.001); 111 | 112 | void calculateLxly(); 113 | }; -------------------------------------------------------------------------------- /sr_lio/include/lioOptimization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // ros 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // eigen 21 | #include 22 | 23 | // pcl 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 "cloudMap.h" 37 | 38 | // cloud processing 39 | #include "cloudProcessing.h" 40 | 41 | // IMU processing 42 | #include "state.h" 43 | 44 | // eskf estimator 45 | #include "eskfEstimator.h" 46 | 47 | // utility 48 | #include "utility.h" 49 | #include "parameters.h" 50 | 51 | class cloudFrame 52 | { 53 | public: 54 | double time_sweep_begin, time_sweep_end; 55 | 56 | int id; // the index in all_cloud_frame 57 | int frame_id; 58 | 59 | state *p_state; 60 | 61 | std::vector point_frame; 62 | 63 | double offset_begin; 64 | double offset_end; 65 | double dt_offset; 66 | 67 | bool success; 68 | 69 | cloudFrame(std::vector &point_frame_, state *p_state_); 70 | 71 | cloudFrame(cloudFrame *p_cloud_frame); 72 | 73 | void release(); 74 | }; 75 | 76 | struct Neighborhood { 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | 79 | Eigen::Vector3d center = Eigen::Vector3d::Zero(); 80 | 81 | Eigen::Vector3d normal = Eigen::Vector3d::Zero(); 82 | 83 | Eigen::Matrix3d covariance = Eigen::Matrix3d::Identity(); 84 | 85 | double a2D = 1.0; // Planarity coefficient 86 | }; 87 | 88 | struct ResidualBlock { 89 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 90 | 91 | Eigen::Vector3d point_closest; 92 | 93 | Eigen::Vector3d pt_imu; 94 | 95 | Eigen::Vector3d normal; 96 | 97 | double alpha_time; 98 | 99 | double weight; 100 | }; 101 | 102 | class estimationSummary { 103 | 104 | public: 105 | 106 | int sample_size = 0; // The number of points sampled 107 | 108 | int number_of_residuals = 0; // The number of keypoints used for ICP registration 109 | 110 | int robust_level = 0; 111 | 112 | double distance_correction = 0.0; // The correction between the last frame's end, and the new frame's beginning 113 | 114 | double relative_distance = 0.0; // The distance between the beginning of the new frame and the end 115 | 116 | double relative_orientation = 0.0; // The distance between the beginning of the new frame and the end 117 | 118 | double ego_orientation = 0.0; // The angular distance between the beginning and the end of the frame 119 | 120 | bool success = true; // Whether the registration was a success 121 | 122 | std::string error_message; 123 | 124 | estimationSummary(); 125 | 126 | void release(); 127 | 128 | }; 129 | 130 | struct optimizeSummary { 131 | 132 | bool success = false; // Whether the registration succeeded 133 | 134 | int num_residuals_used = 0; 135 | 136 | std::string error_log; 137 | }; 138 | 139 | class lioOptimization{ 140 | 141 | private: 142 | 143 | ros::NodeHandle nh; 144 | 145 | ros::Publisher pub_cloud_body; // the registered cloud of cuurent sweep to be published for visualization 146 | ros::Publisher pub_cloud_world; // the cloud of global map to be published for visualization 147 | ros::Publisher pub_odom; // the pose of current sweep after LIO-optimization 148 | ros::Publisher pub_path; // the position of current sweep after LIO-optimization for visualization 149 | 150 | ros::Subscriber sub_cloud_ori; // the data of original point clouds from LiDAR sensor 151 | ros::Subscriber sub_imu_ori; // the data of original accelerometer and gyroscope from IMU sensor 152 | 153 | std::string lidar_topic; 154 | std::string imu_topic; 155 | 156 | cloudProcessing *cloud_pro; 157 | eskfEstimator *eskf_pro; 158 | 159 | bool extrin_enable; 160 | 161 | double laser_point_cov; 162 | 163 | std::vector v_G; 164 | std::vector v_extrin_t; 165 | std::vector v_extrin_R; 166 | 167 | Eigen::Matrix3d R_imu_lidar; 168 | Eigen::Vector3d t_imu_lidar; 169 | 170 | Eigen::Vector3d pose_lid; 171 | 172 | std::queue> time_buffer; 173 | std::queue::Ptr>> feature_buffer; 174 | 175 | std::queue> lidar_buffer; 176 | std::queue imu_buffer; 177 | 178 | std::vector all_cloud_frame; 179 | 180 | pcl::PointCloud::Ptr down_cloud_body; 181 | pcl::PointCloud::Ptr down_cloud_world; 182 | 183 | std::vector>> nearest_points; 184 | 185 | double last_time_lidar; 186 | double last_time_imu; 187 | double last_time_frame; 188 | double current_time; 189 | 190 | int index_frame; 191 | 192 | double time_max_solver; 193 | int num_max_iteration; 194 | 195 | // lidar range 196 | float det_range; 197 | double fov_deg; 198 | 199 | voxelHashMap voxel_map; 200 | 201 | voxelHashMap voxel_map_true; 202 | std::vector points_undecided; 203 | std::vector points_vec_true; 204 | 205 | odometryOptions options; 206 | 207 | geometry_msgs::Quaternion geoQuat; 208 | geometry_msgs::PoseStamped msg_body_pose; 209 | nav_msgs::Path path; 210 | nav_msgs::Odometry odomAftMapped; 211 | 212 | double dt_sum; 213 | 214 | std::vector>> imu_meas; 215 | std::vector imu_states; 216 | 217 | // loop closing 218 | ros::Publisher pub_loop_map; 219 | pcl::PointCloud::Ptr loop_map; 220 | 221 | void pubLocalMap(cloudFrame* p_frame); 222 | // loop closing 223 | 224 | public: 225 | 226 | // initialize class 227 | lioOptimization(); 228 | 229 | void readParameters(); 230 | 231 | void allocateMemory(); 232 | 233 | void initialValue(); 234 | // initialize class 235 | 236 | // get sensor data 237 | void standardCloudHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); 238 | 239 | void imuHandler(const sensor_msgs::Imu::ConstPtr &msg); 240 | // get sensor data 241 | 242 | // main loop 243 | std::vector, std::vector>, std::pair>> getMeasurements(); 244 | 245 | void process(std::vector &const_frame, double timestamp_begin, double timestamp_offset); 246 | 247 | void run(); 248 | // main loop 249 | 250 | // data handle and state estimation 251 | cloudFrame* buildFrame(std::vector &const_frame, state *cur_state, double timestamp_begin, double timestamp_offset); 252 | 253 | void makePointTimestamp(std::vector &sweep, double time_sweep_begin, double time_sweep_end); 254 | 255 | void stateInitialization(state *cur_state); 256 | 257 | optimizeSummary stateEstimation(cloudFrame *p_frame); 258 | 259 | optimizeSummary optimize(cloudFrame *p_frame, const icpOptions &cur_icp_options, double sample_voxel_size); 260 | 261 | optimizeSummary buildPlaneResiduals(const icpOptions &cur_icp_options, voxelHashMap &voxel_map_temp, std::vector &keypoints, std::vector &plane_residuals, cloudFrame *p_frame, double &loss_sum); 262 | 263 | optimizeSummary updateIEKF(const icpOptions &cur_icp_options, voxelHashMap &voxel_map_temp, std::vector &keypoints, cloudFrame *p_frame); 264 | 265 | Neighborhood computeNeighborhoodDistribution(std::vector &points); 266 | 267 | std::vector searchNeighbors(voxelHashMap &map, const Eigen::Vector3d &point, 268 | int nb_voxels_visited, double size_voxel_map, int max_num_neighbors, int threshold_voxel_capacity = 1, std::vector *voxels = nullptr); 269 | // data handle and state estimation 270 | 271 | // map update 272 | void addPointToMap(voxelHashMap &map, point3D &point3d, double voxel_size, int max_num_points_in_voxel, double min_distance_points, int min_num_points, cloudFrame* p_frame); 273 | 274 | void addPointsToMap(voxelHashMap &map, cloudFrame* p_frame, double voxel_size, int max_num_points_in_voxel, double min_distance_points, int min_num_points = 0); 275 | 276 | void removePointsFarFromLocation(voxelHashMap &map, const Eigen::Vector3d &location, double distance); 277 | 278 | size_t mapSize(const voxelHashMap &map); 279 | // map update 280 | 281 | // save result to device 282 | void recordSinglePose(cloudFrame *p_frame); 283 | // save result to device 284 | 285 | // publish result by ROS for visualization 286 | void publish_path(ros::Publisher pub_path,cloudFrame *p_frame); 287 | 288 | void set_posestamp(geometry_msgs::PoseStamped &body_pose_out,cloudFrame *p_frame); 289 | 290 | void addPointToPcl(pcl::PointCloud::Ptr pcl_points, globalPoint& point); 291 | void publishCLoudWorld(ros::Publisher & pub_cloud_world, pcl::PointCloud::Ptr laserCloudFullRes, cloudFrame* p_frame); 292 | 293 | pcl::PointCloud::Ptr points_world; 294 | void publish_odometry(const ros::Publisher & pubOdomAftMapped, cloudFrame *p_frame); 295 | // publish result by ROS for visualization 296 | 297 | // remove dynamic points 298 | void addPointToTrueMap(voxelHashMap &map, point3D &point3d, double voxel_size, int max_num_points_in_voxel, double min_distance_points, int min_num_points); 299 | 300 | void decidePoints(const icpOptions &cur_icp_options, cloudFrame *p_frame); 301 | 302 | void addDecidedPointsToTrueMap(double voxel_size, int max_num_points_in_voxel, double min_distance_points, int min_num_points = 0); 303 | 304 | void detectDynamicLabel(const icpOptions &cur_icp_options, voxelHashMap &voxel_map_temp, cloudFrame *p_frame); 305 | // remove dynamic points 306 | 307 | tf::TransformBroadcaster tfBroadcaster; 308 | tf::StampedTransform laserOdometryTrans; 309 | }; -------------------------------------------------------------------------------- /sr_lio/include/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | 5 | // utility 6 | #include "utility.h" 7 | 8 | class icpOptions { 9 | 10 | public: 11 | // The threshold on the voxel occupancy 12 | // To be considered in the neighbor search, a voxel must have at least threshold_voxel_occupancy points 13 | int threshold_voxel_occupancy = 1; 14 | 15 | int init_num_frames = 20; // The number of frames defining the initialization of the map 16 | 17 | double size_voxel_map = 1.0; //Max Voxel : -32767 to 32767 then 32km map for SIZE_VOXEL_MAP = 1m 18 | 19 | int num_iters_icp = 5; // The Maximum number of ICP iterations performed 20 | 21 | int min_number_neighbors = 20; 22 | 23 | int voxel_neighborhood = 1; // Visits the (3 * voxel_neighborhood)^3 neighboring voxels 24 | 25 | double power_planarity = 2.0; // The power of planarity defined in the weighting scheme 26 | 27 | // Whether to estimate the normal of the key point or the closest neighbor 28 | bool estimate_normal_from_neighborhood = true; 29 | 30 | int max_number_neighbors = 20; 31 | 32 | double max_dist_to_plane_icp = 0.3; // The maximum distance point-to-plane (OLD Version of ICP) 33 | 34 | double threshold_orientation_norm = 0.0001; // Threshold on rotation (deg) for ICP's stopping criterion 35 | 36 | double threshold_translation_norm = 0.001; // Threshold on translation (deg) for ICP's stopping criterion 37 | 38 | bool point_to_plane_with_distortion = true; // Whether to distort the frames at each ICP iteration 39 | 40 | int max_num_residuals = -1; // The maximum number of keypoints used 41 | 42 | int min_num_residuals = 100; // Below this number, CT_ICP will crash 43 | 44 | int num_closest_neighbors = 1; // The number of closest neighbors considered as residuals 45 | 46 | double weight_alpha = 0.9; 47 | 48 | double weight_neighborhood = 0.1; 49 | 50 | // Debug params 51 | bool debug_print = true; // Whether to output debug information to std::cout 52 | 53 | bool debug_viz = false; // Whether to pass the key points to the ExplorationEngine 54 | 55 | void recordParameters(); 56 | }; 57 | 58 | class odometryOptions { 59 | 60 | public: 61 | /* Parameters for initialization of the map */ 62 | double init_voxel_size = 0.2; 63 | 64 | double init_sample_voxel_size = 1.0; 65 | 66 | int init_num_frames = 20; // The number of frames defining the initialization of the map 67 | 68 | int num_for_initialization = 10; 69 | 70 | double voxel_size = 0.5; 71 | 72 | double sample_voxel_size = 1.5; 73 | 74 | double max_distance = 100.0; // The threshold on the voxel size to remove points from the map 75 | 76 | int max_num_points_in_voxel = 20; // The maximum number of points in a voxel 77 | 78 | double min_distance_points = 0.1; // The minimal distance between points in the map 79 | 80 | double distance_error_threshold = 5.0; // The Ego-Motion Distance considered as an error 81 | 82 | icpOptions optimize_options; 83 | 84 | MotionCompensation motion_compensation = CONSTANT_VELOCITY; 85 | 86 | StateInitialization initialization = INIT_CONSTANT_VELOCITY; 87 | 88 | static odometryOptions defaultDrivingProfile(); 89 | 90 | static odometryOptions defaultRobustOutdoorLowInertia(); 91 | 92 | static odometryOptions robustDrivingProfile(); 93 | 94 | void recordParameters(); 95 | 96 | }; -------------------------------------------------------------------------------- /sr_lio/include/state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | 5 | // eigen 6 | #include 7 | 8 | // ceres 9 | #include 10 | 11 | // utility 12 | #include "utility.h" 13 | 14 | class state 15 | { 16 | public: 17 | 18 | Eigen::Quaterniond rotation; 19 | Eigen::Vector3d translation; 20 | Eigen::Vector3d velocity; 21 | Eigen::Vector3d ba; 22 | Eigen::Vector3d bg; 23 | 24 | state(); 25 | 26 | state(const Eigen::Quaterniond &rotation_, const Eigen::Vector3d &translation_, 27 | const Eigen::Vector3d &velocity_, const Eigen::Vector3d& ba_, const Eigen::Vector3d& bg_); 28 | 29 | state(const state* state_temp, bool copy = false); 30 | 31 | void release(); 32 | }; -------------------------------------------------------------------------------- /sr_lio/include/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // c++ 3 | #include 4 | #include 5 | #include 6 | 7 | // ros 8 | #include 9 | 10 | // eigen 11 | #include 12 | 13 | // pcl 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // cloud processing 20 | #include "cloudProcessing.h" 21 | 22 | #include "cloudMap.h" 23 | 24 | #define THETA_THRESHOLD 0.0001 25 | #define MIN_INI_COUNT 10 26 | #define MIN_INI_TIME 3.0 27 | #define MAX_GYR_VAR 0.5 28 | #define MAX_ACC_VAR 0.6 29 | 30 | extern bool debug_output; 31 | extern std::string output_path; 32 | 33 | extern bool time_diff_enable; 34 | extern double time_diff; 35 | 36 | extern float mov_threshold; 37 | 38 | extern bool initial_flag; 39 | 40 | extern Eigen::Vector3d G; 41 | extern double G_norm; 42 | 43 | bool time_list(point3D &point_1, point3D &point_2); 44 | 45 | bool time_list_velodyne(velodyne_ros::Point &point_1, velodyne_ros::Point &point_2); 46 | 47 | bool time_list_robosense(robosense_ros::Point &point_1, robosense_ros::Point &point_2); 48 | 49 | bool time_list_ouster(ouster_ros::Point &point_1, ouster_ros::Point &point_2); 50 | 51 | void point3DtoPCL(std::vector &v_point_temp, pcl::PointCloud::Ptr &p_cloud_temp); 52 | 53 | Eigen::Matrix3d mat33FromArray(std::vector &array); 54 | 55 | Eigen::Vector3d vec3FromArray(std::vector &array); 56 | 57 | void pointBodyToWorld(pcl::PointXYZINormal const * const pi, pcl::PointXYZINormal * const po, Eigen::Matrix3d &R_world_cur, 58 | Eigen::Vector3d &t_world_cur, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar); 59 | 60 | void RGBpointLidarToIMU(pcl::PointXYZINormal const * const pi, pcl::PointXYZINormal * const po, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar); 61 | 62 | bool planeFitting(Eigen::Matrix &plane_parameter, const std::vector> &point, const double &threshold); 63 | 64 | double AngularDistance(const Eigen::Matrix3d &rota, const Eigen::Matrix3d &rotb); 65 | 66 | double AngularDistance(const Eigen::Quaterniond &q_a, const Eigen::Quaterniond &q_b); 67 | 68 | double AngularDistance(const Eigen::Vector3d &d_so3); 69 | 70 | float calculateDist2(pcl::PointXYZINormal point1, pcl::PointXYZINormal point2); 71 | 72 | void saveCutCloud(std::string &str, pcl::PointCloud::Ptr &p_cloud_temp); 73 | 74 | enum SizeParameterization 75 | { 76 | NUM_MATCH_POINTS = 20 77 | }; 78 | 79 | enum MotionCompensation 80 | { 81 | IMU = 0, 82 | CONSTANT_VELOCITY = 1 83 | }; 84 | 85 | enum StateInitialization 86 | { 87 | INIT_IMU = 0, 88 | INIT_CONSTANT_VELOCITY = 1 89 | }; 90 | 91 | class numType 92 | { 93 | public: 94 | template 95 | static Eigen::Matrix normalizeR(const Eigen::MatrixBase &R_in) 96 | { 97 | typedef typename Derived::Scalar Scalar_t; 98 | 99 | Eigen::Quaternion q(R_in); 100 | q.normalize(); 101 | return q.toRotationMatrix(); 102 | } 103 | 104 | template 105 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &mat) 106 | { 107 | Eigen::Matrix mat_skew; 108 | mat_skew << typename Derived::Scalar(0), -mat(2), mat(1), 109 | mat(2), typename Derived::Scalar(0), -mat(0), 110 | -mat(1), mat(0), typename Derived::Scalar(0); 111 | return mat_skew; 112 | } 113 | 114 | template 115 | static Eigen::Matrix derivativeS2(const Eigen::MatrixBase &g_in) 116 | { 117 | typedef typename Derived::Scalar Scalar_t; 118 | 119 | Eigen::Matrix B_x; 120 | 121 | Eigen::Matrix g = g_in; 122 | 123 | g.normalize(); 124 | 125 | B_x(0, 0) = static_cast(1.0) - g(0) * g(0) / (static_cast(1.0) + g(2)); 126 | B_x(0, 1) = - g(0) * g(1) / (static_cast(1.0) + g(2)); 127 | B_x(1, 0) = B_x(0, 1); 128 | B_x(1, 1) = static_cast(1.0) - g(1) * g(1) / (static_cast(1.0) + g(2)); 129 | B_x(2, 0) = - g(0); 130 | B_x(2, 1) = - g(1); 131 | 132 | return B_x; 133 | } 134 | 135 | template 136 | static Eigen::Matrix rotFromV1toV2(const Eigen::MatrixBase &v1_in, const Eigen::MatrixBase &v2_in) 137 | { 138 | typedef typename Derived::Scalar Scalar_t; 139 | 140 | Eigen::Matrix v1 = v1_in; 141 | Eigen::Matrix v2 = v2_in; 142 | 143 | v1.normalize(); 144 | v2.normalize(); 145 | 146 | Eigen::Matrix cross = v1.cross(v2); 147 | Scalar_t dot = v1.dot(v2); 148 | 149 | if (fabs(static_cast(1.0) - dot) < static_cast(1e-6)) 150 | return Eigen::Matrix::Identity(); 151 | 152 | Eigen::Matrix skew = skewSymmetric(cross); 153 | 154 | Eigen::Matrix R = Eigen::Matrix::Identity() + skew + skew * skew * (static_cast(1.0) - dot) 155 | / (cross(0) * cross(0) + cross(1) * cross(1) + cross(2) * cross(2)); 156 | 157 | return R; 158 | } 159 | 160 | template 161 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 162 | { 163 | return q; 164 | } 165 | 166 | template 167 | static Eigen::Matrix rotationToSo3(const Eigen::MatrixBase &R_in) 168 | { 169 | typedef typename Derived::Scalar Scalar_t; 170 | 171 | Eigen::Matrix R = normalizeR(R_in); 172 | Scalar_t theta = acos((R(0, 0) + R(1, 1) + R(2, 2) - static_cast(1.0)) / static_cast(2)); 173 | 174 | if (theta < static_cast(THETA_THRESHOLD)) 175 | return Eigen::Matrix(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)) / static_cast(2.0); 176 | else 177 | return theta * Eigen::Matrix(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)) / (static_cast(2.0) * sin(theta)); 178 | } 179 | 180 | template 181 | static Eigen::Matrix so3ToRotation(const Eigen::MatrixBase &so3_in) 182 | { 183 | typedef typename Derived::Scalar Scalar_t; 184 | 185 | Scalar_t theta = so3_in.norm(); 186 | 187 | if (theta < static_cast(THETA_THRESHOLD)) 188 | { 189 | Eigen::Matrix u_x = skewSymmetric(so3_in); 190 | return Eigen::Matrix::Identity() + u_x + static_cast(0.5) * u_x * u_x; 191 | } 192 | else 193 | { 194 | Eigen::Matrix3d u_x = skewSymmetric(so3_in.normalized()); 195 | return Eigen::Matrix::Identity() + sin(theta) * u_x + (static_cast(1) - cos(theta))* u_x * u_x; 196 | } 197 | } 198 | 199 | template 200 | static Eigen::Quaternion so3ToQuat(const Eigen::MatrixBase &so3_in) 201 | { 202 | typedef typename Derived::Scalar Scalar_t; 203 | 204 | Scalar_t theta = so3_in.norm(); 205 | 206 | if (theta < static_cast(THETA_THRESHOLD)) 207 | { 208 | Eigen::Matrix half_so3 = so3_in; 209 | half_so3 /= static_cast(2.0); 210 | 211 | Eigen::Quaternion q(static_cast(1.0), half_so3.x(), half_so3.y(), half_so3.z()); 212 | q.normalize(); 213 | return q; 214 | } 215 | else 216 | { 217 | Eigen::Matrix u = so3_in.normalized(); 218 | 219 | Eigen::Quaternion q(cos(static_cast(0.5) * theta), u.x() * sin(static_cast(0.5) * theta), 220 | u.y() * sin(static_cast(0.5) * theta), u.z() * sin(static_cast(0.5) * theta)); 221 | q.normalize(); 222 | return q; 223 | } 224 | } 225 | 226 | template 227 | static Eigen::Matrix quatToSo3(const Eigen::QuaternionBase &q_in) 228 | { 229 | return rotationToSo3(q_in.toRotationMatrix()); 230 | } 231 | 232 | template 233 | static Eigen::Matrix JleftSo3(const Eigen::MatrixBase &so3_in) 234 | { 235 | typedef typename Derived::Scalar Scalar_t; 236 | 237 | Scalar_t theta = so3_in.norm(); 238 | 239 | if(theta < static_cast(THETA_THRESHOLD)){ 240 | return Eigen::Matrix::Identity() + skewSymmetric(so3_in) / static_cast(2); 241 | } 242 | else 243 | { 244 | Eigen::Matrix u = so3_in.normalized(); 245 | return sin(theta) / theta * Eigen::Matrix::Identity() + (static_cast(1) - sin(theta) / theta) * u * u.transpose() + (static_cast(1) - cos(theta)) / theta * skewSymmetric(u); 246 | } 247 | } 248 | 249 | template 250 | static Eigen::Matrix invJleftSo3(const Eigen::MatrixBase &so3_in) 251 | { 252 | typedef typename Derived::Scalar Scalar_t; 253 | 254 | Scalar_t theta = so3_in.norm(); 255 | 256 | if(theta < static_cast(THETA_THRESHOLD)) 257 | { 258 | return cos(theta / static_cast(2)) * Eigen::Matrix::Identity() 259 | + static_cast(0.125) * so3_in * so3_in.transpose() - static_cast(0.5) * skewSymmetric(so3_in); 260 | } 261 | else 262 | { 263 | Eigen::Matrix u = so3_in.normalized(); 264 | return static_cast(0.5) * theta / tan(theta / static_cast(2)) * Eigen::Matrix::Identity() + 265 | (static_cast(1) - static_cast(0.5) * theta / tan(theta / static_cast(2))) * u * u.transpose() - static_cast(0.5) * skewSymmetric(so3_in); 266 | } 267 | } 268 | 269 | template 270 | static Eigen::Matrix JrightSo3(const Eigen::MatrixBase &so3_in) 271 | { 272 | typedef typename Derived::Scalar Scalar_t; 273 | 274 | Scalar_t theta = so3_in.norm(); 275 | 276 | if (theta < static_cast(THETA_THRESHOLD)) 277 | return Eigen::Matrix::Identity() - static_cast(0.5) * skewSymmetric(so3_in); 278 | else 279 | { 280 | Eigen::Matrix u = so3_in.normalized(); 281 | return sin(theta) / theta * Eigen::Matrix::Identity() + (static_cast(1) - sin(theta) / theta) * u * u.transpose() 282 | - (static_cast(1) - cos(theta)) / theta * skewSymmetric(u); 283 | } 284 | } 285 | 286 | template 287 | static Eigen::Matrix invJrightSo3(const Eigen::MatrixBase &so3_in) 288 | { 289 | typedef typename Derived::Scalar Scalar_t; 290 | 291 | Scalar_t theta = so3_in.norm(); 292 | 293 | if(theta < static_cast(THETA_THRESHOLD)) 294 | return cos(theta / static_cast(2)) * Eigen::Matrix::Identity() + static_cast(0.125) * so3_in * so3_in.transpose() + static_cast(0.5) * skewSymmetric(so3_in); 295 | else 296 | { 297 | Eigen::Matrix u = so3_in.normalized(); 298 | return static_cast(0.5) * theta / tan(theta / static_cast(2)) * Eigen::Matrix3d::Identity() 299 | + (static_cast(1) - static_cast(0.5) * theta / tan(theta / static_cast(2))) * u * u.transpose() + static_cast(0.5) * skewSymmetric(so3_in); 300 | } 301 | } 302 | }; 303 | 304 | void subSampleFrame(std::vector &frame, double size_voxel); 305 | 306 | void gridSampling(const std::vector &frame, std::vector &keypoints, double size_voxel_subsampling); 307 | 308 | void distortFrameByConstant(std::vector &points, std::vector &imu_states, double time_frame_begin, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar); 309 | 310 | void distortFrameByImu(std::vector &points, std::vector &imu_states, double time_frame_begin, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar); 311 | 312 | void transformPoint(point3D &point_temp, Eigen::Quaterniond &q_end, Eigen::Vector3d &t_end, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar); 313 | 314 | void transformAllImuPoint(std::vector &points, std::vector &imu_states, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar); -------------------------------------------------------------------------------- /sr_lio/launch/lio_ulhk_CA.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /sr_lio/launch/lio_urban_nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /sr_lio/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sr_lio 4 | 0.0.0 5 | The sr_lio package 6 | 7 | 8 | 9 | 10 | wqj 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 | eigen_conversions 53 | geometry_msgs 54 | nav_msgs 55 | pcl_conversions 56 | pcl_ros 57 | roscpp 58 | rospy 59 | sensor_msgs 60 | std_msgs 61 | tf 62 | eigen_conversions 63 | geometry_msgs 64 | nav_msgs 65 | pcl_conversions 66 | pcl_ros 67 | roscpp 68 | rospy 69 | sensor_msgs 70 | std_msgs 71 | tf 72 | eigen_conversions 73 | geometry_msgs 74 | nav_msgs 75 | pcl_conversions 76 | pcl_ros 77 | roscpp 78 | rospy 79 | sensor_msgs 80 | std_msgs 81 | tf 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /sr_lio/rviz_cfg/visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /mapping1 9 | - /mapping1/surround1 10 | - /mapping1/currPoints1 11 | - /mapping1/currPoints1/Autocompute Value Bounds1 12 | - /Odometry1/Odometry1 13 | - /Odometry1/Odometry1/Shape1 14 | - /Odometry1/Odometry1/Covariance1 15 | - /Odometry1/Odometry1/Covariance1/Position1 16 | - /Odometry1/Odometry1/Covariance1/Orientation1 17 | - /Path1 18 | - /MarkerArray1/Namespaces1 19 | - /Path2 20 | Splitter Ratio: 0.6432291865348816 21 | Tree Height: 777 22 | - Class: rviz/Selection 23 | Name: Selection 24 | - Class: rviz/Tool Properties 25 | Expanded: 26 | - /2D Pose Estimate1 27 | - /2D Nav Goal1 28 | - /Publish Point1 29 | Name: Tool Properties 30 | Splitter Ratio: 0.5886790156364441 31 | - Class: rviz/Views 32 | Expanded: 33 | - /Current View1 34 | Name: Views 35 | Splitter Ratio: 0.5 36 | - Class: rviz/Time 37 | Experimental: false 38 | Name: Time 39 | SyncMode: 0 40 | SyncSource: "" 41 | Preferences: 42 | PromptSaveOnExit: true 43 | Toolbars: 44 | toolButtonStyle: 2 45 | Visualization Manager: 46 | Class: "" 47 | Displays: 48 | - Alpha: 1 49 | Cell Size: 1000 50 | Class: rviz/Grid 51 | Color: 160; 160; 164 52 | Enabled: false 53 | Line Style: 54 | Line Width: 0.029999999329447746 55 | Value: Lines 56 | Name: Grid 57 | Normal Cell Count: 0 58 | Offset: 59 | X: 0 60 | Y: 0 61 | Z: 0 62 | Plane: XY 63 | Plane Cell Count: 40 64 | Reference Frame: 65 | Value: false 66 | - Alpha: 1 67 | Class: rviz/Axes 68 | Enabled: false 69 | Length: 0.699999988079071 70 | Name: Axes 71 | Radius: 0.05999999865889549 72 | Reference Frame: 73 | Value: false 74 | - Class: rviz/Group 75 | Displays: 76 | - Alpha: 1 77 | Autocompute Intensity Bounds: true 78 | Autocompute Value Bounds: 79 | Max Value: 10 80 | Min Value: -10 81 | Value: true 82 | Axis: Z 83 | Channel Name: intensity 84 | Class: rviz/PointCloud2 85 | Color: 238; 238; 236 86 | Color Transformer: Intensity 87 | Decay Time: 0 88 | Enabled: false 89 | Invert Rainbow: false 90 | Max Color: 255; 255; 255 91 | Min Color: 238; 238; 236 92 | Name: surround 93 | Position Transformer: XYZ 94 | Queue Size: 1 95 | Selectable: false 96 | Size (Pixels): 1 97 | Size (m): 0.05000000074505806 98 | Style: Points 99 | Topic: /cloud_global_map 100 | Unreliable: false 101 | Use Fixed Frame: true 102 | Use rainbow: true 103 | Value: false 104 | - Alpha: 1 105 | Autocompute Intensity Bounds: true 106 | Autocompute Value Bounds: 107 | Max Value: 12.2799654006958 108 | Min Value: -3.8865151405334473 109 | Value: true 110 | Axis: Z 111 | Channel Name: intensity 112 | Class: rviz/PointCloud2 113 | Color: 255; 255; 255 114 | Color Transformer: AxisColor 115 | Decay Time: 10000000272564224 116 | Enabled: true 117 | Invert Rainbow: true 118 | Max Color: 255; 255; 255 119 | Min Color: 0; 0; 0 120 | Name: currPoints 121 | Position Transformer: XYZ 122 | Queue Size: 100000 123 | Selectable: true 124 | Size (Pixels): 1 125 | Size (m): 0.009999999776482582 126 | Style: Points 127 | Topic: /cloud_global_map 128 | Unreliable: false 129 | Use Fixed Frame: true 130 | Use rainbow: true 131 | Value: true 132 | - Alpha: 1 133 | Autocompute Intensity Bounds: true 134 | Autocompute Value Bounds: 135 | Max Value: 10 136 | Min Value: -10 137 | Value: true 138 | Axis: Z 139 | Channel Name: intensity 140 | Class: rviz/PointCloud2 141 | Color: 255; 0; 0 142 | Color Transformer: FlatColor 143 | Decay Time: 0 144 | Enabled: false 145 | Invert Rainbow: false 146 | Max Color: 255; 255; 255 147 | Min Color: 0; 0; 0 148 | Name: PointCloud2 149 | Position Transformer: XYZ 150 | Queue Size: 10 151 | Selectable: true 152 | Size (Pixels): 3 153 | Size (m): 0.10000000149011612 154 | Style: Flat Squares 155 | Topic: /Laser_map 156 | Unreliable: false 157 | Use Fixed Frame: true 158 | Use rainbow: true 159 | Value: false 160 | Enabled: true 161 | Name: mapping 162 | - Class: rviz/Group 163 | Displays: 164 | - Angle Tolerance: 0.009999999776482582 165 | Class: rviz/Odometry 166 | Covariance: 167 | Orientation: 168 | Alpha: 0.5 169 | Color: 255; 255; 127 170 | Color Style: Unique 171 | Frame: Local 172 | Offset: 1 173 | Scale: 1 174 | Value: true 175 | Position: 176 | Alpha: 0.30000001192092896 177 | Color: 204; 51; 204 178 | Scale: 1 179 | Value: true 180 | Value: true 181 | Enabled: true 182 | Keep: 1 183 | Name: Odometry 184 | Position Tolerance: 0.0010000000474974513 185 | Queue Size: 10 186 | Shape: 187 | Alpha: 1 188 | Axes Length: 1 189 | Axes Radius: 0.20000000298023224 190 | Color: 255; 85; 0 191 | Head Length: 0 192 | Head Radius: 0 193 | Shaft Length: 0.05000000074505806 194 | Shaft Radius: 0.05000000074505806 195 | Value: Axes 196 | Topic: /Odometry_after_opt 197 | Unreliable: false 198 | Value: true 199 | Enabled: true 200 | Name: Odometry 201 | - Alpha: 1 202 | Class: rviz/Axes 203 | Enabled: true 204 | Length: 0.699999988079071 205 | Name: Axes 206 | Radius: 0.10000000149011612 207 | Reference Frame: 208 | Value: true 209 | - Alpha: 0 210 | Buffer Length: 2 211 | Class: rviz/Path 212 | Color: 25; 255; 255 213 | Enabled: true 214 | Head Diameter: 0 215 | Head Length: 0 216 | Length: 0.30000001192092896 217 | Line Style: Billboards 218 | Line Width: 1 219 | Name: Path 220 | Offset: 221 | X: 0 222 | Y: 0 223 | Z: 0 224 | Pose Color: 25; 255; 255 225 | Pose Style: None 226 | Queue Size: 10 227 | Radius: 0.029999999329447746 228 | Shaft Diameter: 0.4000000059604645 229 | Shaft Length: 0.4000000059604645 230 | Topic: /path 231 | Unreliable: false 232 | Value: true 233 | - Alpha: 1 234 | Autocompute Intensity Bounds: false 235 | Autocompute Value Bounds: 236 | Max Value: 10 237 | Min Value: -10 238 | Value: true 239 | Axis: Z 240 | Channel Name: intensity 241 | Class: rviz/PointCloud2 242 | Color: 255; 255; 255 243 | Color Transformer: Intensity 244 | Decay Time: 0 245 | Enabled: false 246 | Invert Rainbow: false 247 | Max Color: 239; 41; 41 248 | Max Intensity: 0 249 | Min Color: 239; 41; 41 250 | Min Intensity: 0 251 | Name: PointCloud2 252 | Position Transformer: XYZ 253 | Queue Size: 10 254 | Selectable: true 255 | Size (Pixels): 4 256 | Size (m): 0.30000001192092896 257 | Style: Spheres 258 | Topic: /cloud_effected 259 | Unreliable: false 260 | Use Fixed Frame: true 261 | Use rainbow: true 262 | Value: false 263 | - Alpha: 1 264 | Autocompute Intensity Bounds: true 265 | Autocompute Value Bounds: 266 | Max Value: 13.139549255371094 267 | Min Value: -32.08251953125 268 | Value: true 269 | Axis: Z 270 | Channel Name: intensity 271 | Class: rviz/PointCloud2 272 | Color: 138; 226; 52 273 | Color Transformer: FlatColor 274 | Decay Time: 0 275 | Enabled: false 276 | Invert Rainbow: false 277 | Max Color: 138; 226; 52 278 | Min Color: 138; 226; 52 279 | Name: PointCloud2 280 | Position Transformer: XYZ 281 | Queue Size: 10 282 | Selectable: true 283 | Size (Pixels): 3 284 | Size (m): 0.10000000149011612 285 | Style: Flat Squares 286 | Topic: /Laser_map 287 | Unreliable: false 288 | Use Fixed Frame: true 289 | Use rainbow: true 290 | Value: false 291 | - Class: rviz/MarkerArray 292 | Enabled: false 293 | Marker Topic: /MarkerArray 294 | Name: MarkerArray 295 | Namespaces: 296 | {} 297 | Queue Size: 100 298 | Value: false 299 | - Alpha: 1 300 | Buffer Length: 1 301 | Class: rviz/Path 302 | Color: 25; 255; 0 303 | Enabled: true 304 | Head Diameter: 0.30000001192092896 305 | Head Length: 0.20000000298023224 306 | Length: 0.30000001192092896 307 | Line Style: Billboards 308 | Line Width: 1 309 | Name: Path 310 | Offset: 311 | X: 0 312 | Y: 0 313 | Z: 0 314 | Pose Color: 255; 85; 255 315 | Pose Style: None 316 | Queue Size: 10 317 | Radius: 0.029999999329447746 318 | Shaft Diameter: 0.10000000149011612 319 | Shaft Length: 0.10000000149011612 320 | Topic: /aft_pgo_path 321 | Unreliable: false 322 | Value: true 323 | Enabled: true 324 | Global Options: 325 | Background Color: 0; 0; 0 326 | Default Light: true 327 | Fixed Frame: camera_init 328 | Frame Rate: 10 329 | Name: root 330 | Tools: 331 | - Class: rviz/Interact 332 | Hide Inactive Objects: true 333 | - Class: rviz/MoveCamera 334 | - Class: rviz/Select 335 | - Class: rviz/FocusCamera 336 | - Class: rviz/Measure 337 | - Class: rviz/SetInitialPose 338 | Theta std deviation: 0.2617993950843811 339 | Topic: /initialpose 340 | X std deviation: 0.5 341 | Y std deviation: 0.5 342 | - Class: rviz/SetGoal 343 | Topic: /move_base_simple/goal 344 | - Class: rviz/PublishPoint 345 | Single click: true 346 | Topic: /clicked_point 347 | Value: true 348 | Views: 349 | Current: 350 | Class: rviz/Orbit 351 | Distance: 106.12321472167969 352 | Enable Stereo Rendering: 353 | Stereo Eye Separation: 0.05999999865889549 354 | Stereo Focal Distance: 1 355 | Swap Stereo Eyes: false 356 | Value: false 357 | Field of View: 0.7853981852531433 358 | Focal Point: 359 | X: -4.982542037963867 360 | Y: -15.83572006225586 361 | Z: -3.063523054122925 362 | Focal Shape Fixed Size: true 363 | Focal Shape Size: 0.05000000074505806 364 | Invert Z Axis: false 365 | Name: Current View 366 | Near Clip Distance: 0.009999999776482582 367 | Pitch: 1.2597960233688354 368 | Target Frame: global 369 | Yaw: 1.4871824979782104 370 | Saved: ~ 371 | Window Geometry: 372 | Displays: 373 | collapsed: false 374 | Height: 1016 375 | Hide Left Dock: false 376 | Hide Right Dock: true 377 | QMainWindow State: 000000ff00000000fd0000000400000000000001c800000346fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000346000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000056a0000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 378 | Selection: 379 | collapsed: false 380 | Time: 381 | collapsed: false 382 | Tool Properties: 383 | collapsed: false 384 | Views: 385 | collapsed: true 386 | Width: 1848 387 | X: 72 388 | Y: 27 389 | -------------------------------------------------------------------------------- /sr_lio/src/cloudMap.cpp: -------------------------------------------------------------------------------- 1 | #include "cloudMap.h" 2 | 3 | globalPoint::globalPoint() 4 | { 5 | 6 | } 7 | 8 | void globalPoint::setPosition(Eigen::Vector3d &position_) 9 | { 10 | position = position_.cast(); 11 | } 12 | 13 | void globalPoint::setLabel(int label_) 14 | { 15 | label = (short)label_; 16 | } 17 | 18 | void globalPoint::setDynamic(bool is_dynamic_) 19 | { 20 | is_dynamic = is_dynamic_; 21 | } 22 | 23 | Eigen::Vector3d globalPoint::getPosition() 24 | { 25 | return position.cast(); 26 | } 27 | 28 | int globalPoint::getLabel() 29 | { 30 | return (int)label; 31 | } 32 | 33 | bool globalPoint::isDynamic() 34 | { 35 | return is_dynamic; 36 | } -------------------------------------------------------------------------------- /sr_lio/src/eskfEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "eskfEstimator.h" 2 | 3 | eskfEstimator::eskfEstimator() 4 | { 5 | noise = Eigen::Matrix::Zero(); 6 | delta_state = Eigen::Matrix::Zero(); 7 | covariance = Eigen::Matrix::Identity(); 8 | 9 | p = Eigen::Vector3d::Zero(); 10 | q = Eigen::Quaterniond::Identity(); 11 | v = Eigen::Vector3d::Zero(); 12 | ba = Eigen::Vector3d::Zero(); 13 | bg = Eigen::Vector3d::Zero(); 14 | g << 0.0, 0.0, 9.81; 15 | 16 | mean_gyr = Eigen::Vector3d(0, 0, 0); 17 | mean_acc = Eigen::Vector3d(0, 0, 9.81); 18 | 19 | is_first_imu_meas = true; 20 | num_init_meas = 1; 21 | } 22 | 23 | void eskfEstimator::setAccCov(double para) 24 | { 25 | acc_cov_scale << para, para, para; 26 | } 27 | 28 | void eskfEstimator::setGyrCov(double para) 29 | { 30 | gyr_cov_scale << para, para, para; 31 | } 32 | 33 | void eskfEstimator::setBiasAccCov(double para) 34 | { 35 | b_acc_cov << para, para, para; 36 | } 37 | 38 | void eskfEstimator::setBiasGyrCov(double para) 39 | { 40 | b_gyr_cov << para, para, para; 41 | } 42 | 43 | void eskfEstimator::tryInit(const std::vector>> &imu_meas) 44 | { 45 | initialization(imu_meas); 46 | 47 | if (num_init_meas > MIN_INI_COUNT && imu_meas.back().first - time_first_imu > MIN_INI_TIME) 48 | { 49 | acc_cov *= std::pow(G_norm / mean_acc.norm(), 2); 50 | 51 | if (gyr_cov.norm() > MAX_GYR_VAR) 52 | { 53 | LOG(ERROR) << "Too large noise of gyroscope measurements. " << gyr_cov.norm() << " > " << MAX_GYR_VAR; 54 | return; 55 | } 56 | 57 | if (acc_cov.norm() > MAX_ACC_VAR) 58 | { 59 | LOG(ERROR) << "Too large noise of accelerometer measurements. " << acc_cov.norm() << " > " << MAX_ACC_VAR; 60 | return; 61 | } 62 | 63 | initial_flag = true; 64 | 65 | gyr_cov = gyr_cov_scale; 66 | acc_cov = acc_cov_scale; 67 | 68 | Eigen::Vector3d init_bg = mean_gyr; 69 | Eigen::Vector3d init_gravity = mean_acc / mean_acc.norm() * G_norm; 70 | 71 | setBg(init_bg); 72 | setGravity(init_gravity); 73 | 74 | covariance.block<3, 3>(9, 9) *= 0.001; 75 | covariance.block<3, 3>(12, 12) *= 0.0001; 76 | covariance.block<2, 2>(15, 15) *= 0.00001; 77 | 78 | initializeNoise(); 79 | 80 | ROS_INFO("IMU Initialization Done."); 81 | 82 | std::cout << "init_gravity = " << init_gravity.transpose() << std::endl; 83 | std::cout << "init_bg = " << init_bg.transpose() << std::endl; 84 | } 85 | else 86 | ROS_INFO("Wait more IMU measurements..."); 87 | 88 | return; 89 | } 90 | 91 | void eskfEstimator::initialization(const std::vector>> &imu_meas) 92 | { 93 | if (is_first_imu_meas) 94 | { 95 | num_init_meas = 1; 96 | is_first_imu_meas = false; 97 | time_first_imu = imu_meas.front().first; 98 | mean_gyr = imu_meas.front().second.first; 99 | mean_acc = imu_meas.front().second.second; 100 | } 101 | 102 | for (const auto &imu : imu_meas) 103 | { 104 | mean_gyr += (imu.second.first - mean_gyr) / num_init_meas; 105 | mean_acc += (imu.second.second - mean_acc) / num_init_meas; 106 | 107 | gyr_cov = gyr_cov * (num_init_meas - 1.0) / num_init_meas 108 | + (imu.second.first - mean_gyr).cwiseProduct(imu.second.first - mean_gyr) * (num_init_meas - 1.0) / (num_init_meas * num_init_meas); 109 | 110 | acc_cov = acc_cov * (num_init_meas - 1.0) / num_init_meas 111 | + (imu.second.second - mean_acc).cwiseProduct(imu.second.second - mean_acc) * (num_init_meas - 1.0) / (num_init_meas * num_init_meas); 112 | 113 | num_init_meas++; 114 | } 115 | 116 | gyr_0 = imu_meas.back().second.first; 117 | acc_0 = imu_meas.back().second.second; 118 | } 119 | 120 | void eskfEstimator::initializeNoise() 121 | { 122 | noise.block<3, 3>(0, 0).diagonal() = acc_cov; 123 | noise.block<3, 3>(3, 3).diagonal() = gyr_cov; 124 | noise.block<3, 3>(6, 6).diagonal() = b_acc_cov; 125 | noise.block<3, 3>(9, 9).diagonal() = b_gyr_cov; 126 | } 127 | 128 | void eskfEstimator::initializeImuData(const Eigen::Vector3d &acc_0_, const Eigen::Vector3d &gyr_0_) 129 | { 130 | acc_0 = acc_0_; 131 | gyr_0 = gyr_0_; 132 | } 133 | 134 | void eskfEstimator::setTranslation(const Eigen::Vector3d &p_) { p = p_; } 135 | 136 | void eskfEstimator::setRotation(const Eigen::Quaterniond &q_) { q = q_; } 137 | 138 | void eskfEstimator::setVelocity(const Eigen::Vector3d &v_) { v = v_; } 139 | 140 | void eskfEstimator::setBa(const Eigen::Vector3d &ba_) { ba = ba_; } 141 | 142 | void eskfEstimator::setBg(const Eigen::Vector3d &bg_) { bg = bg_; } 143 | 144 | void eskfEstimator::setGravity(const Eigen::Vector3d &g_) { g = g_; } 145 | 146 | Eigen::Vector3d eskfEstimator::getTranslation() { return p; } 147 | 148 | Eigen::Quaterniond eskfEstimator::getRotation() { return q; } 149 | 150 | Eigen::Vector3d eskfEstimator::getVelocity() { return v; } 151 | 152 | Eigen::Vector3d eskfEstimator::getBa() { return ba; } 153 | 154 | Eigen::Vector3d eskfEstimator::getBg() { return bg; } 155 | 156 | Eigen::Vector3d eskfEstimator::getGravity() { return g; } 157 | 158 | Eigen::Vector3d eskfEstimator::getLastAcc() { return acc_0; } 159 | 160 | Eigen::Vector3d eskfEstimator::getLastGyr() { return gyr_0; } 161 | 162 | void eskfEstimator::setCovariance(const Eigen::Matrix &covariance_) { covariance = covariance_; } 163 | 164 | Eigen::Matrix eskfEstimator::getCovariance() { return covariance; } 165 | 166 | void eskfEstimator::predict(double dt_, const Eigen::Vector3d &acc_1_, const Eigen::Vector3d &gyr_1_) 167 | { 168 | dt = dt_; 169 | acc_1 = acc_1_; 170 | gyr_1 = gyr_1_; 171 | 172 | Eigen::Vector3d avr_acc = 0.5 * (acc_0 + acc_1); 173 | Eigen::Vector3d avr_gyr = 0.5 * (gyr_0 + gyr_1); 174 | 175 | Eigen::Vector3d p_before = p; 176 | Eigen::Quaterniond q_before = q; 177 | Eigen::Vector3d v_before = v; 178 | Eigen::Vector3d ba_before = ba; 179 | Eigen::Vector3d bg_before = bg; 180 | Eigen::Vector3d g_before = g; 181 | 182 | Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + gyr_1) - bg; 183 | Eigen::Vector3d un_acc = 0.5 * (acc_0 + acc_1) - ba; 184 | q = q * numType::so3ToQuat(un_gyr * dt); 185 | p = p + v * dt; 186 | v = v + q_before.toRotationMatrix() * un_acc * dt - g * dt; 187 | 188 | Eigen::Matrix3d R_omega_x, R_acc_x; 189 | R_omega_x << 0, -un_gyr(2), un_gyr(1), un_gyr(2), 0, -un_gyr(0), -un_gyr(1), un_gyr(0), 0; 190 | R_acc_x << 0, -un_acc(2), un_acc(1), un_acc(2), 0, -un_acc(0), -un_acc(1), un_acc(0), 0; 191 | 192 | Eigen::Matrix B_x = numType::derivativeS2(g); 193 | 194 | Eigen::Matrix F_x = Eigen::MatrixXd::Zero(17, 17); 195 | F_x.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 196 | F_x.block<3, 3>(0, 6) = Eigen::Matrix3d::Identity() * dt; 197 | F_x.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() - R_omega_x * dt; 198 | F_x.block<3, 3>(3, 12) = - Eigen::Matrix3d::Identity() * dt; 199 | F_x.block<3, 3>(6, 3) = - q_before.toRotationMatrix() * R_acc_x * dt; 200 | F_x.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity(); 201 | F_x.block<3, 3>(6, 9) = - q_before.toRotationMatrix() * dt; 202 | F_x.block<3, 2>(6, 15) = numType::skewSymmetric(g) * B_x * dt; 203 | F_x.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity(); 204 | F_x.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity(); 205 | F_x.block<2, 2>(15, 15) = - 1.0 / (g.norm() * g.norm()) * B_x.transpose() * numType::skewSymmetric(g) * numType::skewSymmetric(g) * B_x; 206 | 207 | Eigen::Matrix F_w = Eigen::MatrixXd::Zero(17, 12); 208 | F_w.block<3, 3>(6, 0) = - q_before.toRotationMatrix() * dt; 209 | F_w.block<3, 3>(3, 3) = - Eigen::Matrix3d::Identity() * dt; 210 | F_w.block<3, 3>(9, 6) = - Eigen::Matrix3d::Identity() * dt; 211 | F_w.block<3, 3>(12, 9) = - Eigen::Matrix3d::Identity() * dt; 212 | 213 | covariance = F_x * covariance * F_x.transpose() + F_w * noise * F_w.transpose(); 214 | 215 | acc_0 = acc_1; 216 | gyr_0 = gyr_1; 217 | } 218 | 219 | void eskfEstimator::observe(const Eigen::Matrix &d_x_) 220 | { 221 | p = p + d_x_.head<3>(); 222 | q = (q * numType::so3ToQuat(d_x_.segment<3>(3))).normalized(); 223 | v = v + d_x_.segment<3>(6); 224 | ba = ba + d_x_.segment<3>(9); 225 | bg = bg + d_x_.segment<3>(12); 226 | 227 | Eigen::Matrix B_x = numType::derivativeS2(g); 228 | Eigen::Vector3d so3_dg = B_x * d_x_.tail<2>(); 229 | g = numType::so3ToRotation(so3_dg) * g; 230 | } 231 | 232 | void eskfEstimator::observePose(const Eigen::Vector3d &translation, const Eigen::Quaterniond &rotation, double trans_noise, double ang_noise) 233 | { 234 | Eigen::Matrix H = Eigen::Matrix::Zero(); 235 | H.block<3, 3>(0, 0) = Eigen::MatrixXd::Identity(3, 3); 236 | Eigen::Vector3d so3 = numType::quatToSo3(q); 237 | H.block<3, 3>(3, 3) = numType::invJrightSo3(so3); // gaobo: Identity(); 238 | 239 | Eigen::Matrix noise_vec; 240 | noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise; 241 | 242 | Eigen::Matrix V = noise_vec.asDiagonal(); 243 | Eigen::Matrix K = covariance * H.transpose() * (H * covariance * H.transpose() + V).inverse(); 244 | 245 | 246 | Eigen::Quaterniond update_q = q.inverse() * rotation; 247 | Eigen::Vector3d update_so3 = numType::quatToSo3(update_q); 248 | Eigen::Vector3d update_t = translation - p; 249 | 250 | Eigen::Matrix update_vec = Eigen::Matrix::Zero(); 251 | update_vec.head<3>() = update_t; 252 | update_vec.tail<3>() = update_so3; 253 | 254 | Eigen::Matrix predict_vec = Eigen::Matrix::Zero(); 255 | 256 | delta_state = predict_vec + K * update_vec; 257 | covariance = (Eigen::MatrixXd::Identity(17, 17) - K * H) * covariance; 258 | 259 | updateAndReset(); 260 | } 261 | 262 | void eskfEstimator::updateAndReset() 263 | { 264 | p = p + delta_state.block<3, 1>(0, 0); 265 | q = q * numType::so3ToQuat(delta_state.block<3, 1>(3, 0)); 266 | v = v + delta_state.block<3, 1>(6, 0); 267 | ba = ba + delta_state.block<3, 1>(9, 0); 268 | bg = bg + delta_state.block<3, 1>(12, 0); 269 | 270 | g = g + lxly * delta_state.block<2, 1>(15, 0); 271 | calculateLxly(); 272 | 273 | projectCovariance(); 274 | 275 | delta_state.setZero(); 276 | } 277 | 278 | void eskfEstimator::projectCovariance() 279 | { 280 | Eigen::Matrix J; 281 | J.block<17, 17>(0, 0) = Eigen::MatrixXd::Identity(17, 17); 282 | J.block<3, 3>(3, 3) = Eigen::MatrixXd::Identity(3, 3) - 0.5 * numType::skewSymmetric(delta_state.block<3, 1>(3, 0)); 283 | covariance = J * covariance * J.transpose(); 284 | } 285 | 286 | void eskfEstimator::calculateLxly() 287 | { 288 | Eigen::Vector3d b, c; 289 | Eigen::Vector3d a = g.normalized(); 290 | Eigen::Vector3d temp(0, 0, 1); 291 | if(a == temp) 292 | temp << 1, 0, 0; 293 | b = (temp - a * (a.transpose() * temp)).normalized(); 294 | c = a.cross(b); 295 | 296 | lxly.block<3, 1>(0, 0) = b; 297 | lxly.block<3, 1>(0, 1) = c; 298 | } 299 | -------------------------------------------------------------------------------- /sr_lio/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | odometryOptions odometryOptions::defaultDrivingProfile() { 4 | return odometryOptions{}; 5 | } 6 | 7 | odometryOptions odometryOptions::robustDrivingProfile() 8 | { 9 | odometryOptions default_options; 10 | 11 | default_options.voxel_size = 0.5; 12 | default_options.sample_voxel_size = 1.5; 13 | default_options.max_distance = 200.0; 14 | default_options.min_distance_points = 0.15; 15 | default_options.init_num_frames = 20; 16 | default_options.num_for_initialization = 10; 17 | default_options.max_num_points_in_voxel = 20; 18 | default_options.min_distance_points = 0.05; 19 | default_options.distance_error_threshold = 5.0; 20 | default_options.motion_compensation = CONSTANT_VELOCITY; 21 | default_options.initialization = INIT_CONSTANT_VELOCITY; 22 | 23 | auto &optimize_options = default_options.optimize_options; 24 | optimize_options.debug_print = false; 25 | optimize_options.init_num_frames = 40; 26 | optimize_options.max_number_neighbors = 20; 27 | optimize_options.min_number_neighbors = 20; 28 | optimize_options.num_iters_icp = 15; 29 | optimize_options.max_dist_to_plane_icp = 0.5; 30 | optimize_options.threshold_orientation_norm = 0.1; 31 | optimize_options.threshold_orientation_norm = 0.01; 32 | optimize_options.num_closest_neighbors = 1; 33 | 34 | return default_options; 35 | } 36 | 37 | odometryOptions odometryOptions::defaultRobustOutdoorLowInertia() 38 | { 39 | odometryOptions default_options; 40 | default_options.voxel_size = 0.3; 41 | default_options.sample_voxel_size = 1.5; 42 | default_options.min_distance_points = 0.1; 43 | default_options.max_distance = 200.0; 44 | default_options.init_num_frames = 20; 45 | default_options.num_for_initialization = 10; 46 | default_options.max_num_points_in_voxel = 20; 47 | default_options.distance_error_threshold = 5.0; 48 | default_options.motion_compensation = CONSTANT_VELOCITY; 49 | default_options.initialization = INIT_CONSTANT_VELOCITY; 50 | 51 | auto &optimize_options = default_options.optimize_options; 52 | optimize_options.size_voxel_map = 0.8; 53 | optimize_options.num_iters_icp = 30; 54 | optimize_options.threshold_voxel_occupancy = 5; 55 | optimize_options.min_number_neighbors = 20; 56 | optimize_options.voxel_neighborhood = 1; 57 | 58 | optimize_options.init_num_frames = 20; 59 | optimize_options.max_number_neighbors = 20; 60 | optimize_options.min_number_neighbors = 20; 61 | optimize_options.max_dist_to_plane_icp = 0.5; 62 | optimize_options.threshold_orientation_norm = 0.1; 63 | optimize_options.threshold_orientation_norm = 0.01; 64 | optimize_options.num_closest_neighbors = 1; 65 | optimize_options.weight_neighborhood = 0.2; 66 | optimize_options.weight_alpha = 0.8; 67 | optimize_options.max_num_residuals = 600; 68 | optimize_options.min_num_residuals = 200; 69 | 70 | return default_options; 71 | } 72 | 73 | void odometryOptions::recordParameters() 74 | { 75 | std::string str_temp; 76 | 77 | std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app); 78 | 79 | foutC << "init_voxel_size: " << init_voxel_size << std::endl; 80 | foutC << "init_sample_voxel_size: " << init_sample_voxel_size << std::endl; 81 | foutC << "init_num_frames: " << init_num_frames << std::endl; 82 | foutC << "num_for_initialization: " << num_for_initialization << std::endl; 83 | foutC << "voxel_size: " << voxel_size << std::endl; 84 | foutC << "sample_voxel_size: " << sample_voxel_size << std::endl; 85 | foutC << "max_distance: " << max_distance << std::endl; 86 | foutC << "max_num_points_in_voxel: " << max_num_points_in_voxel << std::endl; 87 | foutC << "min_distance_points: " << min_distance_points << std::endl; 88 | foutC << "distance_error_threshold: " << distance_error_threshold << std::endl; 89 | 90 | switch(motion_compensation) 91 | { 92 | case 0: 93 | str_temp = "IMU"; 94 | break; 95 | case 1: 96 | str_temp = "CONSTANT_VELOCITY"; 97 | break; 98 | default: 99 | break; 100 | } 101 | foutC << "motion_compensation: " << str_temp << std::endl; 102 | switch(initialization) 103 | { 104 | case 0: 105 | str_temp = "INIT_IMU"; 106 | break; 107 | case 1: 108 | str_temp = "INIT_CONSTANT_VELOCITY"; 109 | break; 110 | default: 111 | break; 112 | } 113 | foutC << "initialization: " << str_temp << std::endl; 114 | 115 | foutC.close(); 116 | 117 | optimize_options.recordParameters(); 118 | } 119 | 120 | void icpOptions::recordParameters() 121 | { 122 | std::string str_temp; 123 | 124 | std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app); 125 | 126 | foutC << "threshold_voxel_occupancy: " << threshold_voxel_occupancy << std::endl; 127 | foutC << "init_num_frames: " << init_num_frames << std::endl; 128 | foutC << "size_voxel_map: " << size_voxel_map << std::endl; 129 | foutC << "num_iters_icp: " << num_iters_icp << std::endl; 130 | foutC << "min_number_neighbors: " << min_number_neighbors << std::endl; 131 | foutC << "voxel_neighborhood: " << voxel_neighborhood << std::endl; 132 | foutC << "power_planarity: " << power_planarity << std::endl; 133 | str_temp = estimate_normal_from_neighborhood ? "true" : "false"; 134 | foutC << "estimate_normal_from_neighborhood: " << str_temp << std::endl; 135 | foutC << "max_number_neighbors: " << max_number_neighbors << std::endl; 136 | foutC << "max_dist_to_plane_icp: " << max_dist_to_plane_icp << std::endl; 137 | foutC << "threshold_orientation_norm: " << threshold_orientation_norm << std::endl; 138 | foutC << "threshold_translation_norm: " << threshold_translation_norm << std::endl; 139 | foutC << "max_num_residuals: " << max_num_residuals << std::endl; 140 | foutC << "min_num_residuals: " << min_num_residuals << std::endl; 141 | foutC << "num_closest_neighbors: " << num_closest_neighbors << std::endl; 142 | foutC << "weight_alpha: " << weight_alpha << std::endl; 143 | foutC << "weight_neighborhood: " << weight_neighborhood << std::endl; 144 | 145 | str_temp = debug_print ? "true" : "false"; 146 | foutC << "debug_print: " << str_temp << std::endl; 147 | str_temp = debug_viz ? "true" : "false"; 148 | foutC << "debug_viz: " << str_temp << std::endl; 149 | 150 | foutC.close(); 151 | } -------------------------------------------------------------------------------- /sr_lio/src/state.cpp: -------------------------------------------------------------------------------- 1 | #include "state.h" 2 | 3 | state::state() 4 | { 5 | rotation = Eigen::Quaterniond::Identity(); 6 | translation = Eigen::Vector3d::Zero(); 7 | velocity = Eigen::Vector3d::Zero(); 8 | ba = Eigen::Vector3d::Zero(); 9 | bg = Eigen::Vector3d::Zero(); 10 | } 11 | 12 | state::state(const Eigen::Quaterniond &rotation_, const Eigen::Vector3d &translation_, 13 | const Eigen::Vector3d &velocity_, const Eigen::Vector3d& ba_, const Eigen::Vector3d& bg_) 14 | : rotation{rotation_}, translation{translation_}, velocity{velocity_}, ba{ba_}, bg{bg_} 15 | { 16 | 17 | } 18 | 19 | state::state(const state* state_temp, bool copy) 20 | { 21 | rotation = state_temp->rotation; 22 | translation = state_temp->translation; 23 | velocity = state_temp->velocity; 24 | ba = state_temp->ba; 25 | bg = state_temp->bg; 26 | } 27 | 28 | void state::release() 29 | { 30 | 31 | } -------------------------------------------------------------------------------- /sr_lio/src/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | bool debug_output = false; 4 | std::string output_path = ""; 5 | 6 | bool time_diff_enable = false; 7 | double time_diff = 0.0; 8 | 9 | float mov_threshold = 1.5f; 10 | 11 | bool initial_flag = false; 12 | 13 | Eigen::Vector3d G; 14 | double G_norm; 15 | 16 | bool time_list(point3D &point_1, point3D &point_2) 17 | { 18 | return (point_1.relative_time < point_2.relative_time); 19 | }; 20 | 21 | bool time_list_velodyne(velodyne_ros::Point &point_1, velodyne_ros::Point &point_2) 22 | { 23 | return (point_1.time < point_2.time); 24 | } 25 | 26 | bool time_list_robosense(robosense_ros::Point &point_1, robosense_ros::Point &point_2) 27 | { 28 | return (point_1.timestamp < point_2.timestamp); 29 | } 30 | 31 | bool time_list_ouster(ouster_ros::Point &point_1, ouster_ros::Point &point_2) 32 | { 33 | return (point_1.t < point_2.t); 34 | } 35 | 36 | void point3DtoPCL(std::vector &v_point_temp, pcl::PointCloud::Ptr &p_cloud_temp) 37 | { 38 | for(int i = 0; i < v_point_temp.size(); i++) 39 | { 40 | pcl::PointXYZINormal cloud_temp; 41 | cloud_temp.x = v_point_temp[i].raw_point.x(); 42 | cloud_temp.y = v_point_temp[i].raw_point.y(); 43 | cloud_temp.z = v_point_temp[i].raw_point.z(); 44 | cloud_temp.normal_x = 0; 45 | cloud_temp.normal_y = 0; 46 | cloud_temp.normal_z = 0; 47 | cloud_temp.intensity = v_point_temp[i].alpha_time; 48 | cloud_temp.curvature = v_point_temp[i].relative_time; 49 | 50 | p_cloud_temp->points.push_back(cloud_temp); 51 | } 52 | } 53 | 54 | Eigen::Matrix3d mat33FromArray(std::vector &array) 55 | { 56 | assert(array.size() == 9); 57 | Eigen::Matrix3d mat33; 58 | mat33(0, 0) = array[0]; mat33(0, 1) = array[1]; mat33(0, 2) = array[2]; 59 | mat33(1, 0) = array[3]; mat33(1, 1) = array[4]; mat33(1, 2) = array[5]; 60 | mat33(2, 0) = array[6]; mat33(2, 1) = array[7]; mat33(2, 2) = array[8]; 61 | 62 | return mat33; 63 | } 64 | 65 | Eigen::Vector3d vec3FromArray(std::vector &array) 66 | { 67 | assert(array.size() == 3); 68 | Eigen::Vector3d vec3; 69 | vec3(0, 0) = array[0]; vec3(1, 0) = array[1]; vec3(2, 0) = array[2]; 70 | 71 | return vec3; 72 | } 73 | 74 | void pointBodyToWorld(pcl::PointXYZINormal const * const pi, pcl::PointXYZINormal * const po, Eigen::Matrix3d &R_world_cur, 75 | Eigen::Vector3d &t_world_cur, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar) 76 | { 77 | Eigen::Vector3d point_body(pi->x, pi->y, pi->z); 78 | Eigen::Vector3d point_global(R_world_cur * (R_imu_lidar * point_body + t_imu_lidar) + t_world_cur); 79 | 80 | po->x = point_global(0); 81 | po->y = point_global(1); 82 | po->z = point_global(2); 83 | po->intensity = pi->intensity; 84 | } 85 | 86 | void RGBpointLidarToIMU(pcl::PointXYZINormal const * const pi, pcl::PointXYZINormal * const po, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar) 87 | { 88 | Eigen::Vector3d pt_lidar(pi->x, pi->y, pi->z); 89 | Eigen::Vector3d pt_imu = R_imu_lidar * pt_lidar + t_imu_lidar; 90 | 91 | po->x = pt_imu(0, 0); 92 | po->y = pt_imu(1, 0); 93 | po->z = pt_imu(2, 0); 94 | po->intensity = pi->intensity; 95 | } 96 | 97 | bool planeFitting(Eigen::Matrix &plane_parameter, const std::vector> &point, const double &threshold) 98 | { 99 | Eigen::Matrix A; 100 | Eigen::Matrix b; 101 | A.setZero(); 102 | b.setOnes(); 103 | b *= -1.0f; 104 | 105 | for (int i = 0; i < NUM_MATCH_POINTS; i++) 106 | { 107 | A(i, 0) = point[i].x; 108 | A(i, 1) = point[i].y; 109 | A(i, 2) = point[i].z; 110 | } 111 | 112 | Eigen::Matrix norm_vector = A.colPivHouseholderQr().solve(b); 113 | 114 | double n = norm_vector.norm(); 115 | plane_parameter(0) = norm_vector(0, 0) / n; 116 | plane_parameter(1) = norm_vector(1, 0) / n; 117 | plane_parameter(2) = norm_vector(2, 0) / n; 118 | plane_parameter(3) = 1.0 / n; 119 | 120 | for (int i = 0; i < NUM_MATCH_POINTS; i++) 121 | { 122 | if (fabs(plane_parameter(0, 0) * point[i].x + plane_parameter(1, 0) * point[i].y + plane_parameter(2, 0) * point[i].z + plane_parameter(3, 0)) > threshold) 123 | return false; 124 | } 125 | 126 | return true; 127 | } 128 | 129 | double AngularDistance(const Eigen::Matrix3d &rota, const Eigen::Matrix3d &rotb) 130 | { 131 | double norm = ((rota * rotb.transpose()).trace() - 1) / 2; 132 | norm = std::acos(norm) * 180 / M_PI; 133 | return norm; 134 | } 135 | 136 | double AngularDistance(const Eigen::Quaterniond &q_a, const Eigen::Quaterniond &q_b) 137 | { 138 | Eigen::Matrix3d rota = q_a.toRotationMatrix(); 139 | Eigen::Matrix3d rotb = q_b.toRotationMatrix(); 140 | 141 | double norm = ((rota * rotb.transpose()).trace() - 1) / 2; 142 | norm = std::acos(norm) * 180 / M_PI; 143 | return norm; 144 | } 145 | 146 | double AngularDistance(const Eigen::Vector3d &d_so3) 147 | { 148 | Eigen::Matrix3d d_R = numType::so3ToRotation(d_so3); 149 | 150 | double norm = (d_R.trace() - 1) / 2; 151 | norm = std::acos(norm) * 180 / M_PI; 152 | return norm; 153 | } 154 | 155 | float calculateDist2(pcl::PointXYZINormal point1, pcl::PointXYZINormal point2) 156 | { 157 | float d = (point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y) + (point1.z - point2.z) * (point1.z - point2.z); 158 | return d; 159 | } 160 | 161 | void saveCutCloud(std::string &str, pcl::PointCloud::Ptr &p_cloud_temp) 162 | { 163 | pcl::PCDWriter pcd_writer; 164 | pcd_writer.writeBinary(str, *p_cloud_temp); 165 | } 166 | 167 | void subSampleFrame(std::vector &frame, double size_voxel) 168 | { 169 | std::tr1::unordered_map, std::hash> grid; 170 | for (int i = 0; i < (int) frame.size(); i++) { 171 | auto kx = static_cast(frame[i].point[0] / size_voxel); 172 | auto ky = static_cast(frame[i].point[1] / size_voxel); 173 | auto kz = static_cast(frame[i].point[2] / size_voxel); 174 | grid[voxel(kx, ky, kz)].push_back(frame[i]); 175 | } 176 | frame.resize(0); 177 | int step = 0; 178 | for(const auto &n: grid) 179 | { 180 | if(n.second.size() > 0) 181 | { 182 | frame.push_back(n.second[0]); 183 | step++; 184 | } 185 | } 186 | } 187 | 188 | void gridSampling(const std::vector &frame, std::vector &keypoints, double size_voxel_subsampling) 189 | { 190 | keypoints.resize(0); 191 | std::vector frame_sub; 192 | frame_sub.resize(frame.size()); 193 | for (int i = 0; i < (int) frame_sub.size(); i++) { 194 | frame_sub[i] = frame[i]; 195 | } 196 | subSampleFrame(frame_sub, size_voxel_subsampling); 197 | keypoints.reserve(frame_sub.size()); 198 | for (int i = 0; i < (int) frame_sub.size(); i++) { 199 | keypoints.push_back(frame_sub[i]); 200 | } 201 | } 202 | 203 | void distortFrameByConstant(std::vector &points, std::vector &imu_states, double time_sweep_begin, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar) 204 | { 205 | double time_sweep_end, time_point; 206 | 207 | time_sweep_end = imu_states.back().timestamp; 208 | 209 | Eigen::Quaterniond quat_begin = imu_states.front().quat; 210 | Eigen::Quaterniond quat_end = imu_states.back().quat; 211 | 212 | Eigen::Vector3d trans_begin = imu_states.front().trans; 213 | Eigen::Vector3d trans_end = imu_states.back().trans; 214 | 215 | std::vector::iterator iter = points.begin(); 216 | 217 | while (iter != points.end()) 218 | { 219 | time_point = time_sweep_begin + (*iter).relative_time / 1000.0; 220 | 221 | if (time_point - time_sweep_begin < 1e-6) time_point = time_sweep_begin + 1e-6; 222 | if (time_sweep_end - time_point < 1e-6) time_point = time_sweep_end - 1e-6; 223 | 224 | assert(time_point > time_sweep_begin - 1e-6 && time_point < time_sweep_end + 1e-6); 225 | 226 | double alpha_time = (time_point - time_sweep_begin) / (time_sweep_end - time_sweep_begin); 227 | assert(alpha_time >= 0 && alpha_time <= 1); 228 | 229 | Eigen::Quaterniond quat_alpha = quat_begin.slerp(alpha_time, quat_end); 230 | Eigen::Vector3d trans_alpha = (1.0 - alpha_time) * trans_begin + alpha_time * trans_end; 231 | 232 | (*iter).imu_point = quat_alpha.toRotationMatrix() * (R_imu_lidar * (*iter).raw_point + t_imu_lidar) + trans_alpha; 233 | 234 | iter++; 235 | } 236 | } 237 | 238 | void distortFrameByImu(std::vector &points, std::vector &imu_states, double time_sweep_begin, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar) 239 | { 240 | double time_imu_begin, time_imu_end, time_point; 241 | 242 | Eigen::Quaterniond quat_imu; 243 | Eigen::Vector3d trans_imu, vel_imu; 244 | Eigen::Vector3d un_acc, un_gyr; 245 | 246 | std::vector::iterator iter = points.begin(); 247 | 248 | for (int n = 0; n + 1 < imu_states.size(); n++) 249 | { 250 | // distortion method 1 251 | time_imu_begin = imu_states[n].timestamp; 252 | quat_imu = imu_states[n].quat; 253 | trans_imu = imu_states[n].trans; 254 | vel_imu = imu_states[n].vel; 255 | 256 | time_imu_end = imu_states[n + 1].timestamp; 257 | un_acc = imu_states[n + 1].un_acc; 258 | un_gyr = imu_states[n + 1].un_gyr; 259 | // distortion method 1 260 | 261 | // distortion method 2 262 | /* 263 | time_imu_begin = imu_states[n].timestamp; 264 | Eigen::Quaterniond quat_head = imu_states[n].quat; 265 | Eigen::Vector3d trans_head = imu_states[n].trans; 266 | 267 | time_imu_end = imu_states[n + 1].timestamp; 268 | Eigen::Quaterniond quat_tail = imu_states[n + 1].quat; 269 | Eigen::Vector3d trans_tail = imu_states[n + 1].trans; 270 | */ 271 | // distortion method 2 272 | 273 | while (iter != points.end()) 274 | { 275 | time_point = time_sweep_begin + (*iter).relative_time / 1000.0; 276 | 277 | if (time_point > time_imu_begin - 1e-6 && time_point < time_imu_end + 1e-6) 278 | { 279 | if (fabs(time_point - time_imu_begin) < 1e-6) time_point = time_imu_begin + 1e-6; 280 | if (fabs(time_point - time_imu_end) < 1e-6) time_point = time_imu_end - 1e-6; 281 | 282 | // distortion method 1 283 | double dt = time_point - time_imu_begin; 284 | 285 | Eigen::Quaterniond quat_point = quat_imu * numType::so3ToQuat(un_gyr * dt); 286 | quat_point.normalize(); 287 | Eigen::Vector3d trans_point = trans_imu + vel_imu * dt + 0.5 * un_acc * dt * dt; 288 | 289 | (*iter).imu_point = quat_point.toRotationMatrix() * (R_imu_lidar * (*iter).raw_point + t_imu_lidar) + trans_point; 290 | // distortion method 1 291 | 292 | // distortion method 2 293 | /* 294 | double alpha_time = (time_point - time_imu_begin) / (time_imu_end - time_imu_begin); 295 | assert(alpha_time >= 0 && alpha_time <= 1); 296 | 297 | Eigen::Quaterniond quat_alpha = quat_head.slerp(alpha_time, quat_tail); 298 | Eigen::Vector3d trans_alpha = (1.0 - alpha_time) * trans_head + alpha_time * trans_tail; 299 | 300 | (*iter).imu_point = quat_alpha.toRotationMatrix() * (R_imu_lidar * (*iter).raw_point + t_imu_lidar) + trans_alpha; 301 | */ 302 | // distortion method 2 303 | 304 | iter++; 305 | } 306 | else 307 | { 308 | break; 309 | } 310 | } 311 | } 312 | } 313 | 314 | void transformPoint(point3D &point_temp, Eigen::Quaterniond &q_end, Eigen::Vector3d &t_end, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar) 315 | { 316 | 317 | point_temp.point = q_end.toRotationMatrix() * (R_imu_lidar * point_temp.raw_point + t_imu_lidar) + t_end; 318 | } 319 | 320 | void transformAllImuPoint(std::vector &points, std::vector &imu_states, Eigen::Matrix3d &R_imu_lidar, Eigen::Vector3d &t_imu_lidar) 321 | { 322 | Eigen::Quaterniond quat_end_inv = imu_states.back().quat.inverse(); 323 | Eigen::Vector3d trans_end_inv = - quat_end_inv.toRotationMatrix() * imu_states.back().trans; 324 | 325 | std::vector::iterator iter = points.begin(); 326 | 327 | while (iter != points.end()) 328 | { 329 | (*iter).raw_point = R_imu_lidar.transpose() * (quat_end_inv.toRotationMatrix() * (*iter).imu_point + trans_end_inv) - R_imu_lidar.transpose() * t_imu_lidar; 330 | iter++; 331 | } 332 | } -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | include(GNUInstallDirs) 3 | 4 | 5 | project(tsl-robin-map VERSION 0.6.3) 6 | 7 | add_library(robin_map INTERFACE) 8 | # Use tsl::robin_map as target, more consistent with other libraries conventions (Boost, Qt, ...) 9 | add_library(tsl::robin_map ALIAS robin_map) 10 | 11 | target_include_directories(robin_map INTERFACE 12 | "$" 13 | "$") 14 | 15 | list(APPEND headers "${CMAKE_CURRENT_SOURCE_DIR}/include/tsl/robin_growth_policy.h" 16 | "${CMAKE_CURRENT_SOURCE_DIR}/include/tsl/robin_hash.h" 17 | "${CMAKE_CURRENT_SOURCE_DIR}/include/tsl/robin_map.h" 18 | "${CMAKE_CURRENT_SOURCE_DIR}/include/tsl/robin_set.h") 19 | target_sources(robin_map INTERFACE "$") 20 | 21 | if(MSVC) 22 | target_sources(robin_map INTERFACE 23 | "$" 24 | "$") 25 | endif() 26 | 27 | 28 | 29 | 30 | # Installation (only compatible with CMake version >= 3.3) 31 | if(${CMAKE_VERSION} VERSION_GREATER "3.2") 32 | include(CMakePackageConfigHelpers) 33 | 34 | ## Install include directory and potential natvis file 35 | install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/include/tsl" 36 | DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}") 37 | 38 | if(MSVC) 39 | install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/tsl-robin-map.natvis" 40 | DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}") 41 | endif() 42 | 43 | 44 | 45 | ## Create and install tsl-robin-mapConfig.cmake 46 | configure_package_config_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/tsl-robin-mapConfig.cmake.in" 47 | "${CMAKE_CURRENT_BINARY_DIR}/tsl-robin-mapConfig.cmake" 48 | INSTALL_DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/cmake/tsl-robin-map") 49 | 50 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/tsl-robin-mapConfig.cmake" 51 | DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/cmake/tsl-robin-map") 52 | 53 | 54 | 55 | ## Create and install tsl-robin-mapTargets.cmake 56 | install(TARGETS robin_map 57 | EXPORT tsl-robin-mapTargets) 58 | 59 | install(EXPORT tsl-robin-mapTargets 60 | NAMESPACE tsl:: 61 | DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/cmake/tsl-robin-map") 62 | 63 | 64 | 65 | ## Create and install tsl-robin-mapConfigVersion.cmake 66 | # tsl-robin-map is header-only and does not depend on the architecture. 67 | # Remove CMAKE_SIZEOF_VOID_P from tsl-robin-mapConfigVersion.cmake so that a 68 | # tsl-robin-mapConfig.cmake generated for a 64 bit target can be used for 32 bit 69 | # targets and vice versa. 70 | set(CMAKE_SIZEOF_VOID_P_BACKUP ${CMAKE_SIZEOF_VOID_P}) 71 | unset(CMAKE_SIZEOF_VOID_P) 72 | write_basic_package_version_file("${CMAKE_CURRENT_BINARY_DIR}/tsl-robin-mapConfigVersion.cmake" 73 | COMPATIBILITY SameMajorVersion) 74 | set(CMAKE_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P_BACKUP}) 75 | 76 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/tsl-robin-mapConfigVersion.cmake" 77 | DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/cmake/tsl-robin-map") 78 | endif() 79 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Thibaut Goetghebuer-Planchon 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/Tessil/robin-map.svg?branch=master)](https://travis-ci.org/Tessil/robin-map) [![Build status](https://ci.appveyor.com/api/projects/status/lo79n4ya4nta79q4/branch/master?svg=true)](https://ci.appveyor.com/project/Tessil/robin-map/branch/master) 2 | 3 | ## A C++ implementation of a fast hash map and hash set using robin hood hashing 4 | 5 | The robin-map library is a C++ implementation of a fast hash map and hash set using open-addressing and linear robin hood hashing with backward shift deletion to resolve collisions. 6 | 7 | Four classes are provided: `tsl::robin_map`, `tsl::robin_set`, `tsl::robin_pg_map` and `tsl::robin_pg_set`. The first two are faster and use a power of two growth policy, the last two use a prime growth policy instead and are able to cope better with a poor hash function. Use the prime version if there is a chance of repeating patterns in the lower bits of your hash (e.g. you are storing pointers with an identity hash function). See [GrowthPolicy](#growth-policy) for details. 8 | 9 | A **benchmark** of `tsl::robin_map` against other hash maps may be found [here](https://tessil.github.io/2016/08/29/benchmark-hopscotch-map.html). This page also gives some advices on which hash table structure you should try for your use case (useful if you are a bit lost with the multiple hash tables implementations in the `tsl` namespace). 10 | 11 | ### Key features 12 | 13 | - Header-only library, just add the [include](include/) directory to your include path and you are ready to go. If you use CMake, you can also use the `tsl::robin_map` exported target from the [CMakeLists.txt](CMakeLists.txt). 14 | - Fast hash table, check the [benchmark](https://tessil.github.io/2016/08/29/benchmark-hopscotch-map.html) for some numbers. 15 | - Support for move-only and non-default constructible key/value. 16 | - Support for heterogeneous lookups allowing the usage of `find` with a type different than `Key` (e.g. if you have a map that uses `std::unique_ptr` as key, you can use a `foo*` or a `std::uintptr_t` as key parameter to `find` without constructing a `std::unique_ptr`, see [example](#heterogeneous-lookups)). 17 | - No need to reserve any sentinel value from the keys. 18 | - Possibility to store the hash value alongside the stored key-value for faster rehash and lookup if the hash or the key equal functions are expensive to compute. Note that hash may be stored even if not asked explicitly when the library can detect that it will have no impact on the size of the structure in memory due to alignment. See the [StoreHash](https://tessil.github.io/robin-map/classtsl_1_1robin__map.html#details) template parameter for details. 19 | - If the hash is known before a lookup, it is possible to pass it as parameter to speed-up the lookup (see `precalculated_hash` parameter in [API](https://tessil.github.io/robin-map/classtsl_1_1robin__map.html#a35021b11aabb61820236692a54b3a0f8)). 20 | - The library can be used with exceptions disabled (through `-fno-exceptions` option on Clang and GCC, without an `/EH` option on MSVC or simply by defining `TSL_NO_EXCEPTIONS`). `std::terminate` is used in replacement of the `throw` instruction when exceptions are disabled. 21 | - API closely similar to `std::unordered_map` and `std::unordered_set`. 22 | 23 | ### Differences compared to `std::unordered_map` 24 | 25 | `tsl::robin_map` tries to have an interface similar to `std::unordered_map`, but some differences exist. 26 | - The **strong exception guarantee only holds** if the following statement is true `std::is_nothrow_swappable::value && std::is_nothrow_move_constructible::value` (where `value_type` is `Key` for `tsl::robin_set` and `std::pair` for `tsl::robin_map`). Otherwise if an exception is thrown during the swap or the move, the structure may end up in a undefined state. Note that per the standard, a `value_type` with a noexcept copy constructor and no move constructor also satisfies this condition and will thus guarantee the strong exception guarantee for the structure (see [API](https://tessil.github.io/robin-map/classtsl_1_1robin__map.html#details) for details). 27 | - The type `Key`, and also `T` in case of map, must be swappable. They must also be copy and/or move constructible. 28 | - Iterator invalidation doesn't behave in the same way, any operation modifying the hash table invalidate them (see [API](https://tessil.github.io/robin-map/classtsl_1_1robin__map.html#details) for details). 29 | - References and pointers to keys or values in the map are invalidated in the same way as iterators to these keys-values. 30 | - For iterators of `tsl::robin_map`, `operator*()` and `operator->()` return a reference and a pointer to `const std::pair` instead of `std::pair` making the value `T` not modifiable. To modify the value you have to call the `value()` method of the iterator to get a mutable reference. Example: 31 | ```c++ 32 | tsl::robin_map map = {{1, 1}, {2, 1}, {3, 1}}; 33 | for(auto it = map.begin(); it != map.end(); ++it) { 34 | //it->second = 2; // Illegal 35 | it.value() = 2; // Ok 36 | } 37 | ``` 38 | - No support for some buckets related methods (like `bucket_size`, `bucket`, ...). 39 | 40 | These differences also apply between `std::unordered_set` and `tsl::robin_set`. 41 | 42 | Thread-safety guarantees are the same as `std::unordered_map/set` (i.e. possible to have multiple readers with no writer). 43 | 44 | ### Growth policy 45 | 46 | The library supports multiple growth policies through the `GrowthPolicy` template parameter. Three policies are provided by the library but you can easily implement your own if needed. 47 | 48 | * **[tsl::rh::power_of_two_growth_policy.](https://tessil.github.io/robin-map/classtsl_1_1rh_1_1power__of__two__growth__policy.html)** Default policy used by `tsl::robin_map/set`. This policy keeps the size of the bucket array of the hash table to a power of two. This constraint allows the policy to avoid the usage of the slow modulo operation to map a hash to a bucket, instead of hash % 2n, it uses hash & (2n - 1) (see [fast modulo](https://en.wikipedia.org/wiki/Modulo_operation#Performance_issues)). Fast but this may cause a lot of collisions with a poor hash function as the modulo with a power of two only masks the most significant bits in the end. 49 | * **[tsl::rh::prime_growth_policy.](https://tessil.github.io/robin-map/classtsl_1_1rh_1_1prime__growth__policy.html)** Default policy used by `tsl::robin_pg_map/set`. The policy keeps the size of the bucket array of the hash table to a prime number. When mapping a hash to a bucket, using a prime number as modulo will result in a better distribution of the hash across the buckets even with a poor hash function. To allow the compiler to optimize the modulo operation, the policy use a lookup table with constant primes modulos (see [API](https://tessil.github.io/robin-map/classtsl_1_1rh_1_1prime__growth__policy.html#details) for details). Slower than `tsl::rh::power_of_two_growth_policy` but more secure. 50 | * **[tsl::rh::mod_growth_policy.](https://tessil.github.io/robin-map/classtsl_1_1rh_1_1mod__growth__policy.html)** The policy grows the map by a customizable growth factor passed in parameter. It then just use the modulo operator to map a hash to a bucket. Slower but more flexible. 51 | 52 | 53 | To implement your own policy, you have to implement the following interface. 54 | 55 | ```c++ 56 | struct custom_policy { 57 | // Called on hash table construction and rehash, min_bucket_count_in_out is the minimum buckets 58 | // that the hash table needs. The policy can change it to a higher number of buckets if needed 59 | // and the hash table will use this value as bucket count. If 0 bucket is asked, then the value 60 | // must stay at 0. 61 | explicit custom_policy(std::size_t& min_bucket_count_in_out); 62 | 63 | // Return the bucket [0, bucket_count()) to which the hash belongs. 64 | // If bucket_count() is 0, it must always return 0. 65 | std::size_t bucket_for_hash(std::size_t hash) const noexcept; 66 | 67 | // Return the number of buckets that should be used on next growth 68 | std::size_t next_bucket_count() const; 69 | 70 | // Maximum number of buckets supported by the policy 71 | std::size_t max_bucket_count() const; 72 | 73 | // Reset the growth policy as if the policy was created with a bucket count of 0. 74 | // After a clear, the policy must always return 0 when bucket_for_hash() is called. 75 | void clear() noexcept; 76 | } 77 | ``` 78 | 79 | ### Installation 80 | 81 | To use robin-map, just add the [include](include/) directory to your include path. It is a **header-only** library. 82 | 83 | If you use CMake, you can also use the `tsl::robin_map` exported target from the [CMakeLists.txt](CMakeLists.txt) with `target_link_libraries`. 84 | ```cmake 85 | # Example where the robin-map project is stored in a third-party directory 86 | add_subdirectory(third-party/robin-map) 87 | target_link_libraries(your_target PRIVATE tsl::robin_map) 88 | ``` 89 | 90 | If the project has been installed through `make install`, you can also use `find_package(tsl-robin-map REQUIRED)` instead of `add_subdirectory`. 91 | 92 | The library is available in [vcpkg](https://github.com/Microsoft/vcpkg/tree/master/ports/robin-map) and [conan](https://bintray.com/tessil/tsl/tsl-robin-map%3Atessil). It's also present in [Debian](https://packages.debian.org/buster/robin-map-dev), [Ubuntu](https://packages.ubuntu.com/disco/robin-map-dev) and [Fedora](https://apps.fedoraproject.org/packages/robin-map-devel) package repositories. 93 | 94 | The code should work with any C++11 standard-compliant compiler and has been tested with GCC 4.8.4, Clang 3.5.0 and Visual Studio 2015. 95 | 96 | To run the tests you will need the Boost Test library and CMake. 97 | 98 | ```bash 99 | git clone https://github.com/Tessil/robin-map.git 100 | cd robin-map/tests 101 | mkdir build 102 | cd build 103 | cmake .. 104 | cmake --build . 105 | ./tsl_robin_map_tests 106 | ``` 107 | 108 | ### Usage 109 | 110 | The API can be found [here](https://tessil.github.io/robin-map/). 111 | 112 | All methods are not documented yet, but they replicate the behavior of the ones in `std::unordered_map` and `std::unordered_set`, except if specified otherwise. 113 | 114 | 115 | ### Example 116 | 117 | ```c++ 118 | #include 119 | #include 120 | #include 121 | #include 122 | #include 123 | 124 | int main() { 125 | tsl::robin_map map = {{"a", 1}, {"b", 2}}; 126 | map["c"] = 3; 127 | map["d"] = 4; 128 | 129 | map.insert({"e", 5}); 130 | map.erase("b"); 131 | 132 | for(auto it = map.begin(); it != map.end(); ++it) { 133 | //it->second += 2; // Not valid. 134 | it.value() += 2; 135 | } 136 | 137 | // {d, 6} {a, 3} {e, 7} {c, 5} 138 | for(const auto& key_value : map) { 139 | std::cout << "{" << key_value.first << ", " << key_value.second << "}" << std::endl; 140 | } 141 | 142 | 143 | if(map.find("a") != map.end()) { 144 | std::cout << "Found \"a\"." << std::endl; 145 | } 146 | 147 | const std::size_t precalculated_hash = std::hash()("a"); 148 | // If we already know the hash beforehand, we can pass it in parameter to speed-up lookups. 149 | if(map.find("a", precalculated_hash) != map.end()) { 150 | std::cout << "Found \"a\" with hash " << precalculated_hash << "." << std::endl; 151 | } 152 | 153 | 154 | /* 155 | * Calculating the hash and comparing two std::string may be slow. 156 | * We can store the hash of each std::string in the hash map to make 157 | * the inserts and lookups faster by setting StoreHash to true. 158 | */ 159 | tsl::robin_map, 160 | std::equal_to, 161 | std::allocator>, 162 | true> map2; 163 | 164 | map2["a"] = 1; 165 | map2["b"] = 2; 166 | 167 | // {a, 1} {b, 2} 168 | for(const auto& key_value : map2) { 169 | std::cout << "{" << key_value.first << ", " << key_value.second << "}" << std::endl; 170 | } 171 | 172 | 173 | 174 | 175 | tsl::robin_set set; 176 | set.insert({1, 9, 0}); 177 | set.insert({2, -1, 9}); 178 | 179 | // {0} {1} {2} {9} {-1} 180 | for(const auto& key : set) { 181 | std::cout << "{" << key << "}" << std::endl; 182 | } 183 | } 184 | ``` 185 | 186 | #### Heterogeneous lookups 187 | 188 | Heterogeneous overloads allow the usage of other types than `Key` for lookup and erase operations as long as the used types are hashable and comparable to `Key`. 189 | 190 | To activate the heterogeneous overloads in `tsl::robin_map/set`, the qualified-id `KeyEqual::is_transparent` must be valid. It works the same way as for [`std::map::find`](http://en.cppreference.com/w/cpp/container/map/find). You can either use [`std::equal_to<>`](http://en.cppreference.com/w/cpp/utility/functional/equal_to_void) or define your own function object. 191 | 192 | Both `KeyEqual` and `Hash` will need to be able to deal with the different types. 193 | 194 | ```c++ 195 | #include 196 | #include 197 | #include 198 | #include 199 | 200 | 201 | struct employee { 202 | employee(int id, std::string name) : m_id(id), m_name(std::move(name)) { 203 | } 204 | 205 | // Either we include the comparators in the class and we use `std::equal_to<>`... 206 | friend bool operator==(const employee& empl, int empl_id) { 207 | return empl.m_id == empl_id; 208 | } 209 | 210 | friend bool operator==(int empl_id, const employee& empl) { 211 | return empl_id == empl.m_id; 212 | } 213 | 214 | friend bool operator==(const employee& empl1, const employee& empl2) { 215 | return empl1.m_id == empl2.m_id; 216 | } 217 | 218 | 219 | int m_id; 220 | std::string m_name; 221 | }; 222 | 223 | // ... or we implement a separate class to compare employees. 224 | struct equal_employee { 225 | using is_transparent = void; 226 | 227 | bool operator()(const employee& empl, int empl_id) const { 228 | return empl.m_id == empl_id; 229 | } 230 | 231 | bool operator()(int empl_id, const employee& empl) const { 232 | return empl_id == empl.m_id; 233 | } 234 | 235 | bool operator()(const employee& empl1, const employee& empl2) const { 236 | return empl1.m_id == empl2.m_id; 237 | } 238 | }; 239 | 240 | struct hash_employee { 241 | std::size_t operator()(const employee& empl) const { 242 | return std::hash()(empl.m_id); 243 | } 244 | 245 | std::size_t operator()(int id) const { 246 | return std::hash()(id); 247 | } 248 | }; 249 | 250 | 251 | int main() { 252 | // Use std::equal_to<> which will automatically deduce and forward the parameters 253 | tsl::robin_map> map; 254 | map.insert({employee(1, "John Doe"), 2001}); 255 | map.insert({employee(2, "Jane Doe"), 2002}); 256 | map.insert({employee(3, "John Smith"), 2003}); 257 | 258 | // John Smith 2003 259 | auto it = map.find(3); 260 | if(it != map.end()) { 261 | std::cout << it->first.m_name << " " << it->second << std::endl; 262 | } 263 | 264 | map.erase(1); 265 | 266 | 267 | 268 | // Use a custom KeyEqual which has an is_transparent member type 269 | tsl::robin_map map2; 270 | map2.insert({employee(4, "Johnny Doe"), 2004}); 271 | 272 | // 2004 273 | std::cout << map2.at(4) << std::endl; 274 | } 275 | ``` 276 | 277 | 278 | 279 | ### License 280 | 281 | The code is licensed under the MIT license, see the [LICENSE file](LICENSE) for details. 282 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/appveyor.yml: -------------------------------------------------------------------------------- 1 | environment: 2 | BOOST_ROOT: C:\Libraries\boost_1_67_0 3 | matrix: 4 | - APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2015 5 | ARCH: Win32 6 | BOOST_LIBRARYDIR: C:\Libraries\boost_1_67_0\lib32-msvc-14.0 7 | CMAKE_GENERATOR: Visual Studio 14 2015 8 | 9 | - APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2015 10 | ARCH: x64 11 | BOOST_LIBRARYDIR: C:\Libraries\boost_1_67_0\lib64-msvc-14.0 12 | CMAKE_GENERATOR: Visual Studio 14 2015 Win64 13 | 14 | - APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2017 15 | ARCH: Win32 16 | BOOST_LIBRARYDIR: C:\Libraries\boost_1_67_0\lib32-msvc-14.1 17 | CMAKE_GENERATOR: Visual Studio 15 2017 18 | CXXFLAGS: /permissive- 19 | 20 | - APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2017 21 | ARCH: x64 22 | BOOST_LIBRARYDIR: C:\Libraries\boost_1_67_0\lib64-msvc-14.1 23 | CMAKE_GENERATOR: Visual Studio 15 2017 Win64 24 | CXXFLAGS: /permissive- 25 | 26 | configuration: 27 | - Debug 28 | - Release 29 | 30 | build_script: 31 | - cd tests 32 | - mkdir build 33 | - cd build 34 | - cmake -DCMAKE_BUILD_TYPE=%CONFIGURATION% -G"%CMAKE_GENERATOR%" .. 35 | - cmake --build . --config %CONFIGURATION% 36 | 37 | test_script: 38 | - set PATH=%PATH%;%BOOST_LIBRARYDIR% 39 | - .\%CONFIGURATION%\tsl_robin_map_tests.exe 40 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/cmake/tsl-robin-mapConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # This module sets the following variables: 2 | # * tsl-robin-map_FOUND - true if tsl-robin-map found on the system 3 | # * tsl-robin-map_INCLUDE_DIRS - the directory containing tsl-robin-map headers 4 | @PACKAGE_INIT@ 5 | 6 | if(NOT TARGET tsl::robin_map) 7 | include("${CMAKE_CURRENT_LIST_DIR}/tsl-robin-mapTargets.cmake") 8 | get_target_property(tsl-robin-map_INCLUDE_DIRS tsl::robin_map INTERFACE_INCLUDE_DIRECTORIES) 9 | endif() 10 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/include/tsl/robin_growth_policy.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2017 Thibaut Goetghebuer-Planchon 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | #ifndef TSL_ROBIN_GROWTH_POLICY_H 25 | #define TSL_ROBIN_GROWTH_POLICY_H 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | #ifdef TSL_DEBUG 41 | # define tsl_rh_assert(expr) assert(expr) 42 | #else 43 | # define tsl_rh_assert(expr) (static_cast(0)) 44 | #endif 45 | 46 | 47 | /** 48 | * If exceptions are enabled, throw the exception passed in parameter, otherwise call std::terminate. 49 | */ 50 | #if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || (defined (_MSC_VER) && defined (_CPPUNWIND))) && !defined(TSL_NO_EXCEPTIONS) 51 | # define TSL_RH_THROW_OR_TERMINATE(ex, msg) throw ex(msg) 52 | #else 53 | # define TSL_RH_NO_EXCEPTIONS 54 | # ifdef NDEBUG 55 | # define TSL_RH_THROW_OR_TERMINATE(ex, msg) std::terminate() 56 | # else 57 | # include 58 | # define TSL_RH_THROW_OR_TERMINATE(ex, msg) do { std::cerr << msg << std::endl; std::terminate(); } while(0) 59 | # endif 60 | #endif 61 | 62 | 63 | #if defined(__GNUC__) || defined(__clang__) 64 | # define TSL_RH_LIKELY(exp) (__builtin_expect(!!(exp), true)) 65 | #else 66 | # define TSL_RH_LIKELY(exp) (exp) 67 | #endif 68 | 69 | 70 | namespace tsl { 71 | namespace rh { 72 | 73 | /** 74 | * Grow the hash table by a factor of GrowthFactor keeping the bucket count to a power of two. It allows 75 | * the table to use a mask operation instead of a modulo operation to map a hash to a bucket. 76 | * 77 | * GrowthFactor must be a power of two >= 2. 78 | */ 79 | template 80 | class power_of_two_growth_policy { 81 | public: 82 | /** 83 | * Called on the hash table creation and on rehash. The number of buckets for the table is passed in parameter. 84 | * This number is a minimum, the policy may update this value with a higher value if needed (but not lower). 85 | * 86 | * If 0 is given, min_bucket_count_in_out must still be 0 after the policy creation and 87 | * bucket_for_hash must always return 0 in this case. 88 | */ 89 | explicit power_of_two_growth_policy(std::size_t& min_bucket_count_in_out) { 90 | if(min_bucket_count_in_out > max_bucket_count()) { 91 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 92 | } 93 | 94 | if(min_bucket_count_in_out > 0) { 95 | min_bucket_count_in_out = round_up_to_power_of_two(min_bucket_count_in_out); 96 | m_mask = min_bucket_count_in_out - 1; 97 | } 98 | else { 99 | m_mask = 0; 100 | } 101 | } 102 | 103 | /** 104 | * Return the bucket [0, bucket_count()) to which the hash belongs. 105 | * If bucket_count() is 0, it must always return 0. 106 | */ 107 | std::size_t bucket_for_hash(std::size_t hash) const noexcept { 108 | return hash & m_mask; 109 | } 110 | 111 | /** 112 | * Return the number of buckets that should be used on next growth. 113 | */ 114 | std::size_t next_bucket_count() const { 115 | if((m_mask + 1) > max_bucket_count() / GrowthFactor) { 116 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 117 | } 118 | 119 | return (m_mask + 1) * GrowthFactor; 120 | } 121 | 122 | /** 123 | * Return the maximum number of buckets supported by the policy. 124 | */ 125 | std::size_t max_bucket_count() const { 126 | // Largest power of two. 127 | return (std::numeric_limits::max() / 2) + 1; 128 | } 129 | 130 | /** 131 | * Reset the growth policy as if it was created with a bucket count of 0. 132 | * After a clear, the policy must always return 0 when bucket_for_hash is called. 133 | */ 134 | void clear() noexcept { 135 | m_mask = 0; 136 | } 137 | 138 | private: 139 | static std::size_t round_up_to_power_of_two(std::size_t value) { 140 | if(is_power_of_two(value)) { 141 | return value; 142 | } 143 | 144 | if(value == 0) { 145 | return 1; 146 | } 147 | 148 | --value; 149 | for(std::size_t i = 1; i < sizeof(std::size_t) * CHAR_BIT; i *= 2) { 150 | value |= value >> i; 151 | } 152 | 153 | return value + 1; 154 | } 155 | 156 | static constexpr bool is_power_of_two(std::size_t value) { 157 | return value != 0 && (value & (value - 1)) == 0; 158 | } 159 | 160 | protected: 161 | static_assert(is_power_of_two(GrowthFactor) && GrowthFactor >= 2, "GrowthFactor must be a power of two >= 2."); 162 | 163 | std::size_t m_mask; 164 | }; 165 | 166 | 167 | /** 168 | * Grow the hash table by GrowthFactor::num / GrowthFactor::den and use a modulo to map a hash 169 | * to a bucket. Slower but it can be useful if you want a slower growth. 170 | */ 171 | template> 172 | class mod_growth_policy { 173 | public: 174 | explicit mod_growth_policy(std::size_t& min_bucket_count_in_out) { 175 | if(min_bucket_count_in_out > max_bucket_count()) { 176 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 177 | } 178 | 179 | if(min_bucket_count_in_out > 0) { 180 | m_mod = min_bucket_count_in_out; 181 | } 182 | else { 183 | m_mod = 1; 184 | } 185 | } 186 | 187 | std::size_t bucket_for_hash(std::size_t hash) const noexcept { 188 | return hash % m_mod; 189 | } 190 | 191 | std::size_t next_bucket_count() const { 192 | if(m_mod == max_bucket_count()) { 193 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 194 | } 195 | 196 | const double next_bucket_count = std::ceil(double(m_mod) * REHASH_SIZE_MULTIPLICATION_FACTOR); 197 | if(!std::isnormal(next_bucket_count)) { 198 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 199 | } 200 | 201 | if(next_bucket_count > double(max_bucket_count())) { 202 | return max_bucket_count(); 203 | } 204 | else { 205 | return std::size_t(next_bucket_count); 206 | } 207 | } 208 | 209 | std::size_t max_bucket_count() const { 210 | return MAX_BUCKET_COUNT; 211 | } 212 | 213 | void clear() noexcept { 214 | m_mod = 1; 215 | } 216 | 217 | private: 218 | static constexpr double REHASH_SIZE_MULTIPLICATION_FACTOR = 1.0 * GrowthFactor::num / GrowthFactor::den; 219 | static const std::size_t MAX_BUCKET_COUNT = 220 | std::size_t(double( 221 | std::numeric_limits::max() / REHASH_SIZE_MULTIPLICATION_FACTOR 222 | )); 223 | 224 | static_assert(REHASH_SIZE_MULTIPLICATION_FACTOR >= 1.1, "Growth factor should be >= 1.1."); 225 | 226 | std::size_t m_mod; 227 | }; 228 | 229 | 230 | 231 | namespace detail { 232 | 233 | #if SIZE_MAX >= ULLONG_MAX 234 | #define TSL_RH_NB_PRIMES 51 235 | #elif SIZE_MAX >= ULONG_MAX 236 | #define TSL_RH_NB_PRIMES 40 237 | #else 238 | #define TSL_RH_NB_PRIMES 23 239 | #endif 240 | 241 | static constexpr const std::array PRIMES = {{ 242 | 1u, 5u, 17u, 29u, 37u, 53u, 67u, 79u, 97u, 131u, 193u, 257u, 389u, 521u, 769u, 1031u, 243 | 1543u, 2053u, 3079u, 6151u, 12289u, 24593u, 49157u, 244 | #if SIZE_MAX >= ULONG_MAX 245 | 98317ul, 196613ul, 393241ul, 786433ul, 1572869ul, 3145739ul, 6291469ul, 12582917ul, 246 | 25165843ul, 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, 1610612741ul, 247 | 3221225473ul, 4294967291ul, 248 | #endif 249 | #if SIZE_MAX >= ULLONG_MAX 250 | 6442450939ull, 12884901893ull, 25769803751ull, 51539607551ull, 103079215111ull, 206158430209ull, 251 | 412316860441ull, 824633720831ull, 1649267441651ull, 3298534883309ull, 6597069766657ull, 252 | #endif 253 | }}; 254 | 255 | template 256 | static constexpr std::size_t mod(std::size_t hash) { return hash % PRIMES[IPrime]; } 257 | 258 | // MOD_PRIME[iprime](hash) returns hash % PRIMES[iprime]. This table allows for faster modulo as the 259 | // compiler can optimize the modulo code better with a constant known at the compilation. 260 | static constexpr const std::array MOD_PRIME = {{ 261 | &mod<0>, &mod<1>, &mod<2>, &mod<3>, &mod<4>, &mod<5>, &mod<6>, &mod<7>, &mod<8>, &mod<9>, &mod<10>, 262 | &mod<11>, &mod<12>, &mod<13>, &mod<14>, &mod<15>, &mod<16>, &mod<17>, &mod<18>, &mod<19>, &mod<20>, 263 | &mod<21>, &mod<22>, 264 | #if SIZE_MAX >= ULONG_MAX 265 | &mod<23>, &mod<24>, &mod<25>, &mod<26>, &mod<27>, &mod<28>, &mod<29>, &mod<30>, &mod<31>, &mod<32>, 266 | &mod<33>, &mod<34>, &mod<35>, &mod<36>, &mod<37> , &mod<38>, &mod<39>, 267 | #endif 268 | #if SIZE_MAX >= ULLONG_MAX 269 | &mod<40>, &mod<41>, &mod<42>, &mod<43>, &mod<44>, &mod<45>, &mod<46>, &mod<47>, &mod<48>, &mod<49>, 270 | &mod<50>, 271 | #endif 272 | }}; 273 | 274 | } 275 | 276 | /** 277 | * Grow the hash table by using prime numbers as bucket count. Slower than tsl::rh::power_of_two_growth_policy in 278 | * general but will probably distribute the values around better in the buckets with a poor hash function. 279 | * 280 | * To allow the compiler to optimize the modulo operation, a lookup table is used with constant primes numbers. 281 | * 282 | * With a switch the code would look like: 283 | * \code 284 | * switch(iprime) { // iprime is the current prime of the hash table 285 | * case 0: hash % 5ul; 286 | * break; 287 | * case 1: hash % 17ul; 288 | * break; 289 | * case 2: hash % 29ul; 290 | * break; 291 | * ... 292 | * } 293 | * \endcode 294 | * 295 | * Due to the constant variable in the modulo the compiler is able to optimize the operation 296 | * by a series of multiplications, substractions and shifts. 297 | * 298 | * The 'hash % 5' could become something like 'hash - (hash * 0xCCCCCCCD) >> 34) * 5' in a 64 bits environment. 299 | */ 300 | class prime_growth_policy { 301 | public: 302 | explicit prime_growth_policy(std::size_t& min_bucket_count_in_out) { 303 | auto it_prime = std::lower_bound(detail::PRIMES.begin(), 304 | detail::PRIMES.end(), min_bucket_count_in_out); 305 | if(it_prime == detail::PRIMES.end()) { 306 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 307 | } 308 | 309 | m_iprime = static_cast(std::distance(detail::PRIMES.begin(), it_prime)); 310 | if(min_bucket_count_in_out > 0) { 311 | min_bucket_count_in_out = *it_prime; 312 | } 313 | else { 314 | min_bucket_count_in_out = 0; 315 | } 316 | } 317 | 318 | std::size_t bucket_for_hash(std::size_t hash) const noexcept { 319 | return detail::MOD_PRIME[m_iprime](hash); 320 | } 321 | 322 | std::size_t next_bucket_count() const { 323 | if(m_iprime + 1 >= detail::PRIMES.size()) { 324 | TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size."); 325 | } 326 | 327 | return detail::PRIMES[m_iprime + 1]; 328 | } 329 | 330 | std::size_t max_bucket_count() const { 331 | return detail::PRIMES.back(); 332 | } 333 | 334 | void clear() noexcept { 335 | m_iprime = 0; 336 | } 337 | 338 | private: 339 | unsigned int m_iprime; 340 | 341 | static_assert(std::numeric_limits::max() >= detail::PRIMES.size(), 342 | "The type of m_iprime is not big enough."); 343 | }; 344 | 345 | } 346 | } 347 | 348 | #endif 349 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | 3 | project(tsl_robin_map_tests) 4 | 5 | add_executable(tsl_robin_map_tests "main.cpp" 6 | "custom_allocator_tests.cpp" 7 | "policy_tests.cpp" 8 | "robin_map_tests.cpp" 9 | "robin_set_tests.cpp") 10 | 11 | target_compile_features(tsl_robin_map_tests PRIVATE cxx_std_11) 12 | 13 | if(CMAKE_CXX_COMPILER_ID MATCHES "Clang" OR CMAKE_CXX_COMPILER_ID MATCHES "GNU") 14 | target_compile_options(tsl_robin_map_tests PRIVATE -Werror -Wall -Wextra -Wold-style-cast -DTSL_DEBUG -UNDEBUG) 15 | elseif(CMAKE_CXX_COMPILER_ID MATCHES "MSVC") 16 | target_compile_options(tsl_robin_map_tests PRIVATE /bigobj /WX /W3 /DTSL_DEBUG /UNDEBUG) 17 | endif() 18 | 19 | # Boost::unit_test_framework 20 | find_package(Boost 1.54.0 REQUIRED COMPONENTS unit_test_framework) 21 | target_link_libraries(tsl_robin_map_tests PRIVATE Boost::unit_test_framework) 22 | 23 | # tsl::robin_map 24 | add_subdirectory(../ ${CMAKE_CURRENT_BINARY_DIR}/tsl) 25 | target_link_libraries(tsl_robin_map_tests PRIVATE tsl::robin_map) 26 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tests/custom_allocator_tests.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2017 Thibaut Goetghebuer-Planchon 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | #define BOOST_TEST_DYN_LINK 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include "utils.h" 38 | 39 | 40 | static std::size_t nb_custom_allocs = 0; 41 | 42 | template 43 | class custom_allocator { 44 | public: 45 | using value_type = T; 46 | using pointer = T*; 47 | using const_pointer = const T*; 48 | using reference = T&; 49 | using const_reference = const T&; 50 | using size_type = std::size_t; 51 | using difference_type = std::ptrdiff_t; 52 | using propagate_on_container_move_assignment = std::true_type; 53 | 54 | 55 | template 56 | struct rebind { 57 | using other = custom_allocator; 58 | }; 59 | 60 | custom_allocator() = default; 61 | custom_allocator(const custom_allocator&) = default; 62 | 63 | template 64 | custom_allocator(const custom_allocator&) { 65 | } 66 | 67 | pointer address(reference x) const noexcept { 68 | return &x; 69 | } 70 | 71 | const_pointer address(const_reference x) const noexcept { 72 | return &x; 73 | } 74 | 75 | pointer allocate(size_type n, const void* /*hint*/ = 0) { 76 | nb_custom_allocs++; 77 | 78 | pointer ptr = static_cast(std::malloc(n * sizeof(T))); 79 | if(ptr == nullptr) { 80 | #ifdef TSL_RH_NO_EXCEPTIONS 81 | std::abort(); 82 | #else 83 | throw std::bad_alloc(); 84 | #endif 85 | } 86 | 87 | return ptr; 88 | } 89 | 90 | void deallocate(T* p, size_type /*n*/) { 91 | std::free(p); 92 | } 93 | 94 | size_type max_size() const noexcept { 95 | return std::numeric_limits::max()/sizeof(value_type); 96 | } 97 | 98 | template 99 | void construct(U* p, Args&&... args) { 100 | ::new(static_cast(p)) U(std::forward(args)...); 101 | } 102 | 103 | template 104 | void destroy(U* p) { 105 | p->~U(); 106 | } 107 | }; 108 | 109 | template 110 | bool operator==(const custom_allocator&, const custom_allocator&) { 111 | return true; 112 | } 113 | 114 | template 115 | bool operator!=(const custom_allocator&, const custom_allocator&) { 116 | return false; 117 | } 118 | 119 | 120 | 121 | //TODO Avoid overloading new to check number of global new 122 | // static std::size_t nb_global_new = 0; 123 | // void* operator new(std::size_t sz) { 124 | // nb_global_new++; 125 | // return std::malloc(sz); 126 | // } 127 | // 128 | // void operator delete(void* ptr) noexcept { 129 | // std::free(ptr); 130 | // } 131 | 132 | BOOST_AUTO_TEST_SUITE(test_custom_allocator) 133 | 134 | BOOST_AUTO_TEST_CASE(test_custom_allocator_1) { 135 | // nb_global_new = 0; 136 | nb_custom_allocs = 0; 137 | 138 | tsl::robin_map, std::equal_to, 139 | custom_allocator>> map; 140 | 141 | const int nb_elements = 1000; 142 | for(int i = 0; i < nb_elements; i++) { 143 | map.insert({i, i*2}); 144 | } 145 | 146 | BOOST_CHECK_NE(nb_custom_allocs, 0); 147 | // BOOST_CHECK_EQUAL(nb_global_new, 0); 148 | } 149 | 150 | BOOST_AUTO_TEST_SUITE_END() 151 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tests/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2017 Thibaut Goetghebuer-Planchon 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | #define BOOST_TEST_MODULE robin_map_tests 25 | #define BOOST_TEST_DYN_LINK 26 | 27 | 28 | #include 29 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tests/policy_tests.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2017 Thibaut Goetghebuer-Planchon 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | #define BOOST_TEST_DYN_LINK 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include "utils.h" 35 | 36 | 37 | BOOST_AUTO_TEST_SUITE(test_policy) 38 | 39 | using test_types = boost::mpl::list, 40 | tsl::rh::power_of_two_growth_policy<4>, 41 | tsl::rh::prime_growth_policy, 42 | tsl::rh::mod_growth_policy<>, 43 | tsl::rh::mod_growth_policy>>; 44 | 45 | 46 | BOOST_AUTO_TEST_CASE_TEMPLATE(test_policy, Policy, test_types) { 47 | // Call next_bucket_count() on the policy until we reach its max_bucket_count() 48 | std::size_t bucket_count = 0; 49 | Policy policy(bucket_count); 50 | 51 | BOOST_CHECK_EQUAL(policy.bucket_for_hash(0), 0); 52 | BOOST_CHECK_EQUAL(bucket_count, 0); 53 | 54 | #ifndef TSL_RH_NO_EXCEPTIONS 55 | bool exception_thrown = false; 56 | try { 57 | while(true) { 58 | const std::size_t previous_bucket_count = bucket_count; 59 | 60 | bucket_count = policy.next_bucket_count(); 61 | policy = Policy(bucket_count); 62 | 63 | BOOST_CHECK_EQUAL(policy.bucket_for_hash(0), 0); 64 | BOOST_CHECK(bucket_count > previous_bucket_count); 65 | } 66 | } 67 | catch(const std::length_error& ) { 68 | exception_thrown = true; 69 | } 70 | 71 | BOOST_CHECK(exception_thrown); 72 | #endif 73 | } 74 | 75 | BOOST_AUTO_TEST_CASE_TEMPLATE(test_policy_min_bucket_count, Policy, test_types) { 76 | // Check policy when a bucket_count of 0 is asked. 77 | std::size_t bucket_count = 0; 78 | Policy policy(bucket_count); 79 | 80 | BOOST_CHECK_EQUAL(policy.bucket_for_hash(0), 0); 81 | } 82 | 83 | BOOST_AUTO_TEST_CASE_TEMPLATE(test_policy_max_bucket_count, Policy, test_types) { 84 | // Test a bucket_count equals to the max_bucket_count limit and above 85 | std::size_t bucket_count = 0; 86 | Policy policy(bucket_count); 87 | 88 | 89 | bucket_count = policy.max_bucket_count(); 90 | Policy policy2(bucket_count); 91 | 92 | 93 | bucket_count = std::numeric_limits::max(); 94 | TSL_RH_CHECK_THROW((Policy(bucket_count)), std::length_error); 95 | 96 | 97 | bucket_count = policy.max_bucket_count() + 1; 98 | TSL_RH_CHECK_THROW((Policy(bucket_count)), std::length_error); 99 | } 100 | 101 | 102 | BOOST_AUTO_TEST_SUITE_END() 103 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tests/robin_set_tests.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2017 Thibaut Goetghebuer-Planchon 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | #define BOOST_TEST_DYN_LINK 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include "utils.h" 39 | 40 | 41 | BOOST_AUTO_TEST_SUITE(test_robin_set) 42 | 43 | using test_types = boost::mpl::list, 44 | tsl::robin_set, 45 | tsl::robin_set, 46 | tsl::robin_set, 47 | tsl::robin_pg_set, 48 | tsl::robin_set, 50 | std::equal_to, 51 | std::allocator, 52 | true, 53 | tsl::rh::prime_growth_policy>, 54 | tsl::robin_set, 56 | std::equal_to, 57 | std::allocator, 58 | true, 59 | tsl::rh::mod_growth_policy<>>, 60 | tsl::robin_set, 62 | std::equal_to, 63 | std::allocator, 64 | false, 65 | tsl::rh::mod_growth_policy<>> 66 | >; 67 | 68 | 69 | 70 | BOOST_AUTO_TEST_CASE_TEMPLATE(test_insert, HSet, test_types) { 71 | // insert x values, insert them again, check values 72 | using key_t = typename HSet::key_type; 73 | 74 | const std::size_t nb_values = 1000; 75 | HSet set; 76 | typename HSet::iterator it; 77 | bool inserted; 78 | 79 | for(std::size_t i = 0; i < nb_values; i++) { 80 | std::tie(it, inserted) = set.insert(utils::get_key(i)); 81 | 82 | BOOST_CHECK_EQUAL(*it, utils::get_key(i)); 83 | BOOST_CHECK(inserted); 84 | } 85 | BOOST_CHECK_EQUAL(set.size(), nb_values); 86 | 87 | for(std::size_t i = 0; i < nb_values; i++) { 88 | std::tie(it, inserted) = set.insert(utils::get_key(i)); 89 | 90 | BOOST_CHECK_EQUAL(*it, utils::get_key(i)); 91 | BOOST_CHECK(!inserted); 92 | } 93 | 94 | for(std::size_t i = 0; i < nb_values; i++) { 95 | it = set.find(utils::get_key(i)); 96 | 97 | BOOST_CHECK_EQUAL(*it, utils::get_key(i)); 98 | } 99 | } 100 | 101 | BOOST_AUTO_TEST_CASE(test_compare) { 102 | const tsl::robin_set set1 = {"a", "e", "d", "c", "b"}; 103 | const tsl::robin_set set1_copy = {"e", "c", "b", "a", "d"}; 104 | const tsl::robin_set set2 = {"e", "c", "b", "a", "d", "f"}; 105 | const tsl::robin_set set3 = {"e", "c", "b", "a"}; 106 | const tsl::robin_set set4 = {"a", "e", "d", "c", "z"}; 107 | 108 | BOOST_CHECK(set1 == set1_copy); 109 | BOOST_CHECK(set1_copy == set1); 110 | 111 | BOOST_CHECK(set1 != set2); 112 | BOOST_CHECK(set2 != set1); 113 | 114 | BOOST_CHECK(set1 != set3); 115 | BOOST_CHECK(set3 != set1); 116 | 117 | BOOST_CHECK(set1 != set4); 118 | BOOST_CHECK(set4 != set1); 119 | 120 | BOOST_CHECK(set2 != set3); 121 | BOOST_CHECK(set3 != set2); 122 | 123 | BOOST_CHECK(set2 != set4); 124 | BOOST_CHECK(set4 != set2); 125 | 126 | BOOST_CHECK(set3 != set4); 127 | BOOST_CHECK(set4 != set3); 128 | } 129 | 130 | BOOST_AUTO_TEST_CASE(test_insert_pointer) { 131 | // Test added mainly to be sure that the code compiles with MSVC due to a bug in the compiler. 132 | // See robin_hash::insert_value_impl for details. 133 | std::string value; 134 | std::string* value_ptr = &value; 135 | 136 | tsl::robin_set set; 137 | set.insert(value_ptr); 138 | set.emplace(value_ptr); 139 | 140 | BOOST_CHECK_EQUAL(set.size(), 1); 141 | BOOST_CHECK_EQUAL(**set.begin(), value); 142 | } 143 | 144 | BOOST_AUTO_TEST_SUITE_END() 145 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tests/utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2017 Thibaut Goetghebuer-Planchon 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | #ifndef TSL_UTILS_H 25 | #define TSL_UTILS_H 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #ifdef TSL_RH_NO_EXCEPTIONS 39 | #define TSL_RH_CHECK_THROW(S, E) 40 | #else 41 | #define TSL_RH_CHECK_THROW(S, E) BOOST_CHECK_THROW(S, E) 42 | #endif 43 | 44 | template 45 | class identity_hash { 46 | public: 47 | std::size_t operator()(const T& value) const { 48 | return static_cast(value); 49 | } 50 | }; 51 | 52 | template 53 | class mod_hash { 54 | public: 55 | template 56 | std::size_t operator()(const T& value) const { 57 | return std::hash()(value) % MOD; 58 | } 59 | }; 60 | 61 | class self_reference_member_test { 62 | public: 63 | self_reference_member_test() : m_value(std::to_string(-1)), m_value_ptr(&m_value) { 64 | } 65 | 66 | explicit self_reference_member_test(std::int64_t value) : m_value(std::to_string(value)), m_value_ptr(&m_value) { 67 | } 68 | 69 | self_reference_member_test(const self_reference_member_test& other) : m_value(*other.m_value_ptr), m_value_ptr(&m_value) { 70 | } 71 | 72 | self_reference_member_test(self_reference_member_test&& other) : m_value(*other.m_value_ptr), m_value_ptr(&m_value) { 73 | } 74 | 75 | self_reference_member_test& operator=(const self_reference_member_test& other) { 76 | m_value = *other.m_value_ptr; 77 | m_value_ptr = &m_value; 78 | 79 | return *this; 80 | } 81 | 82 | self_reference_member_test& operator=(self_reference_member_test&& other) { 83 | m_value = *other.m_value_ptr; 84 | m_value_ptr = &m_value; 85 | 86 | return *this; 87 | } 88 | 89 | std::string value() const { 90 | return *m_value_ptr; 91 | } 92 | 93 | friend std::ostream& operator<<(std::ostream& stream, const self_reference_member_test& value) { 94 | stream << *value.m_value_ptr; 95 | 96 | return stream; 97 | } 98 | 99 | friend bool operator==(const self_reference_member_test& lhs, const self_reference_member_test& rhs) { 100 | return *lhs.m_value_ptr == *rhs.m_value_ptr; 101 | } 102 | 103 | friend bool operator!=(const self_reference_member_test& lhs, const self_reference_member_test& rhs) { 104 | return !(lhs == rhs); 105 | } 106 | 107 | friend bool operator<(const self_reference_member_test& lhs, const self_reference_member_test& rhs) { 108 | return *lhs.m_value_ptr < *rhs.m_value_ptr; 109 | } 110 | private: 111 | std::string m_value; 112 | std::string* m_value_ptr; 113 | }; 114 | 115 | 116 | class move_only_test { 117 | public: 118 | explicit move_only_test(std::int64_t value) : m_value(new std::string(std::to_string(value))) { 119 | } 120 | 121 | move_only_test(const move_only_test&) = delete; 122 | move_only_test(move_only_test&&) = default; 123 | move_only_test& operator=(const move_only_test&) = delete; 124 | move_only_test& operator=(move_only_test&&) = default; 125 | 126 | friend std::ostream& operator<<(std::ostream& stream, const move_only_test& value) { 127 | if(value.m_value == nullptr) { 128 | stream << "null"; 129 | } 130 | else { 131 | stream << *value.m_value; 132 | } 133 | 134 | return stream; 135 | } 136 | 137 | friend bool operator==(const move_only_test& lhs, const move_only_test& rhs) { 138 | if(lhs.m_value == nullptr || rhs.m_value == nullptr) { 139 | return lhs.m_value == nullptr && rhs.m_value == nullptr; 140 | } 141 | else { 142 | return *lhs.m_value == *rhs.m_value; 143 | } 144 | } 145 | 146 | friend bool operator!=(const move_only_test& lhs, const move_only_test& rhs) { 147 | return !(lhs == rhs); 148 | } 149 | 150 | friend bool operator<(const move_only_test& lhs, const move_only_test& rhs) { 151 | if(lhs.m_value == nullptr && rhs.m_value == nullptr) { 152 | return false; 153 | } 154 | else if(lhs.m_value == nullptr) { 155 | return true; 156 | } 157 | else if(rhs.m_value == nullptr) { 158 | return false; 159 | } 160 | else { 161 | return *lhs.m_value < *rhs.m_value; 162 | } 163 | } 164 | 165 | const std::string& value() const { 166 | return *m_value; 167 | } 168 | 169 | private: 170 | std::unique_ptr m_value; 171 | }; 172 | 173 | 174 | class copy_only_test { 175 | public: 176 | explicit copy_only_test(std::int64_t value): m_value(std::to_string(value)) { 177 | } 178 | 179 | copy_only_test(const copy_only_test& other): m_value(other.m_value) { 180 | } 181 | 182 | copy_only_test& operator=(const copy_only_test& other) { 183 | m_value = other.m_value; 184 | 185 | return *this; 186 | } 187 | 188 | ~copy_only_test() { 189 | } 190 | 191 | 192 | friend std::ostream& operator<<(std::ostream& stream, const copy_only_test& value) { 193 | stream << value.m_value; 194 | 195 | return stream; 196 | } 197 | 198 | friend bool operator==(const copy_only_test& lhs, const copy_only_test& rhs) { 199 | return lhs.m_value == rhs.m_value; 200 | } 201 | 202 | friend bool operator!=(const copy_only_test& lhs, const copy_only_test& rhs) { 203 | return !(lhs == rhs); 204 | } 205 | 206 | friend bool operator<(const copy_only_test& lhs, const copy_only_test& rhs) { 207 | return lhs.m_value < rhs.m_value; 208 | } 209 | 210 | std::string value() const { 211 | return m_value; 212 | } 213 | 214 | private: 215 | std::string m_value; 216 | }; 217 | 218 | 219 | 220 | namespace std { 221 | template<> 222 | struct hash { 223 | std::size_t operator()(const self_reference_member_test& val) const { 224 | return std::hash()(val.value()); 225 | } 226 | }; 227 | 228 | template<> 229 | struct hash { 230 | std::size_t operator()(const move_only_test& val) const { 231 | return std::hash()(val.value()); 232 | } 233 | }; 234 | 235 | template<> 236 | struct hash { 237 | std::size_t operator()(const copy_only_test& val) const { 238 | return std::hash()(val.value()); 239 | } 240 | }; 241 | } 242 | 243 | 244 | class utils { 245 | public: 246 | template 247 | static T get_key(std::size_t counter); 248 | 249 | template 250 | static T get_value(std::size_t counter); 251 | 252 | template 253 | static HMap get_filled_hash_map(std::size_t nb_elements); 254 | }; 255 | 256 | 257 | 258 | template<> 259 | inline std::int64_t utils::get_key(std::size_t counter) { 260 | return tsl::detail_robin_hash::numeric_cast(counter); 261 | } 262 | 263 | template<> 264 | inline self_reference_member_test utils::get_key(std::size_t counter) { 265 | return self_reference_member_test(tsl::detail_robin_hash::numeric_cast(counter)); 266 | } 267 | 268 | template<> 269 | inline std::string utils::get_key(std::size_t counter) { 270 | return "Key " + std::to_string(counter); 271 | } 272 | 273 | template<> 274 | inline move_only_test utils::get_key(std::size_t counter) { 275 | return move_only_test(tsl::detail_robin_hash::numeric_cast(counter)); 276 | } 277 | 278 | template<> 279 | inline copy_only_test utils::get_key(std::size_t counter) { 280 | return copy_only_test(tsl::detail_robin_hash::numeric_cast(counter)); 281 | } 282 | 283 | 284 | 285 | 286 | template<> 287 | inline std::int64_t utils::get_value(std::size_t counter) { 288 | return tsl::detail_robin_hash::numeric_cast(counter*2); 289 | } 290 | 291 | template<> 292 | inline self_reference_member_test utils::get_value(std::size_t counter) { 293 | return self_reference_member_test(tsl::detail_robin_hash::numeric_cast(counter*2)); 294 | } 295 | 296 | template<> 297 | inline std::string utils::get_value(std::size_t counter) { 298 | return "Value " + std::to_string(counter); 299 | } 300 | 301 | template<> 302 | inline move_only_test utils::get_value(std::size_t counter) { 303 | return move_only_test(tsl::detail_robin_hash::numeric_cast(counter*2)); 304 | } 305 | 306 | template<> 307 | inline copy_only_test utils::get_value(std::size_t counter) { 308 | return copy_only_test(tsl::detail_robin_hash::numeric_cast(counter*2)); 309 | } 310 | 311 | 312 | 313 | template 314 | inline HMap utils::get_filled_hash_map(std::size_t nb_elements) { 315 | using key_t = typename HMap::key_type; using value_t = typename HMap:: mapped_type; 316 | 317 | HMap map; 318 | map.reserve(nb_elements); 319 | 320 | for(std::size_t i = 0; i < nb_elements; i++) { 321 | map.insert({utils::get_key(i), utils::get_value(i)}); 322 | } 323 | 324 | return map; 325 | } 326 | 327 | #endif 328 | -------------------------------------------------------------------------------- /sr_lio/thirdLibrary/tessil-src/tsl-robin-map.natvis: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | {{ size={m_ht.m_nb_elements} }} 9 | 10 | m_ht.m_buckets_data._Mypair._Myval2._Mylast - m_ht.m_buckets_data._Mypair._Myval2._Myfirst 11 | 12 | ((float)m_ht.m_nb_elements) / ((float)(m_ht.m_buckets_data._Mypair._Myval2._Mylast - m_ht.m_buckets_data._Mypair._Myval2._Myfirst)) 13 | 14 | 15 | 0 16 | 17 | m_ht.m_max_load_factor 18 | 19 | 20 | 21 | m_ht.m_nb_elements 22 | 23 | *bucket 24 | 25 | ++bucket 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | {{ size={m_ht.m_nb_elements} }} 34 | 35 | m_ht.m_buckets_data._Mypair._Myval2._Mylast - m_ht.m_buckets_data._Mypair._Myval2._Myfirst 36 | 37 | ((float)m_ht.m_nb_elements) / ((float)(m_ht.m_buckets_data._Mypair._Myval2._Mylast - m_ht.m_buckets_data._Mypair._Myval2._Myfirst)) 38 | 39 | 40 | 0 41 | 42 | m_ht.m_max_load_factor 43 | 44 | 45 | 46 | m_ht.m_nb_elements 47 | 48 | *bucket 49 | 50 | ++bucket 51 | 52 | 53 | 54 | 55 | 56 | 57 | {*m_bucket} 58 | 59 | *m_bucket 60 | 61 | 62 | 63 | 64 | empty 65 | {*reinterpret_cast<$T1*>(&m_value)} 66 | 67 | *reinterpret_cast<$T1*>(&m_value) 68 | 69 | 70 | 71 | 72 | empty 73 | {reinterpret_cast<$T1*>(&m_value)->second} 74 | 75 | *reinterpret_cast<$T1*>(&m_value) 76 | 77 | 78 | 79 | --------------------------------------------------------------------------------