├── img
    ├── gate_03.png
    ├── street_04.png
    ├── street_04_com.png
    └── gate_03_compare.png
├── include
    ├── ikd-Tree
    │   ├── README.md
    │   └── ikd_Tree.h
    ├── utility.h
    ├── common_lib.h
    ├── use-ikfom.hpp
    └── esekfom.hpp
├── msg
    └── Pose6D.msg
├── launch
    ├── mapping_horizon.launch
    ├── mapping_avia.launch
    ├── mapping_ouster64.launch
    ├── mapping_mid360.launch
    ├── mapping_velodyne.launch
    ├── mapping_rs.launch
    ├── mapping_rs_relocalization.launch
    ├── mapping_velodyne_m2dgr.launch
    ├── mapping_velodyne_utbm.launch
    └── mapping_velodyne_liosam.launch
├── README.md
├── config
    ├── rslidar.yaml
    ├── mid360.yaml
    ├── horizon.yaml
    ├── avia.yaml
    ├── ouster64.yaml
    ├── velodyne_utbm.yaml
    ├── velodyne.yaml
    ├── velodyne_liosam.yaml
    └── velodyne_m2dgr.yaml
├── package.xml
├── CMakeLists.txt
├── src
    ├── preprocess.h
    ├── IMU_Processing.hpp
    ├── laserMapping_re.cpp
    ├── preprocess.cpp
    └── laserMapping.cpp
└── rviz_cfg
    ├── relocalization.rviz
    └── loam_livox.rviz


/img/gate_03.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/USTC-AIS-Lab/SI-LIO/HEAD/img/gate_03.png


--------------------------------------------------------------------------------
/img/street_04.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/USTC-AIS-Lab/SI-LIO/HEAD/img/street_04.png


--------------------------------------------------------------------------------
/img/street_04_com.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/USTC-AIS-Lab/SI-LIO/HEAD/img/street_04_com.png


--------------------------------------------------------------------------------
/img/gate_03_compare.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/USTC-AIS-Lab/SI-LIO/HEAD/img/gate_03_compare.png


--------------------------------------------------------------------------------
/include/ikd-Tree/README.md:
--------------------------------------------------------------------------------
1 | # ikd-Tree
2 | ikd-Tree is an incremental k-d tree for robotic applications.
3 | 


--------------------------------------------------------------------------------
/include/utility.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #include <iostream>
 3 | #include <Eigen/Dense>
 4 | 
 5 | template <typename Derived>
 6 | static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
 7 | {
 8 |     Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
 9 |     ans << typename Derived::Scalar(0), -q(2), q(1),
10 |         q(2), typename Derived::Scalar(0), -q(0),
11 |         -q(1), q(0), typename Derived::Scalar(0);
12 |     return ans;
13 | }
14 | void printtest()
15 | {
16 |     std::cout << "test" << std::endl;
17 | }


--------------------------------------------------------------------------------
/msg/Pose6D.msg:
--------------------------------------------------------------------------------
1 | # the preintegrated Lidar states at the time of IMU measurements in a frame
2 | float64  offset_time # the offset time of IMU measurement w.r.t the first lidar point
3 | float64[3] acc       # the preintegrated total acceleration (global frame) at the Lidar origin
4 | float64[3] gyr       # the unbiased angular velocity (body frame) at the Lidar origin
5 | float64[3] vel       # the preintegrated velocity (global frame) at the Lidar origin
6 | float64[3] pos       # the preintegrated position (global frame) at the Lidar origin
7 | float64[9] rot       # the preintegrated rotation (global frame) at the Lidar origin


--------------------------------------------------------------------------------
/launch/mapping_horizon.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | <!-- Launch file for Livox Horizon LiDAR -->
 3 | 
 4 | 	<arg name="rviz" default="true" />
 5 | 
 6 | 	<rosparam command="load" file="$(find sfast_lio)/config/horizon.yaml" />
 7 | 
 8 | 	<param name="feature_extract_enable" type="bool" value="0"/>
 9 | 	<param name="point_filter_num" type="int" value="3"/>
10 | 	<param name="max_iteration" type="int" value="3" />
11 | 	<param name="filter_size_surf" type="double" value="0.5" />
12 | 	<param name="filter_size_map" type="double" value="0.5" />
13 | 	<param name="cube_side_length" type="double" value="1000" />
14 |     <node pkg="sfast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
15 | 
16 | 	<group if="$(arg rviz)">
17 | 	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find sfast_lio)/rviz_cfg/loam_livox.rviz" />
18 | 	</group>
19 | 
20 | </launch>


--------------------------------------------------------------------------------
/launch/mapping_avia.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="rviz" default="true" />
 3 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="Invarian_fastlio" output="screen"> 
 4 |       <rosparam command="load" file="$(find inva_fast_lio)/config/avia.yaml" />
 5 | 
 6 |       <param name="feature_extract_enable" type="bool" value="0"/>
 7 |       <param name="point_filter_num" type="int" value="2"/>
 8 |       <param name="max_iteration" type="int" value="3" />
 9 |       <param name="filter_size_surf" type="double" value="0.5" />
10 |       <param name="filter_size_map" type="double" value="0.5" />
11 |       <param name="cube_side_length" type="double" value="1000" />
12 |     </node>
13 | 
14 |     <group if="$(arg rviz)">
15 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
16 |     </group>
17 | </launch>


--------------------------------------------------------------------------------
/launch/mapping_ouster64.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | <!-- Launch file for ouster OS2-64 LiDAR -->
 3 | 
 4 |     <arg name="rviz" default="true" />
 5 | 
 6 |     <rosparam command="load" file="$(find sfast_lio)/config/ouster64.yaml" />
 7 | 
 8 |     <param name="feature_extract_enable" type="bool" value="0"/>
 9 |     <param name="point_filter_num" type="int" value="4"/>
10 |     <param name="max_iteration" type="int" value="3" />
11 |     <param name="filter_size_surf" type="double" value="0.5" />
12 |     <param name="filter_size_map" type="double" value="0.5" />
13 |     <param name="cube_side_length" type="double" value="1000" />
14 |     <node pkg="sfast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
15 | 
16 |     <group if="$(arg rviz)">
17 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find sfast_lio)/rviz_cfg/loam_livox.rviz" />
18 |     </group>
19 | 
20 | </launch>
21 | 


--------------------------------------------------------------------------------
/launch/mapping_mid360.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | <!-- Launch file for Livox MID360 LiDAR -->
 3 | 
 4 | 	<arg name="rviz" default="true" />
 5 | 
 6 | 	<rosparam command="load" file="$(find inva_fast_lio)/config/mid360.yaml" />
 7 | 
 8 | 	<param name="feature_extract_enable" type="bool" value="0"/>
 9 | 	<param name="point_filter_num" type="int" value="3"/>
10 | 	<param name="max_iteration" type="int" value="3" />
11 | 	<param name="filter_size_surf" type="double" value="0.5" />
12 | 	<param name="filter_size_map" type="double" value="0.5" />
13 | 	<param name="cube_side_length" type="double" value="1000" />
14 | 	<param name="runtime_pos_log_enable" type="bool" value="0" />
15 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="laserMapping" output="screen"/> 
16 | 
17 | 	<group if="$(arg rviz)">
18 | 	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
19 | 	</group>
20 | 
21 | </launch>


--------------------------------------------------------------------------------
/launch/mapping_velodyne.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- Launch file for velodyne16 VLP-16 LiDAR -->
 3 | 
 4 |     <arg name="rviz" default="true" />
 5 | 
 6 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="Invarian_fastlio" output="screen"> 
 7 |       <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne.yaml" />
 8 | 
 9 |       <param name="feature_extract_enable" type="bool" value="0"/>
10 |       <param name="point_filter_num" type="int" value="4"/>
11 |       <param name="max_iteration" type="int" value="3" />
12 |       <param name="filter_size_surf" type="double" value="0.5" />
13 |       <param name="filter_size_map" type="double" value="0.5" />
14 |       <param name="cube_side_length" type="double" value="1000" />
15 |     </node>
16 | 
17 |     <group if="$(arg rviz)">
18 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
19 |     </group>
20 | 
21 | </launch>


--------------------------------------------------------------------------------
/launch/mapping_rs.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | <!-- Launch file for Livox AVIA LiDAR -->
 3 | 
 4 | 	<arg name="rviz" default="true" />
 5 | 
 6 | 	<rosparam command="load" file="$(find sfast_lio)/config/rslidar.yaml" />
 7 | 
 8 | 	<param name="feature_extract_enable" type="bool" value="0"/>
 9 | 	<!-- 100HZ的bag  point_filter_num建议设置为1;   10HZ的bag建议设置为2或3 -->
10 | 	<param name="point_filter_num" type="int" value="2"/>               
11 | 	<param name="max_iteration" type="int" value="4" />
12 | 	<param name="filter_size_surf" type="double" value="0.5" />
13 | 	<param name="filter_size_map" type="double" value="0.5" />
14 | 	<param name="cube_side_length" type="double" value="1000" />
15 |     <node pkg="sfast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
16 | 
17 | 	<group if="$(arg rviz)">
18 | 	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rvizzzzzzzz" args="-d $(find sfast_lio)/rviz_cfg/loam_livox.rviz" />
19 | 	</group>
20 | 
21 | </launch>
22 | 


--------------------------------------------------------------------------------
/launch/mapping_rs_relocalization.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | <!-- Launch file for Livox AVIA LiDAR -->
 3 | 
 4 | 	<arg name="rviz" default="true" />
 5 | 
 6 | 	<rosparam command="load" file="$(find sfast_lio)/config/rslidar.yaml" />
 7 | 
 8 | 	<param name="feature_extract_enable" type="bool" value="0"/>
 9 | 	<!-- 100HZ的bag  point_filter_num建议设置为1;   10HZ的bag建议设置为2或3 -->
10 | 	<param name="point_filter_num" type="int" value="2"/>               
11 | 	<param name="max_iteration" type="int" value="4" />
12 | 	<param name="filter_size_surf" type="double" value="0.5" />
13 | 	<param name="filter_size_map" type="double" value="0.5" />
14 | 	<param name="cube_side_length" type="double" value="1000" />
15 |     <node pkg="sfast_lio" type="fastlio_mapping_re" name="laserMapping_re" output="screen" /> 
16 | 
17 | 	<group if="$(arg rviz)">
18 | 	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find sfast_lio)/rviz_cfg/relocalization.rviz" />
19 | 	</group>
20 | 
21 | </launch>
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # SI_LIO
 2 | **SI_LIO** is based on the Invariant-EKF theory and our code is implemented on S-FAST_LIO.
 3 | <p align="center" style="display: flex; justify-content: center;">
 4 |     <img src="./img/gate_03.png" alt="drawing" width="350"/>
 5 |     <img src="./img/gate_03_compare.png" alt="drawing" width="350"/>
 6 | </p>
 7 | 
 8 | Through theoretical analysis, it can be deduced that our IEKF-based estimation method achieves higher estimation accuracy compared to the Iterated EKF used in FAST-LIO2, particularly in scenarios with large IMU prediction errors. This experimental outcome corroborates our analytical conclusions.
 9 | 
10 | ## 1. Dependices
11 | Sophus
12 | Eigen
13 | PCL
14 | livox_ros_driver
15 | 
16 | ## 2. Build
17 | Clone the repository and catkin_make:
18 | 
19 | ```
20 | cd ~/catkin_ws/src
21 | git clone https://github.com/USTC-AIS-Lab/SI-LIO.git
22 | cd ../
23 | catkin_make -j
24 | source ~/catkin_ws/devel/setup.bash
25 | ```
26 | 
27 | ## 3. Run
28 | We recommend using the [M2DGR](https://github.com/SJTU-ViSYS/M2DGR) dataset.
29 | ```
30 | roslaunch inva_fast_lio mapping_velodyne_m2dgr.launch
31 | rosbag play street_04.bag
32 | ```
33 | 
34 | ## 4. Acknowledgements
35 | Thanks for the authors of [S-FAST-LIO](https://github.com/zlwang7/S-FAST_LIO.git) and [FAST-LIO](https://github.com/hku-mars/FAST_LIO).
36 | 
37 | 


--------------------------------------------------------------------------------
/config/rslidar.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 | #RS LiDar
 3 |     lid_topic: "/rslidar_points"
 4 |     imu_topic: "/imu/data"
 5 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 6 |     
 7 | preprocess:
 8 |     lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 RS
 9 |     scan_line: 32
10 |     scan_rate: 10                # only need to be set for velodyne, unit: Hz,
11 |     blind: 3
12 | 
13 | mapping:
14 |     acc_cov: 0.1
15 |     gyr_cov: 0.1
16 |     b_acc_cov: 0.0001
17 |     b_gyr_cov: 0.0001
18 |     fov_degree:    180
19 |     det_range:     100.0
20 |     extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
21 |     init_pos: [1.0, 1.0, 1.0]
22 |     init_rot: [0, 0, 0, 1]
23 |     extrinsic_T: [ 0, 0, -0.1]
24 |     extrinsic_R: [ 1, 0, 0,
25 |                    0, 1, 0,
26 |                    0, 0, 1]
27 | 
28 | publish:
29 |     path_en:  true
30 |     scan_publish_en:  true       # false: close all the point cloud output
31 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
32 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
33 | 
34 | pcd_save:
35 |     pcd_save_en: false
36 |     interval: 100                 # how many LiDAR frames saved in each pcd file; 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>inva_fast_lio</name>
 4 |   <version>0.0.0</version>
 5 | 
 6 |   <description>
 7 |     This is a modified version of LOAM which is original algorithm
 8 |     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 |   </description>
12 | 
13 |   <maintainer email="dev@livoxtech.com">claydergc</maintainer>
14 | 
15 |   <license>BSD</license>
16 | 
17 |   <author email="zhangji@cmu.edu">Ji Zhang</author>
18 |   
19 |   <buildtool_depend>catkin</buildtool_depend>
20 |   <build_depend>geometry_msgs</build_depend>
21 |   <build_depend>nav_msgs</build_depend>
22 |   <build_depend>roscpp</build_depend>
23 |   <build_depend>rospy</build_depend>
24 |   <build_depend>std_msgs</build_depend>
25 |   <build_depend>sensor_msgs</build_depend>
26 |   <build_depend>tf</build_depend>
27 |   <build_depend>pcl_ros</build_depend>
28 |   <build_depend>livox_ros_driver</build_depend>
29 |   <build_depend>message_generation</build_depend>
30 | 
31 |   <run_depend>geometry_msgs</run_depend>
32 |   <run_depend>nav_msgs</run_depend>
33 |   <run_depend>sensor_msgs</run_depend>
34 |   <run_depend>roscpp</run_depend>
35 |   <run_depend>rospy</run_depend>
36 |   <run_depend>std_msgs</run_depend>
37 |   <run_depend>tf</run_depend>
38 |   <run_depend>pcl_ros</run_depend>
39 |   <run_depend>livox_ros_driver</run_depend>
40 |   <run_depend>message_runtime</run_depend>
41 | 
42 |   <test_depend>rostest</test_depend>
43 |   <test_depend>rosbag</test_depend>
44 | 
45 |   <export>
46 |   </export>
47 | </package>
48 | 


--------------------------------------------------------------------------------
/config/mid360.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic:  "/iris_0/scan"
 3 |     imu_topic:  "/mavros/imu/data_raw"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 | 
 8 | preprocess:
 9 |     lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line: 4
11 |     blind: 0.5
12 | 
13 | mapping:
14 |     acc_cov: 0.1
15 |     gyr_cov: 0.1
16 |     b_acc_cov: 0.0001
17 |     b_gyr_cov: 0.0001
18 |     fov_degree:    360
19 |     det_range:     100.0
20 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
21 |     extrinsic_T: [ 0.5, 0, 0.095]
22 |     extrinsic_R: [ 1, 0, 0,
23 |                    0, 1, 0,
24 |                    0, 0, 1]
25 | 
26 | publish:
27 |     path_en:  true
28 |     scan_publish_en:  true       # false: close all the point cloud output
29 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
30 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
31 | 
32 | pcd_save:
33 |     pcd_save_en: false
34 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
35 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
36 | 


--------------------------------------------------------------------------------
/config/horizon.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic:  "/livox/lidar"
 3 |     imu_topic:  "/livox/imu"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 |     
 8 | preprocess:
 9 |     lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line:  6
11 |     blind: 4
12 | 
13 | mapping:
14 |     acc_cov: 0.1
15 |     gyr_cov: 0.1
16 |     b_acc_cov: 0.0001
17 |     b_gyr_cov: 0.0001
18 |     fov_degree:    100
19 |     det_range:     260.0
20 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
21 |     extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
22 |     extrinsic_R: [ 1, 0, 0,
23 |                    0, 1, 0,
24 |                    0, 0, 1]
25 | 
26 | publish:
27 |     path_en:  false
28 |     scan_publish_en:  true       # false: close all the point cloud output
29 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
30 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
31 | 
32 | pcd_save:
33 |     pcd_save_en: false
34 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
35 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
36 | 


--------------------------------------------------------------------------------
/config/avia.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     # lid_topic:  "/iris_0/scan"
 3 |     # imu_topic:  "/iris_0/mavros/imu/data_raw"
 4 |     lid_topic:  "/livox/lidar"
 5 |     imu_topic:  "/livox/imu"
 6 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 7 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 8 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 9 | 
10 | preprocess:
11 |     lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
12 |     scan_line: 6
13 |     blind: 4
14 | 
15 | mapping:
16 |     acc_cov: 0.1
17 |     gyr_cov: 0.1
18 |     b_acc_cov: 0.0001
19 |     b_gyr_cov: 0.0001
20 |     fov_degree:    90
21 |     det_range:     450.0
22 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
23 |     extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
24 |     extrinsic_R: [ 1, 0, 0,
25 |                    0, 1, 0,
26 |                    0, 0, 1]
27 | 
28 | publish:
29 |     path_en:  true
30 |     scan_publish_en:  true       # false: close all the point cloud output
31 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
32 |     scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
33 | 
34 | pcd_save:
35 |     pcd_save_en: false
36 |     interval: 100                 # how many LiDAR frames saved in each pcd file; 
37 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
38 | 


--------------------------------------------------------------------------------
/config/ouster64.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic:  "/ouster_top/points_raw"
 3 |     imu_topic:  "/ouster_top/imu_raw"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 |     
 8 | preprocess:
 9 |     lidar_type: 3                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line: 64
11 |     timestamp_unit: 3                 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
12 |     blind: 2
13 | 
14 | mapping:
15 |     acc_cov: 0.1
16 |     gyr_cov: 0.1
17 |     b_acc_cov: 0.0001
18 |     b_gyr_cov: 0.0001
19 |     fov_degree:    180
20 |     det_range:     150.0
21 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
22 |     extrinsic_T: [ 0.0, 0.0, 0.0 ]
23 |     extrinsic_R: [1, 0, 0,
24 |                   0, 1, 0,
25 |                   0, 0, 1]
26 | 
27 | publish:
28 |     path_en:  false
29 |     scan_publish_en:  true       # false: close all the point cloud output
30 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
31 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
32 | 
33 | pcd_save:
34 |     pcd_save_en: false
35 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
36 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
37 | 


--------------------------------------------------------------------------------
/launch/mapping_velodyne_m2dgr.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- Launch file for velodyne16 VLP-16 LiDAR -->
 3 | 
 4 |     <arg name="rviz" default="true" />
 5 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="Invarian_fastlio" output="screen"> 
 6 |       <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne_m2dgr.yaml" />
 7 | 
 8 |       <param name="feature_extract_enable" type="bool" value="0"/>
 9 |       <param name="point_filter_num" type="int" value="4"/>
10 |       <param name="max_iteration" type="int" value="3" />
11 |       <param name="filter_size_surf" type="double" value="0.5" />
12 |       <param name="filter_size_map" type="double" value="0.5" />
13 |       <param name="cube_side_length" type="double" value="1000" />
14 |     </node>
15 | 
16 |     <group if="$(arg rviz)">
17 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
18 |     </group>
19 | </launch>
20 |     <!-- <arg name="rviz" default="true" />
21 | 
22 |     <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne_utbm.yaml" />
23 | 
24 |     <param name="feature_extract_enable" type="bool" value="0"/>
25 |     <param name="point_filter_num" type="int" value="4"/>
26 |     <param name="max_iteration" type="int" value="3" />
27 |     <param name="filter_size_surf" type="double" value="0.5" />
28 |     <param name="filter_size_map" type="double" value="0.5" />
29 |     <param name="cube_side_length" type="double" value="1000" />
30 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
31 | 
32 |     <group if="$(arg rviz)">
33 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
34 |     </group> -->
35 | 


--------------------------------------------------------------------------------
/launch/mapping_velodyne_utbm.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- Launch file for velodyne16 VLP-16 LiDAR -->
 3 | 
 4 |     <arg name="rviz" default="true" />
 5 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="Invarian_fastlio" output="screen"> 
 6 |       <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne_utbm.yaml" />
 7 | 
 8 |       <param name="feature_extract_enable" type="bool" value="0"/>
 9 |       <param name="point_filter_num" type="int" value="4"/>
10 |       <param name="max_iteration" type="int" value="3" />
11 |       <param name="filter_size_surf" type="double" value="0.5" />
12 |       <param name="filter_size_map" type="double" value="0.5" />
13 |       <param name="cube_side_length" type="double" value="1000" />
14 |     </node>
15 | 
16 |     <group if="$(arg rviz)">
17 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
18 |     </group>
19 | </launch>
20 |     <!-- <arg name="rviz" default="true" />
21 | 
22 |     <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne_utbm.yaml" />
23 | 
24 |     <param name="feature_extract_enable" type="bool" value="0"/>
25 |     <param name="point_filter_num" type="int" value="4"/>
26 |     <param name="max_iteration" type="int" value="3" />
27 |     <param name="filter_size_surf" type="double" value="0.5" />
28 |     <param name="filter_size_map" type="double" value="0.5" />
29 |     <param name="cube_side_length" type="double" value="1000" />
30 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
31 | 
32 |     <group if="$(arg rviz)">
33 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
34 |     </group> -->
35 | 


--------------------------------------------------------------------------------
/launch/mapping_velodyne_liosam.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- Launch file for velodyne16 VLP-16 LiDAR -->
 3 | 
 4 |     <arg name="rviz" default="true" />
 5 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="Invarian_fastlio" output="screen"> 
 6 |       <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne_liosam.yaml" />
 7 | 
 8 |       <param name="feature_extract_enable" type="bool" value="0"/>
 9 |       <param name="point_filter_num" type="int" value="4"/>
10 |       <param name="max_iteration" type="int" value="3" />
11 |       <param name="filter_size_surf" type="double" value="0.5" />
12 |       <param name="filter_size_map" type="double" value="0.5" />
13 |       <param name="cube_side_length" type="double" value="1000" />
14 |     </node>
15 | 
16 |     <group if="$(arg rviz)">
17 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
18 |     </group>
19 | </launch>
20 |     <!-- <arg name="rviz" default="true" />
21 | 
22 |     <rosparam command="load" file="$(find inva_fast_lio)/config/velodyne_utbm.yaml" />
23 | 
24 |     <param name="feature_extract_enable" type="bool" value="0"/>
25 |     <param name="point_filter_num" type="int" value="4"/>
26 |     <param name="max_iteration" type="int" value="3" />
27 |     <param name="filter_size_surf" type="double" value="0.5" />
28 |     <param name="filter_size_map" type="double" value="0.5" />
29 |     <param name="cube_side_length" type="double" value="1000" />
30 |     <node pkg="inva_fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
31 | 
32 |     <group if="$(arg rviz)">
33 |     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find inva_fast_lio)/rviz_cfg/loam_livox.rviz" />
34 |     </group> -->
35 | 


--------------------------------------------------------------------------------
/config/velodyne_utbm.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic: "/velodyne_points" #utbm
 3 |     imu_topic: "/imu/data"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 | 
 8 | preprocess:
 9 |     lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line: 32
11 |     scan_rate: 10                # only need to be set for velodyne, unit: Hz,
12 |     timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
13 |     blind: 2
14 | 
15 | mapping:
16 |     acc_cov: 0.1
17 |     gyr_cov: 0.1
18 |     b_acc_cov: 0.0001
19 |     b_gyr_cov: 0.0001
20 |     fov_degree:    180
21 |     det_range:     100.0
22 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
23 |     extrinsic_T: [ -0.5, 1.4, 1.5] #utbm
24 |     extrinsic_R: [ 1, 0, 0, 
25 |                    0, 1, 0, 
26 |                    0, 0, 1]
27 | 
28 | publish:
29 |     path_en:  true
30 |     scan_publish_en:  true       # false: close all the point cloud output
31 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
32 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
33 | 
34 | pcd_save:
35 |     pcd_save_en: false
36 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
37 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
38 | 
39 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/utbm/inva_lio_20180713_traj.txt"
40 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/utbm/inva_lio_20180716_traj.txt"
41 | save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/utbm/inva_lio_test_traj.txt"
42 | 


--------------------------------------------------------------------------------
/config/velodyne.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic:  "/points_raw" #nclt
 3 |     imu_topic:  "/imu_raw"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 | 
 8 | preprocess:
 9 |     lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line: 32
11 |     scan_rate: 10                # only need to be set for velodyne, unit: Hz,
12 |     timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
13 |     blind: 2
14 | 
15 | mapping:
16 |     acc_cov: 0.1
17 |     gyr_cov: 0.1
18 |     b_acc_cov: 0.0001
19 |     b_gyr_cov: 0.0001
20 |     fov_degree:    180
21 |     det_range:     100.0
22 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
23 |     extrinsic_T: [ 0, 0, 0.28]
24 |     extrinsic_R: [ 1, 0, 0, 
25 |                    0, 1, 0, 
26 |                    0, 0, 1]
27 | 
28 | publish:
29 |     path_en:  true
30 |     scan_publish_en:  true       # false: close all the point cloud output
31 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
32 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
33 | 
34 | pcd_save:
35 |     pcd_save_en: false
36 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
37 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
38 | 
39 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20120429_traj.txt"
40 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20120511_traj.txt"
41 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20120615_traj.txt"
42 | save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20130110_traj.txt"


--------------------------------------------------------------------------------
/config/velodyne_liosam.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic:  "/points_raw" #nclt
 3 |     imu_topic:  "/imu_correct"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 | 
 8 | preprocess:
 9 |     lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line: 16
11 |     scan_rate: 10                # only need to be set for velodyne, unit: Hz,
12 |     timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
13 |     blind: 2
14 | 
15 | mapping:
16 |     acc_cov: 3.9939570888238808e-03
17 |     gyr_cov: 1.5636343949698187e-03
18 |     b_acc_cov: 6.4356659353532566e-05
19 |     b_gyr_cov: 3.5640318696367613e-05
20 |     fov_degree:    180
21 |     det_range:     100.0
22 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
23 |     extrinsic_T: [ 0, 0, 0]
24 |     extrinsic_R: [ 1, 0, 0, 
25 |                    0, 1, 0, 
26 |                    0, 0, 1]
27 | 
28 | publish:
29 |     path_en:  true
30 |     scan_publish_en:  true       # false: close all the point cloud output
31 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
32 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
33 | 
34 | pcd_save:
35 |     pcd_save_en: false
36 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
37 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
38 | 
39 | save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/liosam/inva_lio_traj.txt"
40 | 
41 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20120511_traj.txt"
42 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20120429_traj.txt"
43 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20120615_traj.txt"
44 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/inva_lio_20130110_traj.txt"


--------------------------------------------------------------------------------
/config/velodyne_m2dgr.yaml:
--------------------------------------------------------------------------------
 1 | common:
 2 |     lid_topic: "/velodyne_points" #utbm
 3 |     imu_topic:  "/handsfree/imu"
 4 |     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
 5 |     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
 6 |                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 7 | 
 8 | preprocess:
 9 |     lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
10 |     scan_line: 32
11 |     scan_rate: 10                # only need to be set for velodyne, unit: Hz,
12 |     timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
13 |     blind: 2
14 | 
15 | mapping:
16 |     acc_cov: 3.7686306102624571e-02
17 |     gyr_cov: 2.3417543020438883e-03
18 |     b_acc_cov: 1.1416642385952368e-03
19 |     b_gyr_cov: 1.4428407712885209e-05
20 |     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
21 |     extrinsic_T: [ 0.27255, -0.00053,0.17954]
22 |     extrinsic_R: [ 1, 0, 0, 
23 |                    0, 1, 0, 
24 |                    0, 0, 1]
25 | 
26 | publish:
27 |     path_en:  true
28 |     scan_publish_en:  true       # false: close all the point cloud output
29 |     dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
30 |     scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
31 | 
32 | pcd_save:
33 |     pcd_save_en: false
34 |     interval: -1                 # how many LiDAR frames saved in each pcd file; 
35 |                                  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
36 | 
37 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/m2dgr/street_03_traj.txt"
38 | # save_path: "/home/zj/workspace/zc_paper_ws/liliom_ws/src/lili-om/LiLi-OM-ROT/Log/m2dgr/street_04_traj_1.txt"
39 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/m2dgr/street_05_traj.txt"
40 | # save_path: "/home/zj/workspace/zc_paper_ws/Inva_FAST-LIO_ws/src/Inva-FAST_LIO/Log/m2dgr/street_08_traj.txt"
41 | # save_path: "/home/zj/workspace/zc_paper_ws/FAST_LIO_ws/src/FAST_LIO/Traj/m2dgr/inva_lio_gate_01_traj.txt"
42 | # save_path: "/home/zj/workspace/zc_paper_ws/FAST_LIO_ws/src/FAST_LIO/Traj/m2dgr/inva_lio_gate_03_traj.txt"
43 | save_path: "/home/zj/workspace/zc_paper_ws/FAST_LIO_ws/src/FAST_LIO/Traj/m2dgr/inva_lio_test_traj.txt"
44 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(inva_fast_lio)
 3 | #message(STATUS "******+++++++++++++++++++++++++++++++++++++++++++++++++*********")
 4 | SET(CMAKE_BUILD_TYPE "Debug")
 5 | 
 6 | ADD_COMPILE_OPTIONS(-std=c++14 )
 7 | ADD_COMPILE_OPTIONS(-std=c++14 )
 8 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
 9 | 
10 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
11 | 
12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
13 | set(CMAKE_CXX_STANDARD 14)
14 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
15 | set(CMAKE_CXX_EXTENSIONS OFF)
16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
17 | 
18 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
19 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
20 |   include(ProcessorCount)
21 |   ProcessorCount(N)
22 |   message("Processer number:  ${N}")
23 |   if(N GREATER 4)
24 |     add_definitions(-DMP_EN)
25 |     add_definitions(-DMP_PROC_NUM=3)
26 |     message("core for MP: 3")
27 |   elseif(N GREATER 3)
28 |     add_definitions(-DMP_EN)
29 |     add_definitions(-DMP_PROC_NUM=2)
30 |     message("core for MP: 2")
31 |   else()
32 |     add_definitions(-DMP_PROC_NUM=1)
33 |   endif()
34 | else()
35 |   add_definitions(-DMP_PROC_NUM=1)
36 | endif()
37 | 
38 | find_package(OpenMP QUIET)
39 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
40 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")
41 | 
42 | find_package(catkin REQUIRED COMPONENTS
43 |   geometry_msgs
44 |   nav_msgs
45 |   sensor_msgs
46 |   roscpp
47 |   rospy
48 |   std_msgs
49 |   pcl_ros
50 |   tf
51 |   livox_ros_driver
52 |   message_generation
53 |   eigen_conversions
54 | )
55 | 
56 | find_package(Eigen3 REQUIRED)
57 | find_package(PCL 1.8 REQUIRED)
58 | find_package( Sophus REQUIRED )
59 | message(STATUS "***************")
60 | set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")
61 | message(STATUS "Sophus_INCLUDE_DIRS: ${Sophus_INCLUDE_DIRS}")
62 | message(STATUS "Sophus_LIBRARIES: ${Sophus_LIBRARIES}")
63 | #include_directories(${PROJECT_SOURCE_DIR}/sophus)
64 | 
65 | #message(Eigen: ${EIGEN3_INCLUDE_DIR})
66 | 
67 | include_directories(
68 | 	${catkin_INCLUDE_DIRS} 
69 |   ${EIGEN3_INCLUDE_DIR}
70 |   ${PCL_INCLUDE_DIRS}
71 |   ${PYTHON_INCLUDE_DIRS}
72 |   ${Sophus_INCLUDE_DIRS}
73 |   include)
74 | 
75 | add_message_files(
76 |   FILES
77 |   Pose6D.msg
78 | )
79 | 
80 | generate_messages(
81 |  DEPENDENCIES
82 |  geometry_msgs
83 | )
84 | 
85 | catkin_package(
86 |   CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
87 |   DEPENDS EIGEN3 PCL
88 |   INCLUDE_DIRS
89 | )
90 | 
91 | add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
92 | target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${Sophus_LIBRARIES})
93 | target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
94 | 
95 | add_executable(fastlio_mapping_re src/laserMapping_re.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
96 | target_link_libraries(fastlio_mapping_re ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${Sophus_LIBRARIES})
97 | target_include_directories(fastlio_mapping_re PRIVATE ${PYTHON_INCLUDE_DIRS})
98 | 


--------------------------------------------------------------------------------
/include/common_lib.h:
--------------------------------------------------------------------------------
  1 | #ifndef COMMON_LIB_H1
  2 | #define COMMON_LIB_H1
  3 | 
  4 | #include <Eigen/Eigen>
  5 | #include <pcl/point_types.h>
  6 | #include <pcl/point_cloud.h>
  7 | #include <inva_fast_lio/Pose6D.h>
  8 | #include <sensor_msgs/Imu.h>
  9 | #include <nav_msgs/Odometry.h>
 10 | #include <tf/transform_broadcaster.h>
 11 | #include <eigen_conversions/eigen_msg.h>
 12 | 
 13 | using namespace std;
 14 | using namespace Eigen;
 15 | 
 16 | #define PI_M (3.14159265358)
 17 | #define G_m_s2 (9.81)               // Gravaty const in GuangDong/China
 18 | #define NUM_MATCH_POINTS    (5)     
 19 | 
 20 | #define VEC_FROM_ARRAY(v)        v[0],v[1],v[2]
 21 | #define MAT_FROM_ARRAY(v)        v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
 22 | #define SKEW_SYM_MATRX(v)        0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
 23 | #define DEBUG_FILE_DIR(name)     (string(string(ROOT_DIR) + "Log/"+ name))
 24 | 
 25 | typedef inva_fast_lio::Pose6D Pose6D;
 26 | typedef pcl::PointXYZINormal PointType;
 27 | typedef pcl::PointCloud<PointType> PointCloudXYZI;
 28 | typedef vector<PointType, Eigen::aligned_allocator<PointType>>  PointVector;
 29 | typedef Vector3d V3D;
 30 | typedef Matrix3d M3D;
 31 | typedef Vector3f V3F;
 32 | typedef Matrix3f M3F;
 33 | 
 34 | M3D Eye3d(M3D::Identity());
 35 | M3F Eye3f(M3F::Identity());
 36 | V3D Zero3d(0, 0, 0);
 37 | V3F Zero3f(0, 0, 0);
 38 | 
 39 | struct MeasureGroup     // Lidar data and imu dates for the current process
 40 | {
 41 |     MeasureGroup()
 42 |     {
 43 |         lidar_beg_time = 0.0;
 44 |         this->lidar.reset(new PointCloudXYZI());
 45 |     };
 46 |     double lidar_beg_time;
 47 |     double lidar_end_time;
 48 |     PointCloudXYZI::Ptr lidar;
 49 |     deque<sensor_msgs::Imu::ConstPtr> imu;
 50 | };
 51 | 
 52 | template<typename T>
 53 | auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, \
 54 |                 const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p, const Matrix<T, 3, 3> &R)
 55 | {
 56 |     Pose6D rot_kp;
 57 |     rot_kp.offset_time = t;
 58 |     for (int i = 0; i < 3; i++)
 59 |     {
 60 |         rot_kp.acc[i] = a(i);
 61 |         rot_kp.gyr[i] = g(i);
 62 |         rot_kp.vel[i] = v(i);
 63 |         rot_kp.pos[i] = p(i);
 64 |         for (int j = 0; j < 3; j++)  rot_kp.rot[i*3+j] = R(i,j);
 65 |     }
 66 |     return move(rot_kp);
 67 | }
 68 | 
 69 | 
 70 | float calc_dist(PointType p1, PointType p2){
 71 |     float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
 72 |     return d;
 73 | }
 74 | 
 75 | template<typename T>
 76 | bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
 77 | {
 78 |     Matrix<T, NUM_MATCH_POINTS, 3> A;
 79 |     Matrix<T, NUM_MATCH_POINTS, 1> b;
 80 |     A.setZero();
 81 |     b.setOnes();
 82 |     b *= -1.0f;
 83 | 
 84 |     //求A/Dx + B/Dy + C/Dz + 1 = 0 的参数 
 85 |     for (int j = 0; j < NUM_MATCH_POINTS; j++)
 86 |     {
 87 |         A(j,0) = point[j].x;
 88 |         A(j,1) = point[j].y;
 89 |         A(j,2) = point[j].z;
 90 |     }
 91 | 
 92 |     Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
 93 | 
 94 |     T n = normvec.norm();
 95 |     //pca_result是平面方程的4个参数  /n是为了归一化
 96 |     pca_result(0) = normvec(0) / n;
 97 |     pca_result(1) = normvec(1) / n;
 98 |     pca_result(2) = normvec(2) / n;
 99 |     pca_result(3) = 1.0 / n;
100 | 
101 |     //如果几个点中有距离该平面>threshold的点 认为是不好的平面 返回false
102 |     for (int j = 0; j < NUM_MATCH_POINTS; j++)
103 |     {
104 |         if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
105 |         {
106 |             return false;
107 |         }
108 |     }
109 |     return true;
110 | }
111 | 
112 | #endif


--------------------------------------------------------------------------------
/src/preprocess.h:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <pcl_conversions/pcl_conversions.h>
  3 | #include <sensor_msgs/PointCloud2.h>
  4 | #include <livox_ros_driver/CustomMsg.h>
  5 | 
  6 | using namespace std;
  7 | 
  8 | #define IS_VALID(a) ((abs(a) > 1e8) ? true : false)
  9 | 
 10 | typedef pcl::PointXYZINormal PointType;
 11 | typedef pcl::PointCloud<PointType> PointCloudXYZI;
 12 | 
 13 | enum LID_TYPE
 14 | {
 15 |   AVIA = 1,
 16 |   VELO16,
 17 |   OUST64,
 18 |   RS32
 19 | }; //{1, 2, 3, 4}
 20 | enum TIME_UNIT
 21 | {
 22 |   SEC = 0,
 23 |   MS = 1,
 24 |   US = 2,
 25 |   NS = 3
 26 | };
 27 | enum Feature
 28 | {
 29 |   Nor,
 30 |   Poss_Plane,
 31 |   Real_Plane,
 32 |   Edge_Jump,
 33 |   Edge_Plane,
 34 |   Wire,
 35 |   ZeroPoint
 36 | };
 37 | enum Surround
 38 | {
 39 |   Prev,
 40 |   Next
 41 | };
 42 | enum E_jump
 43 | {
 44 |   Nr_nor,
 45 |   Nr_zero,
 46 |   Nr_180,
 47 |   Nr_inf,
 48 |   Nr_blind
 49 | };
 50 | 
 51 | struct orgtype
 52 | {
 53 |   double range;
 54 |   double dista;
 55 |   double angle[2];
 56 |   double intersect;
 57 |   E_jump edj[2];
 58 |   Feature ftype;
 59 |   orgtype()
 60 |   {
 61 |     range = 0;
 62 |     edj[Prev] = Nr_nor;
 63 |     edj[Next] = Nr_nor;
 64 |     ftype = Nor;
 65 |     intersect = 2;
 66 |   }
 67 | };
 68 | 
 69 | namespace velodyne_ros
 70 | {
 71 |   struct EIGEN_ALIGN16 Point
 72 |   {
 73 |     PCL_ADD_POINT4D;
 74 |     float intensity;
 75 |     float time;
 76 |     uint16_t ring;
 77 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 78 |   };
 79 | } // namespace velodyne_ros
 80 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
 81 |                                   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring))
 82 | 
 83 | namespace rslidar_ros
 84 | {
 85 |   struct EIGEN_ALIGN16 Point
 86 |   {
 87 |     PCL_ADD_POINT4D;
 88 |     uint8_t intensity;
 89 |     uint16_t ring = 0;
 90 |     double timestamp = 0;
 91 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 92 |   };
 93 | } // namespace rslidar_ros
 94 | POINT_CLOUD_REGISTER_POINT_STRUCT(rslidar_ros::Point,
 95 |                                   (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp))
 96 | 
 97 | namespace ouster_ros
 98 | {
 99 |   struct EIGEN_ALIGN16 Point
100 |   {
101 |     PCL_ADD_POINT4D;
102 |     float intensity;
103 |     uint32_t t;
104 |     uint16_t reflectivity;
105 |     uint8_t ring;
106 |     uint16_t ambient;
107 |     uint32_t range;
108 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109 |   };
110 | } // namespace ouster_ros
111 | 
112 | // clang-format off
113 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
114 |     (float, x, x)
115 |     (float, y, y)
116 |     (float, z, z)
117 |     (float, intensity, intensity)
118 |     // use std::uint32_t to avoid conflicting with pcl::uint32_t
119 |     (std::uint32_t, t, t)
120 |     (std::uint16_t, reflectivity, reflectivity)
121 |     (std::uint8_t, ring, ring)
122 |     (std::uint16_t, ambient, ambient)
123 |     (std::uint32_t, range, range)
124 | )
125 | 
126 | class Preprocess
127 | {
128 |   public:
129 | //   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
130 | 
131 |   Preprocess();
132 |   ~Preprocess();
133 |   
134 |   void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
135 |   void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
136 |   void set(bool feat_en, int lid_type, double bld, int pfilt_num);
137 | 
138 |   // sensor_msgs::PointCloud2::ConstPtr pointcloud;
139 |   PointCloudXYZI pl_full, pl_corn, pl_surf;
140 |   PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
141 |   vector<orgtype> typess[128]; //maximum 128 line lidar
142 |   float time_unit_scale;
143 |   int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
144 |   double blind;
145 |   bool feature_enabled, given_offset_time;
146 |   ros::Publisher pub_full, pub_surf, pub_corn;
147 |     
148 | 
149 |   private:
150 |   void rs_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
151 |   void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
152 |   void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
153 |   void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
154 |   void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
155 |   void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
156 |   int  plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
157 |   bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
158 |   bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
159 |   
160 |   int group_size;
161 |   double disA, disB, inf_bound;
162 |   double limit_maxmid, limit_midmin, limit_maxmin;
163 |   double p2l_ratio;
164 |   double jump_up_limit, jump_down_limit;
165 |   double cos160;
166 |   double edgea, edgeb;
167 |   double smallp_intersect, smallp_ratio;
168 |   double vx, vy, vz;
169 | };
170 | 


--------------------------------------------------------------------------------
/include/use-ikfom.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef USE_IKFOM_H1
  2 | #define USE_IKFOM_H1
  3 | 
  4 | #include <vector>
  5 | #include <cstdlib>
  6 | #include <boost/bind.hpp>
  7 | #include <Eigen/Core>
  8 | #include <Eigen/Geometry>
  9 | #include <Eigen/Dense>
 10 | #include <Eigen/Sparse>
 11 | 
 12 | #include "common_lib.h"
 13 | #include "sophus/so3.h"
 14 | #include "utility.h"
 15 | 
 16 | //该hpp主要包含：状态变量x，输入量u的定义，以及正向传播中相关矩阵的函数
 17 | 
 18 | //24维的状态量x
 19 | struct state_ikfom
 20 | {
 21 | 	Sophus::SO3 rot = Sophus::SO3(Eigen::Matrix3d::Identity());
 22 | 	Eigen::Vector3d pos = Eigen::Vector3d(0,0,0);
 23 | 	Eigen::Vector3d vel = Eigen::Vector3d(0,0,0);
 24 | 	Eigen::Vector3d bg = Eigen::Vector3d(0,0,0);
 25 | 	Eigen::Vector3d ba = Eigen::Vector3d(0,0,0);
 26 | 	Eigen::Vector3d grav = Eigen::Vector3d(0,0,-G_m_s2);
 27 | 
 28 | 	Sophus::SO3 offset_R_L_I = Sophus::SO3(Eigen::Matrix3d::Identity());
 29 | 	Eigen::Vector3d offset_T_L_I = Eigen::Vector3d(0,0,0);
 30 | };
 31 | 
 32 | 
 33 | //输入u
 34 | struct input_ikfom
 35 | {
 36 | 	Eigen::Vector3d acc = Eigen::Vector3d(0,0,0);
 37 | 	Eigen::Vector3d gyro = Eigen::Vector3d(0,0,0);
 38 | };
 39 | 
 40 | 
 41 | //噪声协方差Q的初始化(对应公式(8)的Q, 在IMU_Processing.hpp中使用)
 42 | Eigen::Matrix<double, 12, 12> process_noise_cov()
 43 | {
 44 | 	Eigen::Matrix<double, 12, 12> Q = Eigen::MatrixXd::Zero(12, 12);
 45 | 	Q.block<3, 3>(0, 0) = 0.0001 * Eigen::Matrix3d::Identity();
 46 | 	Q.block<3, 3>(3, 3) = 0.0001 * Eigen::Matrix3d::Identity();
 47 | 	Q.block<3, 3>(6, 6) = 0.00001 * Eigen::Matrix3d::Identity();
 48 | 	Q.block<3, 3>(9, 9) = 0.00001 * Eigen::Matrix3d::Identity();
 49 | 
 50 | 	return Q;
 51 | }
 52 | 
 53 | //对应公式(2) 中的f
 54 | Eigen::Matrix<double, 18, 1> get_f(state_ikfom s, input_ikfom in)	
 55 | {
 56 | // 对应顺序为速度(3)，角速度(3),外参T(3),外参旋转R(3)，加速度(3),角速度偏置(3),加速度偏置(3),位置(3)，与论文公式顺序不一致
 57 | 	Eigen::Matrix<double, 18, 1> res = Eigen::Matrix<double, 18, 1>::Zero();
 58 | 	Eigen::Vector3d omega = in.gyro - s.bg;		// 输入的imu的角速度(也就是实际测量值) - 估计的bias值(对应公式的第1行)
 59 | 	Eigen::Vector3d a_inertial = s.rot.matrix() * (in.acc - s.ba);		//  输入的imu的加速度，先转到世界坐标系（对应公式的第3行）
 60 | 
 61 | 	for (int i = 0; i < 3; i++)
 62 | 	{
 63 | 		res(i) = omega[i];	//角速度（对应公式第1行）
 64 | 		res(i + 3) = s.vel[i];		//速度（对应公式第2行）
 65 | 		res(i + 6) = a_inertial[i] + s.grav[i];		//加速度（对应公式第3行）
 66 | 	}
 67 | 
 68 | 	return res;
 69 | }
 70 | 
 71 | //对应公式(7)的Fx  注意该矩阵没乘dt，没加单位阵
 72 | Eigen::Matrix<double, 18, 18> df_dx(state_ikfom s, input_ikfom in, double &dt)
 73 | {
 74 | 	Eigen::Matrix<double, 18, 18> cov = Eigen::Matrix<double, 18, 18>::Identity();
 75 | 	//旋转
 76 | 	cov.template block<3, 3>(0, 0) = Matrix<double, 3, 3>::Identity() + s.rot.matrix() * skewSymmetric(s.bg);
 77 | 	cov.template block<3, 3>(0, 9) = -s.rot.matrix() * dt;		
 78 | 
 79 | 	//位置
 80 | 	cov.block<3, 3>(3, 0) = skewSymmetric(s.pos + s.vel * dt) * s.rot.matrix() * skewSymmetric(s.bg);	 
 81 | 	cov.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity() * dt;
 82 | 	cov.block<3, 3>(3, 9) = -dt * skewSymmetric((s.pos + s.vel * dt)) * s.rot.matrix();	
 83 | 	
 84 | 	//速度
 85 | 	cov.block<3, 3>(6, 0) = dt * s.rot.matrix() * skewSymmetric(s.ba) - skewSymmetric((dt * s.rot.matrix() * in.acc)) + skewSymmetric(s.vel) * s.rot.matrix() * skewSymmetric(s.bg) + skewSymmetric((s.rot.matrix() * (in.acc - s.ba) + s.grav) * dt) * (Eigen::Matrix<double, 3, 3>::Identity() + s.rot.matrix() * skewSymmetric(s.bg));
 86 | 	cov.block<3, 3>(6, 9) = -dt * skewSymmetric(s.vel + (s.rot.matrix() * (in.acc - s.ba) + s.grav) * dt) * s.rot.matrix();
 87 | 	cov.block<3, 3>(6, 12) = -dt * s.rot.matrix();
 88 | 	cov.block<3, 3>(6, 15) = dt * Matrix<double, 3, 3>::Identity();
 89 | 
 90 | 	//bg
 91 | 	cov.block<3, 3>(9, 0) = skewSymmetric(s.bg) * s.rot.matrix() * skewSymmetric(s.bg);
 92 | 	cov.block<3, 3>(9, 9) = Eigen::Matrix<double, 3, 3>::Identity() - dt * skewSymmetric(s.bg) * s.rot.matrix();
 93 | 	
 94 | 	//ba
 95 | 	cov.block<3, 3>(12, 0) = skewSymmetric(s.ba) * s.rot.matrix() * skewSymmetric(s.bg);
 96 | 	cov.block<3, 3>(12, 9) = - dt * skewSymmetric(s.ba) * s.rot.matrix();
 97 | 
 98 | 	//g
 99 | 	cov.block<3, 3>(15, 0) = skewSymmetric(s.grav) * s.rot.matrix() * skewSymmetric(s.bg);
100 | 	cov.block<3, 3>(15, 9) = - dt * skewSymmetric(s.grav) * s.rot.matrix();
101 | 
102 | 	return cov;
103 | 	// Eigen::Matrix<double, 18, 18> cov = Eigen::Matrix<double, 18, 18>::Zero();
104 | 	// cov.template block<3, 3>(0, 9) = -Eigen::Matrix3d::Identity();		//对应公式(7)第1行第4列 (简化为-I)
105 | 	// cov.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity();	//对应公式(7)第2行第3列   I
106 | 
107 | 	// Eigen::Vector3d acc_ = in.acc - s.ba;   	//测量加速度 = a_m - bias	
108 | 	// cov.block<3, 3>(6, 3) = -s.rot.matrix() * Sophus::SO3::hat(acc_);		//对应公式(7)第3行第1列
109 | 	// cov.block<3, 3>(6, 12) = -s.rot.matrix(); 				//对应公式(7)第3行第5列 
110 | 	// cov.template block<3, 3>(6, 15) = Eigen::Matrix3d::Identity();		//对应公式(7)第3行第6列   I
111 | 	
112 | 	// return cov;
113 | }
114 | 
115 | //对应公式(7)的Fw  注意该矩阵没乘dt
116 | Eigen::Matrix<double, 18, 12> df_dw(state_ikfom s, input_ikfom in, double &dt)
117 | {
118 | 	// Eigen::Matrix<double, 18, 12> cov = Eigen::Matrix<double, 18, 12>::Zero();
119 | 	// cov.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();		//对应公式(7)第1行第1列  -A(w dt)简化为-I
120 | 	// cov.block<3, 3>(6, 3) = -s.rot.matrix();					//对应公式(7)第3行第2列  -R 
121 | 	// cov.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity();		//对应公式(7)第4行第3列  I
122 | 	// cov.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity();		//对应公式(7)第5行第4列  I
123 | 	// return cov;
124 | 	Eigen::Matrix<double, 18, 12> cov = Eigen::Matrix<double, 18, 12>::Zero();
125 | 	//旋转
126 | 	cov.block<3, 3>(0, 0) = -s.rot.matrix();
127 | 	//位置
128 | 	cov.block<3, 3>(3, 0) = -skewSymmetric((s.pos + s.vel * dt)) * s.rot.matrix();
129 | 	//速度
130 | 	cov.block<3, 3>(6, 0) = -skewSymmetric((s.vel + (s.rot.matrix() * (in.acc - s.ba) + s.grav) * dt)) * s.rot.matrix();
131 | 	cov.block<3, 3>(6, 3) = -s.rot.matrix();
132 | 	//bg
133 | 	cov.block<3, 3>(9, 0) = -skewSymmetric(s.bg) * s.rot.matrix();
134 | 	cov.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity();	
135 | 	//ba
136 | 	cov.block<3, 3>(12, 0) = -skewSymmetric(s.ba) * s.rot.matrix();
137 | 	cov.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity();
138 | 
139 | 	//g
140 | 	cov.block<3, 3>(15, 0) = -skewSymmetric(s.grav) * s.rot.matrix();
141 | 	return cov;
142 | }
143 | 
144 | #endif


--------------------------------------------------------------------------------
/include/ikd-Tree/ikd_Tree.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #include <stdio.h>
  3 | #include <queue>
  4 | #include <pthread.h>
  5 | #include <chrono>
  6 | #include <time.h>
  7 | #include <unistd.h>
  8 | #include <math.h>
  9 | #include <algorithm>
 10 | #include <memory.h>
 11 | #include <pcl/point_types.h>
 12 | 
 13 | #define EPSS 1e-6
 14 | #define Minimal_Unbalanced_Tree_Size 10
 15 | #define Multi_Thread_Rebuild_Point_Num 1500
 16 | #define DOWNSAMPLE_SWITCH true
 17 | #define ForceRebuildPercentage 0.2
 18 | #define Q_LEN 1000000
 19 | 
 20 | using namespace std;
 21 | 
 22 | // typedef pcl::PointXYZINormal PointType;
 23 | // typedef vector<PointType, Eigen::aligned_allocator<PointType>>  PointVector;
 24 | 
 25 | struct BoxPointType
 26 | {
 27 |     float vertex_min[3];
 28 |     float vertex_max[3];
 29 | };
 30 | 
 31 | enum operation_set
 32 | {
 33 |     ADD_POINT,
 34 |     DELETE_POINT,
 35 |     DELETE_BOX,
 36 |     ADD_BOX,
 37 |     DOWNSAMPLE_DELETE,
 38 |     PUSH_DOWN
 39 | };
 40 | 
 41 | enum delete_point_storage_set
 42 | {
 43 |     NOT_RECORD,
 44 |     DELETE_POINTS_REC,
 45 |     MULTI_THREAD_REC
 46 | };
 47 | 
 48 | template <typename PointType>
 49 | class KD_TREE
 50 | {
 51 |     // using MANUAL_Q_ = MANUAL_Q<typename PointType>;
 52 |     // using PointVector = std::vector<PointType>;
 53 |     
 54 |     // using MANUAL_Q_ = MANUAL_Q<typename PointType>;
 55 | public:
 56 |     using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
 57 |     using Ptr = std::shared_ptr<KD_TREE<PointType>>;
 58 |     
 59 |     struct KD_TREE_NODE
 60 |     {
 61 |         PointType point;
 62 |         int division_axis;
 63 |         int TreeSize = 1;
 64 |         int invalid_point_num = 0;
 65 |         int down_del_num = 0;
 66 |         bool point_deleted = false;
 67 |         bool tree_deleted = false;
 68 |         bool point_downsample_deleted = false;
 69 |         bool tree_downsample_deleted = false;
 70 |         bool need_push_down_to_left = false;
 71 |         bool need_push_down_to_right = false;
 72 |         bool working_flag = false;
 73 |         pthread_mutex_t push_down_mutex_lock;
 74 |         float node_range_x[2], node_range_y[2], node_range_z[2];
 75 |         float radius_sq;
 76 |         KD_TREE_NODE *left_son_ptr = nullptr;
 77 |         KD_TREE_NODE *right_son_ptr = nullptr;
 78 |         KD_TREE_NODE *father_ptr = nullptr;
 79 |         // For paper data record
 80 |         float alpha_del;
 81 |         float alpha_bal;
 82 |     };
 83 | 
 84 |     struct Operation_Logger_Type
 85 |     {
 86 |         PointType point;
 87 |         BoxPointType boxpoint;
 88 |         bool tree_deleted, tree_downsample_deleted;
 89 |         operation_set op;
 90 |     };
 91 |     // static const PointType zeroP;
 92 | 
 93 |     struct PointType_CMP
 94 |     {
 95 |         PointType point;
 96 |         float dist = 0.0;
 97 |         PointType_CMP(PointType p = PointType(), float d = INFINITY)
 98 |         {
 99 |             this->point = p;
100 |             this->dist = d;
101 |         };
102 |         bool operator<(const PointType_CMP &a) const
103 |         {
104 |             if (fabs(dist - a.dist) < 1e-10)
105 |                 return point.x < a.point.x;
106 |             else
107 |                 return dist < a.dist;
108 |         }
109 |     };
110 | 
111 |     class MANUAL_HEAP
112 |     {
113 | 
114 |     public:
115 |         MANUAL_HEAP(int max_capacity = 100)
116 | 
117 |         {
118 |             cap = max_capacity;
119 |             heap = new PointType_CMP[max_capacity];
120 |             heap_size = 0;
121 |         }
122 | 
123 |         ~MANUAL_HEAP()
124 |         {
125 |             delete[] heap;
126 |         }
127 |         void pop()
128 |         {
129 |             if (heap_size == 0)
130 |                 return;
131 |             heap[0] = heap[heap_size - 1];
132 |             heap_size--;
133 |             MoveDown(0);
134 |             return;
135 |         }
136 |         PointType_CMP top()
137 |         {
138 |             return heap[0];
139 |         }
140 |         void push(PointType_CMP point)
141 |         {
142 |             if (heap_size >= cap)
143 |                 return;
144 |             heap[heap_size] = point;
145 |             FloatUp(heap_size);
146 |             heap_size++;
147 |             return;
148 |         }
149 |         int size()
150 |         {
151 |             return heap_size;
152 |         }
153 |         void clear()
154 |         {
155 |             heap_size = 0;
156 |             return;
157 |         }
158 | 
159 |     private:
160 |         PointType_CMP *heap;
161 |         void MoveDown(int heap_index)
162 |         {
163 |             int l = heap_index * 2 + 1;
164 |             PointType_CMP tmp = heap[heap_index];
165 |             while (l < heap_size)
166 |             {
167 |                 if (l + 1 < heap_size && heap[l] < heap[l + 1])
168 |                     l++;
169 |                 if (tmp < heap[l])
170 |                 {
171 |                     heap[heap_index] = heap[l];
172 |                     heap_index = l;
173 |                     l = heap_index * 2 + 1;
174 |                 }
175 |                 else
176 |                     break;
177 |             }
178 |             heap[heap_index] = tmp;
179 |             return;
180 |         }
181 |         void FloatUp(int heap_index)
182 |         {
183 |             int ancestor = (heap_index - 1) / 2;
184 |             PointType_CMP tmp = heap[heap_index];
185 |             while (heap_index > 0)
186 |             {
187 |                 if (heap[ancestor] < tmp)
188 |                 {
189 |                     heap[heap_index] = heap[ancestor];
190 |                     heap_index = ancestor;
191 |                     ancestor = (heap_index - 1) / 2;
192 |                 }
193 |                 else
194 |                     break;
195 |             }
196 |             heap[heap_index] = tmp;
197 |             return;
198 |         }
199 |         int heap_size = 0;
200 |         int cap = 0;
201 |     };
202 | 
203 |     class MANUAL_Q
204 |     {
205 |     private:
206 |         int head = 0, tail = 0, counter = 0;
207 |         Operation_Logger_Type q[Q_LEN];
208 |         bool is_empty;
209 | 
210 |     public:
211 |         void pop()
212 |         {
213 |             if (counter == 0)
214 |                 return;
215 |             head++;
216 |             head %= Q_LEN;
217 |             counter--;
218 |             if (counter == 0)
219 |                 is_empty = true;
220 |             return;
221 |         }
222 |         Operation_Logger_Type front()
223 |         {
224 |             return q[head];
225 |         }
226 |         Operation_Logger_Type back()
227 |         {
228 |             return q[tail];
229 |         }
230 |         void clear()
231 |         {
232 |             head = 0;
233 |             tail = 0;
234 |             counter = 0;
235 |             is_empty = true;
236 |             return;
237 |         }
238 |         void push(Operation_Logger_Type op)
239 |         {
240 |             q[tail] = op;
241 |             counter++;
242 |             if (is_empty)
243 |                 is_empty = false;
244 |             tail++;
245 |             tail %= Q_LEN;
246 |         }
247 |         bool empty()
248 |         {
249 |             return is_empty;
250 |         }
251 |         int size()
252 |         {
253 |             return counter;
254 |         }
255 |     };
256 | 
257 | private:
258 |     // Multi-thread Tree Rebuild
259 |     bool termination_flag = false;
260 |     bool rebuild_flag = false;
261 |     pthread_t rebuild_thread;
262 |     pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex;
263 |     pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
264 |     // queue<Operation_Logger_Type> Rebuild_Logger;
265 |     MANUAL_Q Rebuild_Logger;
266 |     PointVector Rebuild_PCL_Storage;
267 |     KD_TREE_NODE **Rebuild_Ptr = nullptr;
268 |     int search_mutex_counter = 0;
269 |     static void *multi_thread_ptr(void *arg);
270 |     void multi_thread_rebuild();
271 |     void start_thread();
272 |     void stop_thread();
273 |     void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation);
274 |     // KD Tree Functions and augmented variables
275 |     int Treesize_tmp = 0, Validnum_tmp = 0;
276 |     float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;
277 |     float delete_criterion_param = 0.5f;
278 |     float balance_criterion_param = 0.7f;
279 |     float downsample_size = 0.2f;
280 |     bool Delete_Storage_Disabled = false;
281 |     KD_TREE_NODE *STATIC_ROOT_NODE = nullptr;
282 |     PointVector Points_deleted;
283 |     PointVector Downsample_Storage;
284 |     PointVector Multithread_Points_deleted;
285 |     void InitTreeNode(KD_TREE_NODE *root);
286 |     void Test_Lock_States(KD_TREE_NODE *root);
287 |     void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage);
288 |     void Rebuild(KD_TREE_NODE **root);
289 |     int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);
290 |     void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild);
291 |     void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis);
292 |     void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild);
293 |     void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue<PointType_CMP>
294 |     void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);
295 |     void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage);
296 |     bool Criterion_Check(KD_TREE_NODE *root);
297 |     void Push_Down(KD_TREE_NODE *root);
298 |     void Update(KD_TREE_NODE *root);
299 |     void delete_tree_nodes(KD_TREE_NODE **root);
300 |     void downsample(KD_TREE_NODE **root);
301 |     bool same_point(PointType a, PointType b);
302 |     float calc_dist(PointType a, PointType b);
303 |     float calc_box_dist(KD_TREE_NODE *node, PointType point);
304 |     static bool point_cmp_x(PointType a, PointType b);
305 |     static bool point_cmp_y(PointType a, PointType b);
306 |     static bool point_cmp_z(PointType a, PointType b);
307 | 
308 | public:
309 |     KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2);
310 |     ~KD_TREE();
311 |     void Set_delete_criterion_param(float delete_param)
312 |     {
313 |         delete_criterion_param = delete_param;
314 |     }
315 |     void Set_balance_criterion_param(float balance_param)
316 |     {
317 |         balance_criterion_param = balance_param;
318 |     }
319 |     void set_downsample_param(float downsample_param)
320 |     {
321 |         downsample_size = downsample_param;
322 |     }
323 |     void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);
324 |     int size();
325 |     int validnum();
326 |     void root_alpha(float &alpha_bal, float &alpha_del);
327 |     void Build(PointVector point_cloud);
328 |     void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist = INFINITY);
329 |     void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage);
330 |     void Radius_Search(PointType point, const float radius, PointVector &Storage);
331 |     int Add_Points(PointVector &PointToAdd, bool downsample_on);
332 |     void Add_Point_Boxes(vector<BoxPointType> &BoxPoints);
333 |     void Delete_Points(PointVector &PointToDel);
334 |     int Delete_Point_Boxes(vector<BoxPointType> &BoxPoints);
335 |     void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type);
336 |     void acquire_removed_points(PointVector &removed_points);
337 |     BoxPointType tree_range();
338 |     PointVector PCL_Storage;
339 |     KD_TREE_NODE *Root_Node = nullptr;
340 |     int max_queue_size = 0;
341 | };
342 | 
343 | // template <typename PointType>
344 | // PointType KD_TREE<PointType>::zeroP = PointType(0,0,0);
345 | 


--------------------------------------------------------------------------------
/rviz_cfg/relocalization.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 0
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Axes1
  9 |         - /mapping1/surround1
 10 |         - /mapping1/currPoints1/Autocompute Value Bounds1
 11 |         - /Odometry1
 12 |         - /Odometry1/Odometry1
 13 |         - /Odometry1/Odometry1/Shape1
 14 |         - /Odometry1/Odometry1/Covariance1
 15 |         - /Odometry1/Odometry1/Covariance1/Position1
 16 |         - /Odometry1/Odometry1/Covariance1/Orientation1
 17 |         - /PointCloud21
 18 |         - /MarkerArray1/Namespaces1
 19 |       Splitter Ratio: 0.6432291865348816
 20 |     Tree Height: 1105
 21 |   - Class: rviz/Selection
 22 |     Name: Selection
 23 |   - Class: rviz/Tool Properties
 24 |     Expanded:
 25 |       - /2D Pose Estimate1
 26 |       - /2D Nav Goal1
 27 |       - /Publish Point1
 28 |     Name: Tool Properties
 29 |     Splitter Ratio: 0.5886790156364441
 30 |   - Class: rviz/Views
 31 |     Expanded:
 32 |       - /Current View1
 33 |     Name: Views
 34 |     Splitter Ratio: 0.5
 35 |   - Class: rviz/Time
 36 |     Experimental: false
 37 |     Name: Time
 38 |     SyncMode: 0
 39 |     SyncSource: surround
 40 | Preferences:
 41 |   PromptSaveOnExit: true
 42 | Toolbars:
 43 |   toolButtonStyle: 2
 44 | Visualization Manager:
 45 |   Class: ""
 46 |   Displays:
 47 |     - Alpha: 1
 48 |       Cell Size: 1000
 49 |       Class: rviz/Grid
 50 |       Color: 160; 160; 164
 51 |       Enabled: false
 52 |       Line Style:
 53 |         Line Width: 0.029999999329447746
 54 |         Value: Lines
 55 |       Name: Grid
 56 |       Normal Cell Count: 0
 57 |       Offset:
 58 |         X: 0
 59 |         Y: 0
 60 |         Z: 0
 61 |       Plane: XY
 62 |       Plane Cell Count: 40
 63 |       Reference Frame: <Fixed Frame>
 64 |       Value: false
 65 |     - Alpha: 1
 66 |       Class: rviz/Axes
 67 |       Enabled: true
 68 |       Length: 2
 69 |       Name: Axes
 70 |       Radius: 0.05000000074505806
 71 |       Reference Frame: <Fixed Frame>
 72 |       Show Trail: false
 73 |       Value: true
 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: true
 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): 3
 97 |           Size (m): 0.05000000074505806
 98 |           Style: Points
 99 |           Topic: /cloud_registered
100 |           Unreliable: false
101 |           Use Fixed Frame: true
102 |           Use rainbow: true
103 |           Value: true
104 |         - Alpha: 0.10000000149011612
105 |           Autocompute Intensity Bounds: true
106 |           Autocompute Value Bounds:
107 |             Max Value: 22.501062393188477
108 |             Min Value: -1.1335842609405518
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: 1000
116 |           Enabled: false
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_registered
128 |           Unreliable: false
129 |           Use Fixed Frame: true
130 |           Use rainbow: true
131 |           Value: false
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: 3
189 |             Axes Radius: 0.30000001192092896
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
197 |           Unreliable: false
198 |           Value: true
199 |       Enabled: true
200 |       Name: Odometry
201 |     - Alpha: 0
202 |       Buffer Length: 2
203 |       Class: rviz/Path
204 |       Color: 25; 255; 255
205 |       Enabled: true
206 |       Head Diameter: 0
207 |       Head Length: 0
208 |       Length: 0.30000001192092896
209 |       Line Style: Billboards
210 |       Line Width: 0.20000000298023224
211 |       Name: Path
212 |       Offset:
213 |         X: 0
214 |         Y: 0
215 |         Z: 0
216 |       Pose Color: 25; 255; 255
217 |       Pose Style: None
218 |       Queue Size: 10
219 |       Radius: 0.029999999329447746
220 |       Shaft Diameter: 0.4000000059604645
221 |       Shaft Length: 0.4000000059604645
222 |       Topic: /path
223 |       Unreliable: false
224 |       Value: true
225 |     - Alpha: 0.10000000149011612
226 |       Autocompute Intensity Bounds: false
227 |       Autocompute Value Bounds:
228 |         Max Value: 27.465036392211914
229 |         Min Value: -10.730713844299316
230 |         Value: true
231 |       Axis: Z
232 |       Channel Name: intensity
233 |       Class: rviz/PointCloud2
234 |       Color: 138; 226; 52
235 |       Color Transformer: AxisColor
236 |       Decay Time: 0
237 |       Enabled: true
238 |       Invert Rainbow: false
239 |       Max Color: 92; 53; 102
240 |       Max Intensity: 255
241 |       Min Color: 173; 127; 168
242 |       Min Intensity: 0
243 |       Name: PointCloud2
244 |       Position Transformer: XYZ
245 |       Queue Size: 10
246 |       Selectable: true
247 |       Size (Pixels): 1
248 |       Size (m): 0.10000000149011612
249 |       Style: Points
250 |       Topic: /Laser_map
251 |       Unreliable: false
252 |       Use Fixed Frame: true
253 |       Use rainbow: false
254 |       Value: true
255 |     - Class: rviz/MarkerArray
256 |       Enabled: false
257 |       Marker Topic: /MarkerArray
258 |       Name: MarkerArray
259 |       Namespaces:
260 |         {}
261 |       Queue Size: 100
262 |       Value: false
263 |   Enabled: true
264 |   Global Options:
265 |     Background Color: 0; 0; 0
266 |     Default Light: true
267 |     Fixed Frame: camera_init
268 |     Frame Rate: 10
269 |   Name: root
270 |   Tools:
271 |     - Class: rviz/Interact
272 |       Hide Inactive Objects: true
273 |     - Class: rviz/MoveCamera
274 |     - Class: rviz/Select
275 |     - Class: rviz/FocusCamera
276 |     - Class: rviz/Measure
277 |     - Class: rviz/SetInitialPose
278 |       Theta std deviation: 0.2617993950843811
279 |       Topic: /initialpose
280 |       X std deviation: 0.5
281 |       Y std deviation: 0.5
282 |     - Class: rviz/SetGoal
283 |       Topic: /move_base_simple/goal
284 |     - Class: rviz/PublishPoint
285 |       Single click: true
286 |       Topic: /clicked_point
287 |   Value: true
288 |   Views:
289 |     Current:
290 |       Class: rviz/Orbit
291 |       Distance: 93.19964599609375
292 |       Enable Stereo Rendering:
293 |         Stereo Eye Separation: 0.05999999865889549
294 |         Stereo Focal Distance: 1
295 |         Swap Stereo Eyes: false
296 |         Value: false
297 |       Field of View: 0.7853981852531433
298 |       Focal Point:
299 |         X: 48.48363494873047
300 |         Y: 11.751655578613281
301 |         Z: -8.824173927307129
302 |       Focal Shape Fixed Size: true
303 |       Focal Shape Size: 0.05000000074505806
304 |       Invert Z Axis: false
305 |       Name: Current View
306 |       Near Clip Distance: 0.009999999776482582
307 |       Pitch: 0.4497966766357422
308 |       Target Frame: global
309 |       Yaw: 3.782217264175415
310 |     Saved: ~
311 | Window Geometry:
312 |   Displays:
313 |     collapsed: false
314 |   Height: 1369
315 |   Hide Left Dock: false
316 |   Hide Right Dock: true
317 |   QMainWindow State: 000000ff00000000fd0000000400000000000002c300000498fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004700000498000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009a800000052fc0100000002fb0000000800540069006d00650100000000000009a80000037100fffffffb0000000800540069006d00650100000000000004500000000000000000000006de0000049800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
318 |   Selection:
319 |     collapsed: false
320 |   Time:
321 |     collapsed: false
322 |   Tool Properties:
323 |     collapsed: false
324 |   Views:
325 |     collapsed: true
326 |   Width: 2472
327 |   X: 1920
328 |   Y: 34
329 | 


--------------------------------------------------------------------------------
/include/esekfom.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef ESEKFOM_EKF_HPP1
  2 | #define ESEKFOM_EKF_HPP1
  3 | 
  4 | #include <vector>
  5 | #include <cstdlib>
  6 | #include <boost/bind.hpp>
  7 | #include <Eigen/Core>
  8 | #include <Eigen/Geometry>
  9 | #include <Eigen/Dense>
 10 | #include <Eigen/Sparse>
 11 | #include "utility.h"
 12 | 
 13 | #include "use-ikfom.hpp"
 14 | #include <ikd-Tree/ikd_Tree.h>
 15 | 
 16 | //该hpp主要包含：广义加减法，前向传播主函数，计算特征点残差及其雅可比，ESKF主函数
 17 | 
 18 | const double epsi = 0.001; // ESKF迭代时，如果dx<epsi 认为收敛
 19 | 
 20 | namespace esekfom
 21 | {
 22 | 	using namespace Eigen;
 23 | 
 24 | 	PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));		  //特征点在地图中对应的平面参数(平面的单位法向量,以及当前点到平面距离)
 25 | 	PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); //有效特征点
 26 | 	PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); //有效特征点对应点法相量
 27 | 	bool point_selected_surf[100000] = {1};							  //判断是否是有效特征点
 28 | 
 29 | 	struct dyn_share_datastruct
 30 | 	{
 31 | 		bool valid;												   //有效特征点数量是否满足要求
 32 | 		bool converge;											   //迭代时，是否已经收敛
 33 | 		Eigen::Matrix<double, Eigen::Dynamic, 1> h;				   //残差	(公式(14)中的z)
 34 | 		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> h_x; //雅可比矩阵H (公式(14)中的H)
 35 | 	};
 36 | 
 37 | 	class esekf
 38 | 	{
 39 | 	public:
 40 | 		typedef Matrix<double, 18, 18> cov;				// 24X24的协方差矩阵
 41 | 		typedef Matrix<double, 18, 1> vectorized_state; // 24X1的向量
 42 | 
 43 | 		esekf(){};
 44 | 		~esekf(){};
 45 | 
 46 | 		state_ikfom get_x()
 47 | 		{
 48 | 			return x_;
 49 | 		}
 50 | 
 51 | 		cov get_P()
 52 | 		{
 53 | 			return P_;
 54 | 		}
 55 | 
 56 | 		void change_x(state_ikfom &input_state)
 57 | 		{
 58 | 			x_ = input_state;
 59 | 		}
 60 | 
 61 | 		void change_P(cov &input_cov)
 62 | 		{
 63 | 			P_ = input_cov;
 64 | 		}
 65 | 
 66 | 		//广义加法  公式(4)
 67 | 		state_ikfom boxplus(state_ikfom x, Eigen::Matrix<double, 18, 1> f_)
 68 | 		{
 69 | 			state_ikfom x_r;
 70 | 			// cout << "f_.block<3, 1>(0, 0): " << f_.block<3, 1>(0, 0) << endl;
 71 | 			x_r.rot = x.rot * Sophus::SO3::exp(f_.block<3, 1>(0, 0));
 72 | 			// cout << "---------------------------------------" << endl;
 73 | 			x_r.pos = x.pos + f_.block<3, 1>(3, 0);
 74 | 			x_r.vel = x.vel + f_.block<3, 1>(6, 0);
 75 | 			x_r.bg = x.bg + f_.block<3, 1>(9, 0);
 76 | 			x_r.ba = x.ba + f_.block<3, 1>(12, 0);
 77 | 			x_r.grav = x.grav + f_.block<3, 1>(15, 0);
 78 | 
 79 | 			return x_r;
 80 | 		}
 81 | 
 82 | 		//前向传播  公式(4-8)
 83 | 		void predict(double &dt, Eigen::Matrix<double, 12, 12> &Q, const input_ikfom &i_in)
 84 | 		{
 85 | 			Eigen::Matrix<double, 18, 1> f_ = get_f(x_, i_in);	  //公式(3)的f
 86 | 			Eigen::Matrix<double, 18, 18> f_x_ = df_dx(x_, i_in, dt); //公式(7)的df/dx
 87 | 			Eigen::Matrix<double, 18, 12> f_w_ = df_dw(x_, i_in, dt); //公式(7)的df/dw
 88 | 			// cout << "f_:\n" << f_.transpose() << endl;
 89 | 			x_ = boxplus(x_, f_ * dt); //前向传播 公式(4)
 90 | 
 91 | 			// f_x_ = Matrix<double, 18, 18>::Identity() + f_x_ * dt; //之前Fx矩阵里的项没加单位阵，没乘dt   这里补上
 92 | 
 93 | 			// P_ = (f_x_)*P_ * (f_x_).transpose() + (dt * f_w_) * Q * (dt * f_w_).transpose(); //传播协方差矩阵，即公式(8)
 94 | 			P_ = (f_x_)*P_ * (f_x_).transpose() + dt * dt * (f_w_) * Q * (f_w_).transpose(); //传播协方差矩阵，即公式(8)
 95 | 		}
 96 | 
 97 | 		//计算每个特征点的残差及H矩阵
 98 | 		void h_share_model(dyn_share_datastruct &ekfom_data, PointCloudXYZI::Ptr &feats_down_body,
 99 | 						   KD_TREE<PointType> &ikdtree, vector<PointVector> &Nearest_Points, bool extrinsic_est)
100 | 		{
101 | 			int feats_down_size = feats_down_body->points.size();
102 | 			laserCloudOri->clear();
103 | 			corr_normvect->clear();
104 | 
105 | #ifdef MP_EN
106 | 			omp_set_num_threads(MP_PROC_NUM);
107 | #pragma omp parallel for
108 | #endif
109 | 
110 | 			for (int i = 0; i < feats_down_size; i++) //遍历所有的特征点
111 | 			{
112 | 				PointType &point_body = feats_down_body->points[i];
113 | 				PointType point_world;
114 | 				
115 | 				V3D p_body(point_body.x, point_body.y, point_body.z);
116 | 				//把Lidar坐标系的点先转到IMU坐标系，再根据前向传播估计的位姿x，转到世界坐标系
117 | 				V3D p_global(x_.rot * (x_.offset_R_L_I * p_body + x_.offset_T_L_I) + x_.pos);
118 | 				point_world.x = p_global(0);
119 | 				point_world.y = p_global(1);
120 | 				point_world.z = p_global(2);
121 | 				point_world.intensity = point_body.intensity;
122 | 
123 | 				vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
124 | 				auto &points_near = Nearest_Points[i]; // Nearest_Points[i]打印出来发现是按照离point_world距离，从小到大的顺序的vector
125 | 
126 | 				double ta = omp_get_wtime();
127 | 				if (ekfom_data.converge)
128 | 				{
129 | 					//寻找point_world的最近邻的平面点
130 | 					ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);
131 | 					//判断是否是有效匹配点，与loam系列类似，要求特征点最近邻的地图点数量>阈值，距离<阈值  满足条件的才置为true
132 | 					point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false
133 | 																																		: true;
134 | 				}
135 | 				if (!point_selected_surf[i])
136 | 					continue; //如果该点不满足条件  不进行下面步骤
137 | 
138 | 				Matrix<float, 4, 1> pabcd;		//平面点信息
139 | 				point_selected_surf[i] = false; //将该点设置为无效点，用来判断是否满足条件
140 | 				//拟合平面方程ax+by+cz+d=0并求解点到平面距离
141 | 				if (esti_plane(pabcd, points_near, 0.1f))
142 | 				{
143 | 					float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); //当前点到平面的距离
144 | 					float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());												   //如果残差大于经验阈值，则认为该点是有效点  简言之，距离原点越近的lidar点  要求点到平面的距离越苛刻
145 | 
146 | 					if (s > 0.9) //如果残差大于阈值，则认为该点是有效点
147 | 					{
148 | 						point_selected_surf[i] = true;
149 | 						normvec->points[i].x = pabcd(0); //存储平面的单位法向量  以及当前点到平面距离
150 | 						normvec->points[i].y = pabcd(1);
151 | 						normvec->points[i].z = pabcd(2);
152 | 						normvec->points[i].intensity = pd2;
153 | 					}
154 | 				}
155 | 			}
156 | 
157 | 			int effct_feat_num = 0; //有效特征点的数量
158 | 			for (int i = 0; i < feats_down_size; i++)
159 | 			{
160 | 				if (point_selected_surf[i]) //对于满足要求的点
161 | 				{
162 | 					laserCloudOri->points[effct_feat_num] = feats_down_body->points[i]; //把这些点重新存到laserCloudOri中
163 | 					corr_normvect->points[effct_feat_num] = normvec->points[i];			//存储这些点对应的法向量和到平面的距离
164 | 					effct_feat_num++;
165 | 				}
166 | 			}
167 | 
168 | 			if (effct_feat_num < 1)
169 | 			{
170 | 				ekfom_data.valid = false;
171 | 				ROS_WARN("No Effective Points! \n");
172 | 				return;
173 | 			}
174 | 
175 | 			// 雅可比矩阵H和残差向量的计算
176 | 			ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 18);
177 | 			ekfom_data.h.resize(effct_feat_num);
178 | 
179 | 			for (int i = 0; i < effct_feat_num; i++)
180 | 			{
181 | 				V3D point_(laserCloudOri->points[i].x, laserCloudOri->points[i].y, laserCloudOri->points[i].z);
182 | 				M3D point_crossmat;
183 | 				point_crossmat << SKEW_SYM_MATRX(point_);
184 | 				V3D point_I_ = x_.offset_R_L_I * point_ + x_.offset_T_L_I;
185 | 				// M3D point_I_crossmat;
186 | 				// point_I_crossmat << SKEW_SYM_MATRX(point_I_);
187 | 
188 | 				// 得到对应的平面的法向量
189 | 				const PointType &norm_p = corr_normvect->points[i];
190 | 				V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);
191 | 
192 | 				// 计算雅可比矩阵H
193 | 				// V3D C(x_.rot.matrix().transpose() * norm_vec);
194 | 				// V3D A(point_I_crossmat * C);
195 | 				V3D A = (-1) * norm_vec.transpose() * skewSymmetric(x_.rot.matrix() * point_I_ + x_.pos); // 单位化
196 | 				// if (extrinsic_est)
197 | 				// {
198 | 				// 	V3D B(point_crossmat * x_.offset_R_L_I.matrix().transpose() * C);
199 | 				// 	ekfom_data.h_x.block<1, 12>(i, 0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
200 | 				// }
201 | 				// else
202 | 				// {
203 | 					ekfom_data.h_x.block<1, 18>(i, 0) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, 0, 0, 0,
204 | 														0, 0, 0, 0, 0, 0, 0, 0, 0;
205 | 				// }
206 | 
207 | 				//残差：点面距离
208 | 				ekfom_data.h(i) = -norm_p.intensity;
209 | 			}
210 | 		}
211 | 
212 | 		//广义减法
213 | 		vectorized_state boxminus(state_ikfom x1, state_ikfom x2)
214 | 		{
215 | 			vectorized_state x_r = vectorized_state::Zero();
216 | 
217 | 			x_r.block<3, 1>(0, 0) = Sophus::SO3(x2.rot.matrix().transpose() * x1.rot.matrix()).log();
218 | 			x_r.block<3, 1>(3, 0) = x1.pos - x2.pos;
219 | 			// x_r.block<3, 1>(6, 0) = Sophus::SO3(x2.offset_R_L_I.matrix().transpose() * x1.offset_R_L_I.matrix()).log();
220 | 			// x_r.block<3, 1>(9, 0) = x1.offset_T_L_I - x2.offset_T_L_I;
221 | 			x_r.block<3, 1>(6, 0) = x1.vel - x2.vel;
222 | 			x_r.block<3, 1>(9, 0) = x1.bg - x2.bg;
223 | 			x_r.block<3, 1>(12, 0) = x1.ba - x2.ba;
224 | 			x_r.block<3, 1>(15, 0) = x1.grav - x2.grav;
225 | 
226 | 			return x_r;
227 | 		}
228 | 
229 | 		// ESKF
230 | 		void update_iterated_dyn_share_modified(double R, PointCloudXYZI::Ptr &feats_down_body,
231 | 												KD_TREE<PointType> &ikdtree, vector<PointVector> &Nearest_Points, int maximum_iter, bool extrinsic_est, int &iter_count)
232 | 		{
233 | 			normvec->resize(int(feats_down_body->points.size()));
234 | 
235 | 			dyn_share_datastruct dyn_share;
236 | 			dyn_share.valid = true;
237 | 			dyn_share.converge = true;
238 | 			int t = 0;
239 | 			state_ikfom x_propagated = x_; //这里的x_和P_分别是经过正向传播后的状态量和协方差矩阵，因为会先调用predict函数再调用这个函数
240 | 			cov P_propagated = P_;
241 | 
242 | 			// vectorized_state dx_new = vectorized_state::Zero(); // 24X1的向量
243 | 
244 | 			for (int i = -1; i < 0; i++) // maximum_iter是卡尔曼滤波的最大迭代次数
245 | 			{
246 | 				dyn_share.valid = true;
247 | 				double t00 = omp_get_wtime();
248 | 				// 计算雅克比，也就是点面残差的导数 H(代码里是h_x)
249 | 				h_share_model(dyn_share, feats_down_body, ikdtree, Nearest_Points, extrinsic_est);
250 | 				double t11 = omp_get_wtime();
251 | 				std::cout << "residual compute time (ms):  " << (t11 - t00) * 1000 << std::endl
252 | 						<< std::endl;
253 | 
254 | 				if (!dyn_share.valid)
255 | 				{
256 | 					iter_count = i;
257 | 					continue;
258 | 				}
259 | 
260 | 				// double t00 = omp_get_wtime();
261 | 				Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> K;
262 | 				auto H = dyn_share.h_x; //m*18
263 | 				int rows = H.rows();
264 | 				Eigen::Matrix<double, 18, 18> HTH = H.transpose() * H;
265 | 				K = (HTH / R + P_propagated.inverse()).inverse() * H.transpose() / R; //18*m
266 | 				Eigen::Matrix<double, 18, 1> Kh = K * dyn_share.h; //18*1
267 | 				double Kh_norm = Kh.norm();
268 | 				Eigen::Matrix<double, 8, 8> S = Eigen::Matrix<double, 8, 8>::Zero();
269 | 				S.block<3, 3>(0, 0) = skewSymmetric(Kh.block<3, 1>(0, 0));
270 | 				S.block<3, 1>(0, 3) = Kh.block<3, 1>(3, 0);
271 | 				S.block<3, 1>(0, 4) = Kh.block<3, 1>(6, 0);
272 | 				S.block<3, 1>(0, 5) = Kh.block<3, 1>(9, 0);
273 | 				S.block<3, 1>(0, 6) = Kh.block<3, 1>(12, 0);
274 | 				S.block<3, 1>(0, 7) = Kh.block<3, 1>(15, 0);
275 | 				Eigen::Matrix<double, 8, 8> EXPS = Eigen::Matrix<double, 8, 8>::Identity() +
276 | 													S + ((1 - cos(Kh_norm)) / (Kh_norm * Kh_norm)) * S * S +
277 | 													((Kh_norm - sin(Kh_norm)) / (Kh_norm * Kh_norm * Kh_norm)) * S * S * S;
278 | 				// cout << "here2" << endl;
279 | 				Eigen::Matrix<double, 8, 8> X_matrix = Eigen::Matrix<double, 8, 8>::Identity();
280 | 				X_matrix.block<3, 3>(0, 0) = x_propagated.rot.matrix();
281 | 				X_matrix.block<3, 1>(0, 3) = x_propagated.pos;
282 | 				X_matrix.block<3, 1>(0, 4) = x_propagated.vel;
283 | 				X_matrix.block<3, 1>(0, 5) = x_propagated.bg;
284 | 				X_matrix.block<3, 1>(0, 6) = x_propagated.ba;
285 | 				X_matrix.block<3, 1>(0, 7) = x_propagated.grav;
286 | 
287 | 				
288 | 				Eigen::Matrix<double, 8, 8> newx_ = EXPS * X_matrix;
289 | 				// cout << "newx_" << endl;
290 | 				// cout << newx_ << endl;
291 | 				// cout << "here3" << endl;
292 | 				x_.rot = Sophus::SO3(newx_.block<3, 3>(0, 0));
293 | 				x_.pos = newx_.block<3, 1>(0, 3);
294 | 				x_.vel = newx_.block<3, 1>(0, 4);
295 | 				x_.bg = newx_.block<3, 1>(0, 5);
296 | 				x_.ba = newx_.block<3, 1>(0, 6);
297 | 				x_.grav = newx_.block<3, 1>(0, 7);
298 | 
299 | 				P_ = P_propagated - K * H * P_propagated; 
300 | 				// double t11 = omp_get_wtime();
301 | 				// std::cout << "Update time (ms):  " << (t11 - t00) * 1000 << std::endl
302 | 				// 		<< std::endl;
303 | 				iter_count = i;
304 | 			}
305 | 		}
306 | 
307 | 	private:
308 | 		state_ikfom x_;
309 | 		cov P_ = cov::Identity();
310 | 	};
311 | 
312 | } // namespace esekfom
313 | 
314 | #endif //  ESEKFOM_EKF_HPP1
315 | 


--------------------------------------------------------------------------------
/rviz_cfg/loam_livox.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/currPoints1/Autocompute Value Bounds1
  9 |         - /Odometry1/Odometry1
 10 |         - /Odometry1/Odometry1/Shape1
 11 |         - /Odometry1/Odometry1/Covariance1
 12 |         - /Odometry1/Odometry1/Covariance1/Position1
 13 |         - /Odometry1/Odometry1/Covariance1/Orientation1
 14 |         - /Path1
 15 |         - /MarkerArray1/Namespaces1
 16 |       Splitter Ratio: 0.6432291865348816
 17 |     Tree Height: 777
 18 |   - Class: rviz/Selection
 19 |     Name: Selection
 20 |   - Class: rviz/Tool Properties
 21 |     Expanded:
 22 |       - /2D Pose Estimate1
 23 |       - /2D Nav Goal1
 24 |       - /Publish Point1
 25 |     Name: Tool Properties
 26 |     Splitter Ratio: 0.5886790156364441
 27 |   - Class: rviz/Views
 28 |     Expanded:
 29 |       - /Current View1
 30 |     Name: Views
 31 |     Splitter Ratio: 0.5
 32 |   - Class: rviz/Time
 33 |     Name: Time
 34 |     SyncMode: 0
 35 |     SyncSource: currPoints
 36 | Preferences:
 37 |   PromptSaveOnExit: true
 38 | Toolbars:
 39 |   toolButtonStyle: 2
 40 | Visualization Manager:
 41 |   Class: ""
 42 |   Displays:
 43 |     - Alpha: 1
 44 |       Cell Size: 1000
 45 |       Class: rviz/Grid
 46 |       Color: 160; 160; 164
 47 |       Enabled: false
 48 |       Line Style:
 49 |         Line Width: 0.029999999329447746
 50 |         Value: Lines
 51 |       Name: Grid
 52 |       Normal Cell Count: 0
 53 |       Offset:
 54 |         X: 0
 55 |         Y: 0
 56 |         Z: 0
 57 |       Plane: XY
 58 |       Plane Cell Count: 40
 59 |       Reference Frame: <Fixed Frame>
 60 |       Value: false
 61 |     - Alpha: 1
 62 |       Class: rviz/Axes
 63 |       Enabled: false
 64 |       Length: 0.699999988079071
 65 |       Name: Axes
 66 |       Radius: 0.05999999865889549
 67 |       Reference Frame: <Fixed Frame>
 68 |       Show Trail: false
 69 |       Value: false
 70 |     - Class: rviz/Group
 71 |       Displays:
 72 |         - Alpha: 1
 73 |           Autocompute Intensity Bounds: true
 74 |           Autocompute Value Bounds:
 75 |             Max Value: 10
 76 |             Min Value: -10
 77 |             Value: true
 78 |           Axis: Z
 79 |           Channel Name: intensity
 80 |           Class: rviz/PointCloud2
 81 |           Color: 238; 238; 236
 82 |           Color Transformer: Intensity
 83 |           Decay Time: 0
 84 |           Enabled: false
 85 |           Invert Rainbow: false
 86 |           Max Color: 255; 255; 255
 87 |           Min Color: 238; 238; 236
 88 |           Name: surround
 89 |           Position Transformer: XYZ
 90 |           Queue Size: 1
 91 |           Selectable: false
 92 |           Size (Pixels): 3
 93 |           Size (m): 0.05000000074505806
 94 |           Style: Points
 95 |           Topic: /Invarian_fastlio/cloud_registered
 96 |           Unreliable: false
 97 |           Use Fixed Frame: true
 98 |           Use rainbow: true
 99 |           Value: false
100 |         - Alpha: 0.10000000149011612
101 |           Autocompute Intensity Bounds: true
102 |           Autocompute Value Bounds:
103 |             Max Value: 20.979427337646484
104 |             Min Value: -1.2759575843811035
105 |             Value: true
106 |           Axis: Z
107 |           Channel Name: intensity
108 |           Class: rviz/PointCloud2
109 |           Color: 255; 255; 255
110 |           Color Transformer: AxisColor
111 |           Decay Time: 1000
112 |           Enabled: true
113 |           Invert Rainbow: true
114 |           Max Color: 255; 255; 255
115 |           Min Color: 0; 0; 0
116 |           Name: currPoints
117 |           Position Transformer: XYZ
118 |           Queue Size: 100000
119 |           Selectable: true
120 |           Size (Pixels): 1
121 |           Size (m): 0.009999999776482582
122 |           Style: Points
123 |           Topic: /Invarian_fastlio/cloud_registered
124 |           Unreliable: false
125 |           Use Fixed Frame: true
126 |           Use rainbow: true
127 |           Value: true
128 |         - Alpha: 1
129 |           Autocompute Intensity Bounds: true
130 |           Autocompute Value Bounds:
131 |             Max Value: 10
132 |             Min Value: -10
133 |             Value: true
134 |           Axis: Z
135 |           Channel Name: intensity
136 |           Class: rviz/PointCloud2
137 |           Color: 255; 0; 0
138 |           Color Transformer: FlatColor
139 |           Decay Time: 0
140 |           Enabled: false
141 |           Invert Rainbow: false
142 |           Max Color: 255; 255; 255
143 |           Min Color: 0; 0; 0
144 |           Name: PointCloud2
145 |           Position Transformer: XYZ
146 |           Queue Size: 10
147 |           Selectable: true
148 |           Size (Pixels): 3
149 |           Size (m): 0.10000000149011612
150 |           Style: Flat Squares
151 |           Topic: /Laser_map
152 |           Unreliable: false
153 |           Use Fixed Frame: true
154 |           Use rainbow: true
155 |           Value: false
156 |       Enabled: true
157 |       Name: mapping
158 |     - Class: rviz/Group
159 |       Displays:
160 |         - Angle Tolerance: 0.009999999776482582
161 |           Class: rviz/Odometry
162 |           Covariance:
163 |             Orientation:
164 |               Alpha: 0.5
165 |               Color: 255; 255; 127
166 |               Color Style: Unique
167 |               Frame: Local
168 |               Offset: 1
169 |               Scale: 1
170 |               Value: true
171 |             Position:
172 |               Alpha: 0.30000001192092896
173 |               Color: 204; 51; 204
174 |               Scale: 1
175 |               Value: true
176 |             Value: true
177 |           Enabled: true
178 |           Keep: 1
179 |           Name: Odometry
180 |           Position Tolerance: 0.0010000000474974513
181 |           Queue Size: 10
182 |           Shape:
183 |             Alpha: 1
184 |             Axes Length: 10
185 |             Axes Radius: 1
186 |             Color: 255; 85; 0
187 |             Head Length: 0
188 |             Head Radius: 0
189 |             Shaft Length: 0.05000000074505806
190 |             Shaft Radius: 0.05000000074505806
191 |             Value: Axes
192 |           Topic: /Invarian_fastlio/Odometry
193 |           Unreliable: false
194 |           Value: true
195 |       Enabled: false
196 |       Name: Odometry
197 |     - Alpha: 1
198 |       Class: rviz/Axes
199 |       Enabled: false
200 |       Length: 10
201 |       Name: Axes
202 |       Radius: 1
203 |       Reference Frame: <Fixed Frame>
204 |       Show Trail: false
205 |       Value: false
206 |     - Alpha: 0
207 |       Buffer Length: 2
208 |       Class: rviz/Path
209 |       Color: 204; 0; 0
210 |       Enabled: true
211 |       Head Diameter: 0
212 |       Head Length: 0
213 |       Length: 0.30000001192092896
214 |       Line Style: Billboards
215 |       Line Width: 2
216 |       Name: Path
217 |       Offset:
218 |         X: 0
219 |         Y: 0
220 |         Z: 0
221 |       Pose Color: 25; 255; 255
222 |       Pose Style: None
223 |       Queue Size: 10
224 |       Radius: 0.029999999329447746
225 |       Shaft Diameter: 0.4000000059604645
226 |       Shaft Length: 0.4000000059604645
227 |       Topic: /Invarian_fastlio/path
228 |       Unreliable: false
229 |       Value: true
230 |     - Alpha: 1
231 |       Autocompute Intensity Bounds: true
232 |       Autocompute Value Bounds:
233 |         Max Value: 13.139549255371094
234 |         Min Value: -32.08251953125
235 |         Value: true
236 |       Axis: Z
237 |       Channel Name: intensity
238 |       Class: rviz/PointCloud2
239 |       Color: 138; 226; 52
240 |       Color Transformer: FlatColor
241 |       Decay Time: 0
242 |       Enabled: false
243 |       Invert Rainbow: false
244 |       Max Color: 138; 226; 52
245 |       Min Color: 138; 226; 52
246 |       Name: PointCloud2
247 |       Position Transformer: XYZ
248 |       Queue Size: 10
249 |       Selectable: true
250 |       Size (Pixels): 3
251 |       Size (m): 0.10000000149011612
252 |       Style: Flat Squares
253 |       Topic: /Laser_map
254 |       Unreliable: false
255 |       Use Fixed Frame: true
256 |       Use rainbow: true
257 |       Value: false
258 |     - Class: rviz/MarkerArray
259 |       Enabled: false
260 |       Marker Topic: /MarkerArray
261 |       Name: MarkerArray
262 |       Namespaces:
263 |         {}
264 |       Queue Size: 100
265 |       Value: false
266 |     - Alpha: 1
267 |       Buffer Length: 1
268 |       Class: rviz/Path
269 |       Color: 245; 121; 0
270 |       Enabled: true
271 |       Head Diameter: 0.30000001192092896
272 |       Head Length: 0.20000000298023224
273 |       Length: 0.30000001192092896
274 |       Line Style: Billboards
275 |       Line Width: 8
276 |       Name: Path
277 |       Offset:
278 |         X: 0
279 |         Y: 0
280 |         Z: 0
281 |       Pose Color: 255; 85; 255
282 |       Pose Style: None
283 |       Queue Size: 10
284 |       Radius: 0.029999999329447746
285 |       Shaft Diameter: 0.10000000149011612
286 |       Shaft Length: 0.10000000149011612
287 |       Topic: /path
288 |       Unreliable: false
289 |       Value: true
290 |   Enabled: true
291 |   Global Options:
292 |     Background Color: 0; 0; 0
293 |     Default Light: true
294 |     Fixed Frame: camera_init
295 |     Frame Rate: 10
296 |   Name: root
297 |   Tools:
298 |     - Class: rviz/Interact
299 |       Hide Inactive Objects: true
300 |     - Class: rviz/MoveCamera
301 |     - Class: rviz/Select
302 |     - Class: rviz/FocusCamera
303 |     - Class: rviz/Measure
304 |     - Class: rviz/SetInitialPose
305 |       Theta std deviation: 0.2617993950843811
306 |       Topic: /initialpose
307 |       X std deviation: 0.5
308 |       Y std deviation: 0.5
309 |     - Class: rviz/SetGoal
310 |       Topic: /move_base_simple/goal
311 |     - Class: rviz/PublishPoint
312 |       Single click: true
313 |       Topic: /clicked_point
314 |   Value: true
315 |   Views:
316 |     Current:
317 |       Class: rviz/Orbit
318 |       Distance: 256.545166015625
319 |       Enable Stereo Rendering:
320 |         Stereo Eye Separation: 0.05999999865889549
321 |         Stereo Focal Distance: 1
322 |         Swap Stereo Eyes: false
323 |         Value: false
324 |       Field of View: 0.7853981852531433
325 |       Focal Point:
326 |         X: 46.58790969848633
327 |         Y: -18.992557525634766
328 |         Z: -63.428977966308594
329 |       Focal Shape Fixed Size: true
330 |       Focal Shape Size: 0.05000000074505806
331 |       Invert Z Axis: false
332 |       Name: Current View
333 |       Near Clip Distance: 0.009999999776482582
334 |       Pitch: 1.544796347618103
335 |       Target Frame: global
336 |       Yaw: 4.0332932472229
337 |     Saved: ~
338 | Window Geometry:
339 |   Displays:
340 |     collapsed: false
341 |   Height: 1016
342 |   Hide Left Dock: false
343 |   Hide Right Dock: true
344 |   QMainWindow State: 000000ff00000000fd0000000400000000000001c800000346fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000346000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000056a0000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
345 |   Selection:
346 |     collapsed: false
347 |   Time:
348 |     collapsed: false
349 |   Tool Properties:
350 |     collapsed: false
351 |   Views:
352 |     collapsed: true
353 |   Width: 1848
354 |   X: 1992
355 |   Y: 27
356 | 


--------------------------------------------------------------------------------
/src/IMU_Processing.hpp:
--------------------------------------------------------------------------------
  1 | #include <cmath>
  2 | #include <math.h>
  3 | #include <deque>
  4 | #include <mutex>
  5 | #include <thread>
  6 | #include <fstream>
  7 | #include <csignal>
  8 | #include <ros/ros.h>
  9 | #include <Eigen/Eigen>
 10 | #include <common_lib.h>
 11 | #include <pcl/common/io.h>
 12 | #include <pcl/point_cloud.h>
 13 | #include <pcl/point_types.h>
 14 | #include <condition_variable>
 15 | #include <nav_msgs/Odometry.h>
 16 | #include <pcl/common/transforms.h>
 17 | #include <pcl/kdtree/kdtree_flann.h>
 18 | #include <tf/transform_broadcaster.h>
 19 | #include <eigen_conversions/eigen_msg.h>
 20 | #include <pcl_conversions/pcl_conversions.h>
 21 | #include <sensor_msgs/Imu.h>
 22 | #include <sensor_msgs/PointCloud2.h>
 23 | #include <geometry_msgs/Vector3.h>
 24 | 
 25 | #include "use-ikfom.hpp"
 26 | #include "esekfom.hpp"
 27 | 
 28 | /*
 29 | 这个hpp主要包含：
 30 | IMU数据预处理：IMU初始化，IMU正向传播，反向传播补偿运动失真   
 31 | */
 32 | 
 33 | #define MAX_INI_COUNT (10)  //最大迭代次数
 34 | //判断点的时间先后顺序(注意curvature中存储的是时间戳)
 35 | const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
 36 | 
 37 | class ImuProcess
 38 | {
 39 |  public:
 40 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 41 | 
 42 |   ImuProcess();
 43 |   ~ImuProcess();
 44 |   
 45 |   void Reset();
 46 |   void set_param(const V3D &transl, const M3D &rot, const V3D &gyr, const V3D &acc, const V3D &gyr_bias, const V3D &acc_bias);
 47 |   Eigen::Matrix<double, 12, 12> Q;    //噪声协方差矩阵  对应论文式(8)中的Q
 48 |   void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr &pcl_un_);
 49 | 
 50 |   V3D cov_acc;             //加速度协方差
 51 |   V3D cov_gyr;             //角速度协方差
 52 |   V3D cov_acc_scale;       //外部传入的 初始加速度协方差
 53 |   V3D cov_gyr_scale;       //外部传入的 初始角速度协方差
 54 |   V3D cov_bias_gyr;        //角速度bias的协方差
 55 |   V3D cov_bias_acc;        //加速度bias的协方差
 56 |   double first_lidar_time; //当前帧第一个点云时间
 57 | 
 58 |  private:
 59 |   void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N);
 60 |   void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out);
 61 | 
 62 |   PointCloudXYZI::Ptr cur_pcl_un_;        //当前帧点云未去畸变
 63 |   sensor_msgs::ImuConstPtr last_imu_;     // 上一帧imu
 64 |   vector<Pose6D> IMUpose;                 // 存储imu位姿(反向传播用)
 65 |   M3D Lidar_R_wrt_IMU;                    // lidar到IMU的旋转外参
 66 |   V3D Lidar_T_wrt_IMU;                    // lidar到IMU的平移外参
 67 |   V3D mean_acc;                           //加速度均值,用于计算方差
 68 |   V3D mean_gyr;                           //角速度均值，用于计算方差
 69 |   V3D angvel_last;                        //上一帧角速度
 70 |   V3D acc_s_last;                         //上一帧加速度
 71 |   double start_timestamp_;                //开始时间戳
 72 |   double last_lidar_end_time_;            //上一帧结束时间戳
 73 |   int init_iter_num = 1;                  //初始化迭代次数
 74 |   bool b_first_frame_ = true;             //是否是第一帧
 75 |   bool imu_need_init_ = true;             //是否需要初始化imu
 76 | };
 77 | 
 78 | ImuProcess::ImuProcess()
 79 |     : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)
 80 | {
 81 |   init_iter_num = 1;                          //初始化迭代次数
 82 |   Q = process_noise_cov();                    //调用use-ikfom.hpp里面的process_noise_cov初始化噪声协方差
 83 |   cov_acc = V3D(0.1, 0.1, 0.1);               //加速度协方差初始化
 84 |   cov_gyr = V3D(0.1, 0.1, 0.1);               //角速度协方差初始化
 85 |   cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); //角速度bias协方差初始化
 86 |   cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); //加速度bias协方差初始化
 87 |   mean_acc = V3D(0, 0, -1.0);
 88 |   mean_gyr = V3D(0, 0, 0);
 89 |   angvel_last = Zero3d;                       //上一帧角速度初始化
 90 |   Lidar_T_wrt_IMU = Zero3d;                   // lidar到IMU的位置外参初始化
 91 |   Lidar_R_wrt_IMU = Eye3d;                    // lidar到IMU的旋转外参初始化
 92 |   last_imu_.reset(new sensor_msgs::Imu());    //上一帧imu初始化
 93 | }
 94 | 
 95 | ImuProcess::~ImuProcess() {}
 96 | 
 97 | void ImuProcess::Reset()   //重置参数
 98 | {
 99 |   // ROS_WARN("Reset ImuProcess");
100 |   mean_acc = V3D(0, 0, -1.0);
101 |   mean_gyr = V3D(0, 0, 0);
102 |   angvel_last = Zero3d;
103 |   imu_need_init_ = true;                   //是否需要初始化imu
104 |   start_timestamp_ = -1;                   //开始时间戳
105 |   init_iter_num = 1;                       //初始化迭代次数
106 |   IMUpose.clear();                         // imu位姿清空
107 |   last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化
108 |   cur_pcl_un_.reset(new PointCloudXYZI()); //当前帧点云未去畸变初始化
109 | }
110 | 
111 | //传入外部参数
112 | void ImuProcess::set_param(const V3D &transl, const M3D &rot, const V3D &gyr, const V3D &acc, const V3D &gyr_bias, const V3D &acc_bias)  
113 | {
114 |   Lidar_T_wrt_IMU = transl;
115 |   Lidar_R_wrt_IMU = rot;
116 |   cov_gyr_scale = gyr;
117 |   cov_acc_scale = acc;
118 |   cov_bias_gyr = gyr_bias;
119 |   cov_bias_acc = acc_bias;
120 | }
121 | 
122 | 
123 | //IMU初始化：利用开始的IMU帧的平均值初始化状态量x
124 | void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N)
125 | {
126 |   //MeasureGroup这个struct表示当前过程中正在处理的所有数据，包含IMU队列和一帧lidar的点云 以及lidar的起始和结束时间
127 |   //初始化重力、陀螺仪偏差、acc和陀螺仪协方差  将加速度测量值归一化为单位重力   **/
128 |   V3D cur_acc, cur_gyr;
129 |   
130 |   if (b_first_frame_) //如果为第一帧IMU
131 |   {
132 |     Reset();    //重置IMU参数
133 |     N = 1;      //将迭代次数置1
134 |     b_first_frame_ = false;
135 |     const auto &imu_acc = meas.imu.front()->linear_acceleration;    //IMU初始时刻的加速度
136 |     const auto &gyr_acc = meas.imu.front()->angular_velocity;       //IMU初始时刻的角速度
137 |     mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;              //第一帧加速度值作为初始化均值
138 |     mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;              //第一帧角速度值作为初始化均值
139 |     first_lidar_time = meas.lidar_beg_time;                   //将当前IMU帧对应的lidar起始时间 作为初始时间
140 |   }
141 | 
142 |   for (const auto &imu : meas.imu)    //根据所有IMU数据，计算平均值和方差
143 |   {
144 |     const auto &imu_acc = imu->linear_acceleration;
145 |     const auto &gyr_acc = imu->angular_velocity;
146 |     cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
147 |     cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
148 | 
149 |     mean_acc  += (cur_acc - mean_acc) / N;    //根据当前帧和均值差作为均值的更新
150 |     mean_gyr  += (cur_gyr - mean_gyr) / N;
151 | 
152 |     cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc)  / N;
153 |     cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr)  / N / N * (N-1);
154 | 
155 |     N ++;
156 |   }
157 |   
158 |   state_ikfom init_state = kf_state.get_x();        //在esekfom.hpp获得x_的状态
159 |   init_state.grav = - mean_acc / mean_acc.norm() * G_m_s2;    //得平均测量的单位方向向量 * 重力加速度预设值
160 |   
161 |   init_state.bg  = mean_gyr;      //角速度测量作为陀螺仪偏差
162 |   init_state.offset_T_L_I = Lidar_T_wrt_IMU;      //将lidar和imu外参传入
163 |   init_state.offset_R_L_I = Sophus::SO3(Lidar_R_wrt_IMU);
164 |   kf_state.change_x(init_state);      //将初始化后的状态传入esekfom.hpp中的x_
165 | 
166 |   Matrix<double, 18, 18> init_P = MatrixXd::Identity(18,18);      //在esekfom.hpp获得P_的协方差矩阵
167 |   // init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;
168 |   // init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
169 |   init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.0001;
170 |   init_P(12,12) = init_P(13,13) = init_P(14,14) = 0.001;
171 |   init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.00001; 
172 |   kf_state.change_P(init_P);
173 |   last_imu_ = meas.imu.back();
174 | 
175 |   // std::cout << "IMU init new -- init_state  " << init_state.pos  <<" " << init_state.bg <<" " << init_state.ba <<" " << init_state.grav << std::endl;
176 | }
177 | 
178 | //反向传播
179 | void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out)
180 | {
181 |   /***将上一帧最后尾部的imu添加到当前帧头部的imu ***/
182 |   auto v_imu = meas.imu;         //取出当前帧的IMU队列
183 |   v_imu.push_front(last_imu_);   //将上一帧最后尾部的imu添加到当前帧头部的imu
184 |   const double &imu_end_time = v_imu.back()->header.stamp.toSec();    //拿到当前帧尾部的imu的时间
185 |   const double &pcl_beg_time = meas.lidar_beg_time;      // 点云开始和结束的时间戳
186 |   const double &pcl_end_time = meas.lidar_end_time;
187 |   
188 |   // 根据点云中每个点的时间戳对点云进行重排序
189 |   pcl_out = *(meas.lidar);
190 |   sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);  //这里curvature中存放了时间戳（在preprocess.cpp中）
191 | 
192 | 
193 |   state_ikfom imu_state = kf_state.get_x();  // 获取上一次KF估计的后验状态作为本次IMU预测的初始状态
194 |   IMUpose.clear();
195 |   IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.matrix()));
196 |   //将初始状态加入IMUpose中,包含有时间间隔，上一帧加速度，上一帧角速度，上一帧速度，上一帧位置，上一帧旋转矩阵
197 | 
198 |   /*** 前向传播 ***/
199 |   V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu; // angvel_avr为平均角速度，acc_avr为平均加速度，acc_imu为imu加速度，vel_imu为imu速度，pos_imu为imu位置
200 |   M3D R_imu;    //IMU旋转矩阵 消除运动失真的时候用
201 | 
202 |   double dt = 0;
203 | 
204 |   input_ikfom in;
205 |   // 遍历本次估计的所有IMU测量并且进行积分，离散中值法 前向传播
206 |   for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
207 |   {
208 |     auto &&head = *(it_imu);        //拿到当前帧的imu数据
209 |     auto &&tail = *(it_imu + 1);    //拿到下一帧的imu数据
210 |     //判断时间先后顺序：下一帧时间戳是否小于上一帧结束时间戳 不符合直接continue
211 |     if (tail->header.stamp.toSec() < last_lidar_end_time_)    continue;
212 |     
213 |     angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),      // 中值积分
214 |                 0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
215 |                 0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
216 |     acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
217 |                 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
218 |                 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
219 | 
220 |     acc_avr  = acc_avr * G_m_s2 / mean_acc.norm(); //通过重力数值对加速度进行调整(除上初始化的IMU大小*9.8)
221 | 
222 |     //如果IMU开始时刻早于上次雷达最晚时刻(因为将上次最后一个IMU插入到此次开头了，所以会出现一次这种情况)
223 |     if(head->header.stamp.toSec() < last_lidar_end_time_)
224 |     {
225 |       dt = tail->header.stamp.toSec() - last_lidar_end_time_; //从上次雷达时刻末尾开始传播 计算与此次IMU结尾之间的时间差
226 |     }
227 |     else
228 |     {
229 |       dt = tail->header.stamp.toSec() - head->header.stamp.toSec();     //两个IMU时刻之间的时间间隔
230 |     }
231 |     
232 |     in.acc = acc_avr;     // 两帧IMU的中值作为输入in  用于前向传播
233 |     in.gyro = angvel_avr;
234 |     Q.block<3, 3>(0, 0).diagonal() = cov_gyr;         // 配置协方差矩阵
235 |     Q.block<3, 3>(3, 3).diagonal() = cov_acc;
236 |     Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
237 |     Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
238 | 
239 |     kf_state.predict(dt, Q, in);    // IMU前向传播，每次传播的时间间隔为dt
240 | 
241 |     imu_state = kf_state.get_x();   //更新IMU状态为积分后的状态
242 |     //更新上一帧角速度 = 后一帧角速度-bias  
243 |     angvel_last = V3D(tail->angular_velocity.x, tail->angular_velocity.y, tail->angular_velocity.z) - imu_state.bg;
244 |     //更新上一帧世界坐标系下的加速度 = R*(加速度-bias) - g
245 |     acc_s_last  = V3D(tail->linear_acceleration.x, tail->linear_acceleration.y, tail->linear_acceleration.z) * G_m_s2 / mean_acc.norm();   
246 | 
247 |     // std::cout << "acc_s_last: " << acc_s_last.transpose() << std::endl;
248 |     // std::cout << "imu_state.ba: " << imu_state.ba.transpose() << std::endl;
249 |     // std::cout << "imu_state.grav: " << imu_state.grav.transpose() << std::endl;
250 |     acc_s_last = imu_state.rot * (acc_s_last - imu_state.ba) + imu_state.grav;
251 |     // std::cout << "--acc_s_last: " << acc_s_last.transpose() << std::endl<< std::endl;
252 | 
253 |     double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;    //后一个IMU时刻距离此次雷达开始的时间间隔
254 |     IMUpose.push_back( set_pose6d( offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.matrix() ) );
255 |   }
256 | 
257 |   // 把最后一帧IMU测量也补上
258 |   dt = abs(pcl_end_time - imu_end_time);
259 |   kf_state.predict(dt, Q, in);
260 |   imu_state = kf_state.get_x();   
261 |   last_imu_ = meas.imu.back();              //保存最后一个IMU测量，以便于下一帧使用
262 |   last_lidar_end_time_ = pcl_end_time;      //保存这一帧最后一个雷达测量的结束时间，以便于下一帧使用
263 | 
264 |    /***消除每个激光雷达点的失真（反向传播）***/
265 |   if (pcl_out.points.begin() == pcl_out.points.end()) return;
266 |   auto it_pcl = pcl_out.points.end() - 1;
267 | 
268 |   //遍历每个IMU帧
269 |   for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
270 |   {
271 |     auto head = it_kp - 1;
272 |     auto tail = it_kp;
273 |     R_imu<<MAT_FROM_ARRAY(head->rot);   //拿到前一帧的IMU旋转矩阵
274 |     // cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
275 |     vel_imu<<VEC_FROM_ARRAY(head->vel);     //拿到前一帧的IMU速度
276 |     pos_imu<<VEC_FROM_ARRAY(head->pos);     //拿到前一帧的IMU位置
277 |     acc_imu<<VEC_FROM_ARRAY(tail->acc);     //拿到后一帧的IMU加速度
278 |     angvel_avr<<VEC_FROM_ARRAY(tail->gyr);  //拿到后一帧的IMU角速度
279 | 
280 |     //之前点云按照时间从小到大排序过，IMUpose也同样是按照时间从小到大push进入的
281 |     //此时从IMUpose的末尾开始循环，也就是从时间最大处开始，因此只需要判断 点云时间需>IMU head时刻  即可   不需要判断 点云时间<IMU tail
282 |     for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
283 |     {
284 |       dt = it_pcl->curvature / double(1000) - head->offset_time;    //点到IMU开始时刻的时间间隔 
285 | 
286 |       /*    P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei)    */
287 | 
288 |       M3D R_i(R_imu * Sophus::SO3::exp(angvel_avr * dt).matrix() );   //点it_pcl所在时刻的旋转：前一帧的IMU旋转矩阵 * exp(后一帧角速度*dt)   
289 |       
290 |       V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);   //点所在时刻的位置(雷达坐标系下)
291 |       V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);   //从点所在的世界位置-雷达末尾世界位置
292 |       V3D P_compensate = imu_state.offset_R_L_I.matrix().transpose() * (imu_state.rot.matrix().transpose() * (R_i * (imu_state.offset_R_L_I.matrix() * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);
293 | 
294 |       it_pcl->x = P_compensate(0);
295 |       it_pcl->y = P_compensate(1);
296 |       it_pcl->z = P_compensate(2);
297 | 
298 |       if (it_pcl == pcl_out.points.begin()) break;
299 |     }
300 |   }
301 | }
302 | 
303 | 
304 | double T1,T2;
305 | void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr &cur_pcl_un_)
306 | {
307 |   // T1 = omp_get_wtime();
308 | 
309 |   if(meas.imu.empty()) {return;};
310 |   ROS_ASSERT(meas.lidar != nullptr);
311 | 
312 |   if (imu_need_init_)   
313 |   {
314 |     // The very first lidar frame
315 |     IMU_init(meas, kf_state, init_iter_num);  //如果开头几帧  需要初始化IMU参数
316 | 
317 |     imu_need_init_ = true;
318 |     
319 |     last_imu_   = meas.imu.back();
320 | 
321 |     state_ikfom imu_state = kf_state.get_x();
322 | 
323 |     if (init_iter_num > MAX_INI_COUNT)
324 |     {
325 |       cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
326 |       imu_need_init_ = false;
327 | 
328 |       cov_acc = cov_acc_scale;
329 |       cov_gyr = cov_gyr_scale;
330 |       ROS_INFO("IMU Initial Done");
331 |     }
332 | 
333 |     return;
334 |   }
335 | 
336 |   UndistortPcl(meas, kf_state, *cur_pcl_un_); 
337 | 
338 |   // T2 = omp_get_wtime();
339 |   // cout<<"[ IMU Process ]: Time: "<<T2 - T1<<endl;
340 | }
341 | 


--------------------------------------------------------------------------------
/src/laserMapping_re.cpp:
--------------------------------------------------------------------------------
  1 | #include <omp.h>
  2 | #include <mutex>
  3 | #include <math.h>
  4 | #include <thread>
  5 | #include <fstream>
  6 | #include <csignal>
  7 | #include <unistd.h>
  8 | #include <ros/ros.h>
  9 | #include <Eigen/Core>
 10 | #include <nav_msgs/Odometry.h>
 11 | #include <nav_msgs/Path.h>
 12 | #include <visualization_msgs/Marker.h>
 13 | #include <pcl_conversions/pcl_conversions.h>
 14 | #include <pcl/point_cloud.h>
 15 | #include <pcl/point_types.h>
 16 | #include <pcl/filters/voxel_grid.h>
 17 | #include <pcl/io/pcd_io.h>
 18 | #include <sensor_msgs/PointCloud2.h>
 19 | #include <tf/transform_datatypes.h>
 20 | #include <tf/transform_broadcaster.h>
 21 | #include <geometry_msgs/Vector3.h>
 22 | #include <livox_ros_driver/CustomMsg.h>
 23 | #include "preprocess.h"
 24 | #include <ikd-Tree/ikd_Tree.h>
 25 | 
 26 | #include "IMU_Processing.hpp"
 27 | 
 28 | #define INIT_TIME (0.1)
 29 | #define LASER_POINT_COV (0.001)
 30 | #define PUBFRAME_PERIOD (20)
 31 | 
 32 | /*** Time Log Variables ***/
 33 | int add_point_size = 0, kdtree_delete_counter = 0;
 34 | bool pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true, path_en = true;
 35 | /**************************/
 36 | 
 37 | float res_last[100000] = {0.0};
 38 | float DET_RANGE = 300.0f;
 39 | const float MOV_THRESHOLD = 1.5f;
 40 | double time_diff_lidar_to_imu = 0.0;
 41 | 
 42 | mutex mtx_buffer;
 43 | condition_variable sig_buffer;
 44 | ros::Publisher pubLaserCloudMap;
 45 | 
 46 | string root_dir = ROOT_DIR;
 47 | string map_file_path, lid_topic, imu_topic;
 48 | 
 49 | double last_timestamp_lidar = 0, last_timestamp_imu = -1.0;
 50 | double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001;
 51 | double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0;
 52 | double cube_len = 0, lidar_end_time = 0, first_lidar_time = 0.0;
 53 | int scan_count = 0, publish_count = 0;
 54 | int feats_down_size = 0, NUM_MAX_ITERATIONS = 0, pcd_index = 0;
 55 | 
 56 | bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited;
 57 | bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
 58 | 
 59 | vector<BoxPointType> cub_needrm;
 60 | vector<PointVector> Nearest_Points;
 61 | vector<double> extrinT(3, 0.0);
 62 | vector<double> extrinR(9, 0.0);
 63 | vector<double> init_pos(3, 0.0);
 64 | vector<double> init_rot{0, 0, 0, 1};
 65 | deque<double> time_buffer;
 66 | deque<PointCloudXYZI::Ptr> lidar_buffer;
 67 | deque<sensor_msgs::Imu::ConstPtr> imu_buffer;
 68 | 
 69 | PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());
 70 | PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());
 71 | PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());  //畸变纠正后降采样的单帧点云，lidar系
 72 | PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，W系
 73 | PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
 74 | 
 75 | pcl::VoxelGrid<PointType> downSizeFilterSurf;
 76 | pcl::VoxelGrid<PointType> downSizeFilterMap;
 77 | 
 78 | KD_TREE<PointType> ikdtree;
 79 | 
 80 | V3D Lidar_T_wrt_IMU(Zero3d);
 81 | M3D Lidar_R_wrt_IMU(Eye3d);
 82 | 
 83 | /*** EKF inputs and output ***/
 84 | MeasureGroup Measures;
 85 | 
 86 | esekfom::esekf kf;
 87 | 
 88 | state_ikfom state_point;
 89 | Eigen::Vector3d pos_lid; //估计的W系下的位置
 90 | 
 91 | nav_msgs::Path path;
 92 | nav_msgs::Odometry odomAftMapped;
 93 | geometry_msgs::PoseStamped msg_body_pose;
 94 | 
 95 | shared_ptr<Preprocess> p_pre(new Preprocess());
 96 | 
 97 | void SigHandle(int sig)
 98 | {
 99 |     flg_exit = true;
100 |     ROS_WARN("catch sig %d", sig);
101 |     sig_buffer.notify_all();
102 | }
103 | 
104 | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
105 | {
106 |     mtx_buffer.lock();
107 |     scan_count++;
108 |     double preprocess_start_time = omp_get_wtime();
109 |     if (msg->header.stamp.toSec() < last_timestamp_lidar)
110 |     {
111 |         ROS_ERROR("lidar loop back, clear buffer");
112 |         lidar_buffer.clear();
113 |     }
114 | 
115 |     PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
116 |     p_pre->process(msg, ptr);
117 |     lidar_buffer.push_back(ptr);
118 |     time_buffer.push_back(msg->header.stamp.toSec());
119 |     last_timestamp_lidar = msg->header.stamp.toSec();
120 |     mtx_buffer.unlock();
121 |     sig_buffer.notify_all();
122 | }
123 | 
124 | double timediff_lidar_wrt_imu = 0.0;
125 | bool timediff_set_flg = false;
126 | void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
127 | {
128 |     mtx_buffer.lock();
129 |     double preprocess_start_time = omp_get_wtime();
130 |     scan_count++;
131 |     if (msg->header.stamp.toSec() < last_timestamp_lidar)
132 |     {
133 |         ROS_ERROR("lidar loop back, clear buffer");
134 |         lidar_buffer.clear();
135 |     }
136 |     last_timestamp_lidar = msg->header.stamp.toSec();
137 | 
138 |     if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty())
139 |     {
140 |         printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n", last_timestamp_imu, last_timestamp_lidar);
141 |     }
142 | 
143 |     if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())
144 |     {
145 |         timediff_set_flg = true;
146 |         timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu;
147 |         printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu);
148 |     }
149 | 
150 |     PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
151 |     p_pre->process(msg, ptr);
152 |     lidar_buffer.push_back(ptr);
153 |     time_buffer.push_back(last_timestamp_lidar);
154 | 
155 |     mtx_buffer.unlock();
156 |     sig_buffer.notify_all();
157 | }
158 | 
159 | void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
160 | {
161 |     publish_count++;
162 |     // cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;
163 |     sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
164 | 
165 |     if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en)
166 |     {
167 |         msg->header.stamp =
168 |             ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec());
169 |     }
170 | 
171 |     msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_diff_lidar_to_imu);
172 | 
173 |     double timestamp = msg->header.stamp.toSec();
174 | 
175 |     mtx_buffer.lock();
176 | 
177 |     if (timestamp < last_timestamp_imu)
178 |     {
179 |         ROS_WARN("imu loop back, clear buffer");
180 |         imu_buffer.clear();
181 |     }
182 | 
183 |     last_timestamp_imu = timestamp;
184 | 
185 |     imu_buffer.push_back(msg);
186 |     mtx_buffer.unlock();
187 |     sig_buffer.notify_all();
188 | }
189 | 
190 | double lidar_mean_scantime = 0.0;
191 | int scan_num = 0;
192 | //把当前要处理的LIDAR和IMU数据打包到meas
193 | bool sync_packages(MeasureGroup &meas)
194 | {
195 |     if (lidar_buffer.empty() || imu_buffer.empty())
196 |     {
197 |         return false;
198 |     }
199 | 
200 |     /*** push a lidar scan ***/
201 |     if (!lidar_pushed)
202 |     {
203 |         meas.lidar = lidar_buffer.front();
204 |         meas.lidar_beg_time = time_buffer.front();
205 |         if (meas.lidar->points.size() <= 5) // time too little
206 |         {
207 |             lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
208 |             ROS_WARN("Too few input point cloud!\n");
209 |         }
210 |         else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime)
211 |         {
212 |             lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
213 |         }
214 |         else
215 |         {
216 |             scan_num++;
217 |             lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
218 |             lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
219 |         }
220 | 
221 |         meas.lidar_end_time = lidar_end_time;
222 | 
223 |         lidar_pushed = true;
224 |     }
225 | 
226 |     if (last_timestamp_imu < lidar_end_time)
227 |     {
228 |         return false;
229 |     }
230 | 
231 |     /*** push imu data, and pop from imu buffer ***/
232 |     double imu_time = imu_buffer.front()->header.stamp.toSec();
233 |     meas.imu.clear();
234 |     while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
235 |     {
236 |         imu_time = imu_buffer.front()->header.stamp.toSec();
237 |         if (imu_time > lidar_end_time)
238 |             break;
239 |         meas.imu.push_back(imu_buffer.front());
240 |         imu_buffer.pop_front();
241 |     }
242 | 
243 |     lidar_buffer.pop_front();
244 |     time_buffer.pop_front();
245 |     lidar_pushed = false;
246 |     return true;
247 | }
248 | 
249 | void pointBodyToWorld(PointType const *const pi, PointType *const po)
250 | {
251 |     V3D p_body(pi->x, pi->y, pi->z);
252 |     V3D p_global(state_point.rot.matrix() * (state_point.offset_R_L_I.matrix() * p_body + state_point.offset_T_L_I) + state_point.pos);
253 | 
254 |     po->x = p_global(0);
255 |     po->y = p_global(1);
256 |     po->z = p_global(2);
257 |     po->intensity = pi->intensity;
258 | }
259 | 
260 | template <typename T>
261 | void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
262 | {
263 |     V3D p_body(pi[0], pi[1], pi[2]);
264 |     V3D p_global(state_point.rot.matrix() * (state_point.offset_R_L_I.matrix() * p_body + state_point.offset_T_L_I) + state_point.pos);
265 | 
266 |     po[0] = p_global(0);
267 |     po[1] = p_global(1);
268 |     po[2] = p_global(2);
269 | }
270 | 
271 | BoxPointType LocalMap_Points;      // ikd-tree地图立方体的2个角点
272 | bool Localmap_Initialized = false; // 局部地图是否初始化
273 | void lasermap_fov_segment()
274 | {
275 |     cub_needrm.clear(); // 清空需要移除的区域
276 |     kdtree_delete_counter = 0;
277 | 
278 |     V3D pos_LiD = pos_lid; // W系下位置
279 |     //初始化局部地图范围，以pos_LiD为中心,长宽高均为cube_len
280 |     if (!Localmap_Initialized)
281 |     {
282 |         for (int i = 0; i < 3; i++)
283 |         {
284 |             LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
285 |             LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
286 |         }
287 |         Localmap_Initialized = true;
288 |         return;
289 |     }
290 | 
291 |     //各个方向上pos_LiD与局部地图边界的距离
292 |     float dist_to_map_edge[3][2];
293 |     bool need_move = false;
294 |     for (int i = 0; i < 3; i++)
295 |     {
296 |         dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
297 |         dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
298 |         // 与某个方向上的边界距离（1.5*300m）太小，标记需要移除need_move(FAST-LIO2论文Fig.3)
299 |         if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
300 |             need_move = true;
301 |     }
302 |     if (!need_move)
303 |         return; //如果不需要，直接返回，不更改局部地图
304 | 
305 |     BoxPointType New_LocalMap_Points, tmp_boxpoints;
306 |     New_LocalMap_Points = LocalMap_Points;
307 |     //需要移动的距离
308 |     float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1)));
309 |     for (int i = 0; i < 3; i++)
310 |     {
311 |         tmp_boxpoints = LocalMap_Points;
312 |         if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE)
313 |         {
314 |             New_LocalMap_Points.vertex_max[i] -= mov_dist;
315 |             New_LocalMap_Points.vertex_min[i] -= mov_dist;
316 |             tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
317 |             cub_needrm.push_back(tmp_boxpoints);
318 |         }
319 |         else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
320 |         {
321 |             New_LocalMap_Points.vertex_max[i] += mov_dist;
322 |             New_LocalMap_Points.vertex_min[i] += mov_dist;
323 |             tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
324 |             cub_needrm.push_back(tmp_boxpoints);
325 |         }
326 |     }
327 |     LocalMap_Points = New_LocalMap_Points;
328 | 
329 |     PointVector points_history;
330 |     ikdtree.acquire_removed_points(points_history);
331 | 
332 |     if (cub_needrm.size() > 0)
333 |         kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); //删除指定范围内的点
334 | }
335 | 
336 | void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
337 | {
338 |     V3D p_body_lidar(pi->x, pi->y, pi->z);
339 |     V3D p_body_imu(state_point.offset_R_L_I.matrix() * p_body_lidar + state_point.offset_T_L_I);
340 | 
341 |     po->x = p_body_imu(0);
342 |     po->y = p_body_imu(1);
343 |     po->z = p_body_imu(2);
344 |     po->intensity = pi->intensity;
345 | }
346 | 
347 | //根据最新估计位姿  增量添加点云到map
348 | void init_ikdtree()
349 | {
350 |     //加载读取点云数据到cloud中
351 |     string all_points_dir(string(string(ROOT_DIR) + "PCD/") + "GlobalMap_ikdtree.pcd");
352 |     if (pcl::io::loadPCDFile<PointType>(all_points_dir, *cloud) == -1)
353 |     {
354 |         PCL_ERROR("Read file fail!\n");
355 |     }
356 | 
357 |     ikdtree.set_downsample_param(filter_size_map_min);
358 |     ikdtree.Build(cloud->points);
359 |     std::cout << "---- ikdtree size: " << ikdtree.size() << std::endl;
360 | }
361 | 
362 | PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
363 | PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
364 | void publish_frame_world(const ros::Publisher &pubLaserCloudFull_)
365 | {
366 |     if (scan_pub_en)
367 |     {
368 |         PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
369 |         int size = laserCloudFullRes->points.size();
370 |         PointCloudXYZI::Ptr laserCloudWorld(
371 |             new PointCloudXYZI(size, 1));
372 | 
373 |         for (int i = 0; i < size; i++)
374 |         {
375 |             pointBodyToWorld(&laserCloudFullRes->points[i],
376 |                              &laserCloudWorld->points[i]);
377 |         }
378 | 
379 |         sensor_msgs::PointCloud2 laserCloudmsg;
380 |         pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
381 |         laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
382 |         laserCloudmsg.header.frame_id = "camera_init";
383 |         pubLaserCloudFull_.publish(laserCloudmsg);
384 |         publish_count -= PUBFRAME_PERIOD;
385 |     }
386 | 
387 |     /**************** save map ****************/
388 |     /* 1. make sure you have enough memories
389 |     /* 2. noted that pcd save will influence the real-time performences **/
390 |     if (pcd_save_en)
391 |     {
392 |         int size = feats_undistort->points.size();
393 |         PointCloudXYZI::Ptr laserCloudWorld(
394 |             new PointCloudXYZI(size, 1));
395 | 
396 |         for (int i = 0; i < size; i++)
397 |         {
398 |             pointBodyToWorld(&feats_undistort->points[i],
399 |                              &laserCloudWorld->points[i]);
400 |         }
401 | 
402 |         static int scan_wait_num = 0;
403 |         scan_wait_num++;
404 | 
405 |         if (scan_wait_num % 4 == 0)
406 |             *pcl_wait_save += *laserCloudWorld;
407 |     }
408 | }
409 | 
410 | void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body)
411 | {
412 |     int size = feats_undistort->points.size();
413 |     PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
414 | 
415 |     for (int i = 0; i < size; i++)
416 |     {
417 |         RGBpointBodyLidarToIMU(&feats_undistort->points[i],
418 |                                &laserCloudIMUBody->points[i]);
419 |     }
420 | 
421 |     sensor_msgs::PointCloud2 laserCloudmsg;
422 |     pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
423 |     laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
424 |     laserCloudmsg.header.frame_id = "body";
425 |     pubLaserCloudFull_body.publish(laserCloudmsg);
426 |     publish_count -= PUBFRAME_PERIOD;
427 | }
428 | 
429 | void publish_map(const ros::Publisher &pubLaserCloudMap)
430 | {
431 |     sensor_msgs::PointCloud2 laserCloudMap;
432 |     pcl::toROSMsg(*featsFromMap, laserCloudMap);
433 |     laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);
434 |     laserCloudMap.header.frame_id = "camera_init";
435 |     pubLaserCloudMap.publish(laserCloudMap);
436 | }
437 | 
438 | template <typename T>
439 | void set_posestamp(T &out)
440 | {
441 |     out.pose.position.x = state_point.pos(0);
442 |     out.pose.position.y = state_point.pos(1);
443 |     out.pose.position.z = state_point.pos(2);
444 | 
445 |     auto q_ = Eigen::Quaterniond(state_point.rot.matrix());
446 |     out.pose.orientation.x = q_.coeffs()[0];
447 |     out.pose.orientation.y = q_.coeffs()[1];
448 |     out.pose.orientation.z = q_.coeffs()[2];
449 |     out.pose.orientation.w = q_.coeffs()[3];
450 | }
451 | 
452 | void publish_odometry(const ros::Publisher &pubOdomAftMapped)
453 | {
454 |     odomAftMapped.header.frame_id = "camera_init";
455 |     odomAftMapped.child_frame_id = "body";
456 |     odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);
457 |     set_posestamp(odomAftMapped.pose);
458 |     pubOdomAftMapped.publish(odomAftMapped);
459 | 
460 |     auto P = kf.get_P();
461 |     for (int i = 0; i < 6; i++)
462 |     {
463 |         int k = i < 3 ? i + 3 : i - 3;
464 |         odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3);
465 |         odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4);
466 |         odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5);
467 |         odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0);
468 |         odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1);
469 |         odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2);
470 |     }
471 | 
472 |     static tf::TransformBroadcaster br;
473 |     tf::Transform transform;
474 |     tf::Quaternion q;
475 |     transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x,
476 |                                     odomAftMapped.pose.pose.position.y,
477 |                                     odomAftMapped.pose.pose.position.z));
478 |     q.setW(odomAftMapped.pose.pose.orientation.w);
479 |     q.setX(odomAftMapped.pose.pose.orientation.x);
480 |     q.setY(odomAftMapped.pose.pose.orientation.y);
481 |     q.setZ(odomAftMapped.pose.pose.orientation.z);
482 |     transform.setRotation(q);
483 |     br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "body"));
484 | }
485 | 
486 | void publish_path(const ros::Publisher pubPath)
487 | {
488 |     set_posestamp(msg_body_pose);
489 |     msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
490 |     msg_body_pose.header.frame_id = "camera_init";
491 | 
492 |     /*** if path is too large, the rvis will crash ***/
493 |     static int jjj = 0;
494 |     jjj++;
495 |     if (jjj % 10 == 0)
496 |     {
497 |         path.poses.push_back(msg_body_pose);
498 |         pubPath.publish(path);
499 |     }
500 | }
501 | 
502 | int main(int argc, char **argv)
503 | {
504 |     ros::init(argc, argv, "laserMapping");
505 |     ros::NodeHandle nh;
506 | 
507 |     nh.param<bool>("publish/path_en", path_en, true);
508 |     nh.param<bool>("publish/scan_publish_en", scan_pub_en, true);            // 是否发布当前正在扫描的点云的topic
509 |     nh.param<bool>("publish/dense_publish_en", dense_pub_en, true);          // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic
510 |     nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, true); // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，需要该变量和上一个变量同时为true才发布
511 |     nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4);                   // 卡尔曼滤波的最大迭代次数
512 |     nh.param<string>("map_file_path", map_file_path, "");                    // 地图保存路径
513 |     nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar");         // 雷达点云topic名称
514 |     nh.param<string>("common/imu_topic", imu_topic, "/livox/imu");           // IMU的topic名称
515 |     nh.param<bool>("common/time_sync_en", time_sync_en, false);              // 是否需要时间同步，只有当外部未进行时间同步时设为true
516 |     nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
517 |     nh.param<double>("filter_size_corner", filter_size_corner_min, 0.5); // VoxelGrid降采样时的体素大小
518 |     nh.param<double>("filter_size_surf", filter_size_surf_min, 0.5);
519 |     nh.param<double>("filter_size_map", filter_size_map_min, 0.5);
520 |     nh.param<double>("cube_side_length", cube_len, 200);    // 地图的局部区域的长度（FastLio2论文中有解释）
521 |     nh.param<float>("mapping/det_range", DET_RANGE, 300.f); // 激光雷达的最大探测范围
522 |     nh.param<double>("mapping/fov_degree", fov_deg, 180);
523 |     nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1);               // IMU陀螺仪的协方差
524 |     nh.param<double>("mapping/acc_cov", acc_cov, 0.1);               // IMU加速度计的协方差
525 |     nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001);        // IMU陀螺仪偏置的协方差
526 |     nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001);        // IMU加速度计偏置的协方差
527 |     nh.param<double>("preprocess/blind", p_pre->blind, 0.01);        // 最小距离阈值，即过滤掉0～blind范围内的点云
528 |     nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA); // 激光雷达的类型
529 |     nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);       // 激光雷达扫描的线数（livox avia为6线）
530 |     nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US);
531 |     nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
532 |     nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);           // 采样间隔，即每隔point_filter_num个点取1个点
533 |     nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); // 是否提取特征点（FAST_LIO2默认不进行特征点提取）
534 |     nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true);
535 |     nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false); // 是否将点云地图保存到PCD文件
536 |     nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>()); // 雷达相对于IMU的外参T（即雷达在IMU坐标系中的坐标）
537 |     nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>()); // 雷达相对于IMU的外参R
538 | 
539 |     nh.param<vector<double>>("mapping/init_pos", init_pos, vector<double>()); // 雷达相对于IMU的外参T（即雷达在IMU坐标系中的坐标）
540 |     nh.param<vector<double>>("mapping/init_rot", init_rot, vector<double>()); // 雷达相对于IMU的外参R
541 | 
542 |     cout << "Lidar_type: " << p_pre->lidar_type << endl;
543 |     // 初始化path的header（包括时间戳和帧id），path用于保存odemetry的路径
544 |     path.header.stamp = ros::Time::now();
545 |     path.header.frame_id = "camera_init";
546 | 
547 |     /*** ROS subscribe initialization ***/
548 |     ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
549 |     ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
550 |     ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100000);
551 |     ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_body", 100000);
552 |     ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>("/cloud_effected", 100000);
553 |     pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/Laser_map", 100000);
554 |     ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/Odometry", 100000);
555 |     ros::Publisher pubPath = nh.advertise<nav_msgs::Path>("/path", 100000);
556 | 
557 |     downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
558 |     downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
559 | 
560 |     shared_ptr<ImuProcess> p_imu1(new ImuProcess());
561 |     Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT);
562 |     Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR);
563 |     p_imu1->set_param(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU, V3D(gyr_cov, gyr_cov, gyr_cov), V3D(acc_cov, acc_cov, acc_cov),
564 |                       V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov), V3D(b_acc_cov, b_acc_cov, b_acc_cov));
565 | 
566 |     signal(SIGINT, SigHandle);
567 |     ros::Rate rate(5000);
568 | 
569 |     init_ikdtree(); //读取点云文件 初始化ikdtree
570 | 
571 |     state_point = kf.get_x();
572 |     state_point.pos = Eigen::Vector3d(init_pos[0], init_pos[1], init_pos[2]);
573 |     Eigen::Quaterniond q(init_rot[3], init_rot[0], init_rot[1], init_rot[2]);
574 |     Sophus::SO3 SO3_q(q);
575 |     state_point.rot = SO3_q;
576 |     kf.change_x(state_point);
577 | 
578 |     while (ros::ok())
579 |     {
580 |         if (flg_exit)
581 |             break;
582 |         ros::spinOnce();
583 | 
584 |         if (sync_packages(Measures)) //把一次的IMU和LIDAR数据打包到Measures
585 |         {
586 |             double t00 = omp_get_wtime();
587 | 
588 |             if (flg_first_scan)
589 |             {
590 |                 first_lidar_time = Measures.lidar_beg_time;
591 |                 p_imu1->first_lidar_time = first_lidar_time;
592 | 
593 |                 string all_points_dir(string(string(ROOT_DIR) + "PCD/") + "GlobalMap.pcd");
594 |                 if (pcl::io::loadPCDFile<PointType>(all_points_dir, *cloud) == -1)
595 |                 {
596 |                     PCL_ERROR("Read file fail!\n");
597 |                 }
598 | 
599 |                 sensor_msgs::PointCloud2 laserCloudMap;
600 |                 pcl::toROSMsg(*cloud, laserCloudMap);
601 |                 laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);
602 |                 laserCloudMap.header.frame_id = "camera_init";
603 |                 pubLaserCloudMap.publish(laserCloudMap);
604 | 
605 |                 flg_first_scan = false;
606 |                 continue;
607 |             }
608 | 
609 |             p_imu1->Process(Measures, kf, feats_undistort);
610 | 
611 |             //如果feats_undistort为空 ROS_WARN
612 |             if (feats_undistort->empty() || (feats_undistort == NULL))
613 |             {
614 |                 ROS_WARN("No point, skip this scan!\n");
615 |                 continue;
616 |             }
617 | 
618 |             state_point = kf.get_x();
619 |             pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I;
620 | 
621 |             flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true;
622 | 
623 |             lasermap_fov_segment(); //更新localmap边界，然后降采样当前帧点云
624 | 
625 |             //点云下采样
626 |             downSizeFilterSurf.setInputCloud(feats_undistort);
627 |             downSizeFilterSurf.filter(*feats_down_body);
628 |             feats_down_size = feats_down_body->points.size();
629 | 
630 |             // std::cout << "feats_down_size :" << feats_down_size << std::endl;
631 |             if (feats_down_size < 5)
632 |             {
633 |                 ROS_WARN("No point, skip this scan!\n");
634 |                 continue;
635 |             }
636 | 
637 |             if (0) // If you need to see map point, change to "if(1)"
638 |             {
639 |                 PointVector().swap(ikdtree.PCL_Storage);
640 |                 ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
641 |                 featsFromMap->clear();
642 |                 featsFromMap->points = ikdtree.PCL_Storage;
643 |                 std::cout << "ikdtree size: " << featsFromMap->points.size() << std::endl;
644 |             }
645 | 
646 |             /*** iterated state estimation ***/
647 |             Nearest_Points.resize(feats_down_size); //存储近邻点的vector
648 |             int a;
649 |             kf.update_iterated_dyn_share_modified(LASER_POINT_COV, feats_down_body, ikdtree, Nearest_Points, NUM_MAX_ITERATIONS, extrinsic_est_en, a);
650 | 
651 |             state_point = kf.get_x();
652 |             pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I;
653 | 
654 |             /******* Publish odometry *******/
655 |             publish_odometry(pubOdomAftMapped);
656 | 
657 |             /*** add the feature points to map kdtree ***/
658 |             feats_down_world->resize(feats_down_size);
659 |             // map_incremental();
660 | 
661 |             /******* Publish points *******/
662 |             if (path_en)
663 |                 publish_path(pubPath);
664 |             if (scan_pub_en || pcd_save_en)
665 |                 publish_frame_world(pubLaserCloudFull);
666 |             if (scan_pub_en && scan_body_pub_en)
667 |                 publish_frame_body(pubLaserCloudFull_body);
668 | 
669 |             double t11 = omp_get_wtime();
670 |             std::cout << "feats_down_size: " << feats_down_size << "  Whole mapping time(ms):  " << (t11 - t00) * 1000 << std::endl
671 |                       << std::endl;
672 |         }
673 | 
674 |         rate.sleep();
675 |     }
676 | 
677 |     return 0;
678 | }
679 | 


--------------------------------------------------------------------------------
/src/preprocess.cpp:
--------------------------------------------------------------------------------
   1 | #include "preprocess.h"
   2 | 
   3 | #define RETURN0 0x00
   4 | #define RETURN0AND1 0x10
   5 | 
   6 | Preprocess::Preprocess()
   7 |     : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
   8 | {
   9 |   inf_bound = 10;
  10 |   N_SCANS = 6;
  11 |   SCAN_RATE = 10;
  12 |   group_size = 8;
  13 |   disA = 0.01;
  14 |   disA = 0.1; // B?
  15 |   p2l_ratio = 225;
  16 |   limit_maxmid = 6.25;
  17 |   limit_midmin = 6.25;
  18 |   limit_maxmin = 3.24;
  19 |   jump_up_limit = 170.0;
  20 |   jump_down_limit = 8.0;
  21 |   cos160 = 160.0;
  22 |   edgea = 2;
  23 |   edgeb = 0.1;
  24 |   smallp_intersect = 172.5;
  25 |   smallp_ratio = 1.2;
  26 |   given_offset_time = false;
  27 | 
  28 |   jump_up_limit = cos(jump_up_limit / 180 * M_PI);
  29 |   jump_down_limit = cos(jump_down_limit / 180 * M_PI);
  30 |   cos160 = cos(cos160 / 180 * M_PI);
  31 |   smallp_intersect = cos(smallp_intersect / 180 * M_PI);
  32 | }
  33 | 
  34 | Preprocess::~Preprocess() {}
  35 | 
  36 | void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
  37 | {
  38 |   feature_enabled = feat_en;
  39 |   lidar_type = lid_type;
  40 |   blind = bld;
  41 |   point_filter_num = pfilt_num;
  42 | }
  43 | 
  44 | void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
  45 | {
  46 |   avia_handler(msg);
  47 |   *pcl_out = pl_surf;
  48 | }
  49 | 
  50 | void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
  51 | {
  52 |   switch (time_unit)
  53 |   {
  54 |   case SEC:
  55 |     time_unit_scale = 1.e3f;
  56 |     break;
  57 |   case MS:
  58 |     time_unit_scale = 1.f;
  59 |     break;
  60 |   case US:
  61 |     time_unit_scale = 1.e-3f;
  62 |     break;
  63 |   case NS:
  64 |     time_unit_scale = 1.e-6f;
  65 |     break;
  66 |   default:
  67 |     time_unit_scale = 1.f;
  68 |     break;
  69 |   }
  70 | 
  71 |   switch (lidar_type)
  72 |   {
  73 |   case OUST64:
  74 |     oust64_handler(msg);
  75 |     break;
  76 | 
  77 |   case VELO16:
  78 |     velodyne_handler(msg);
  79 |     break;
  80 | 
  81 |   case RS32:
  82 |     rs_handler(msg);
  83 |     break;
  84 | 
  85 |   default:
  86 |     printf("Error LiDAR Type");
  87 |     break;
  88 |   }
  89 |   *pcl_out = pl_surf;
  90 | }
  91 | 
  92 | void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
  93 | {
  94 |   pl_surf.clear();
  95 |   pl_corn.clear();
  96 |   pl_full.clear();
  97 |   double t1 = omp_get_wtime();
  98 |   int plsize = msg->point_num;
  99 |   // cout<<"plsie: "<<plsize<<endl;
 100 | 
 101 |   pl_corn.reserve(plsize);
 102 |   pl_surf.reserve(plsize);
 103 |   pl_full.resize(plsize);
 104 | 
 105 |   for (int i = 0; i < N_SCANS; i++)
 106 |   {
 107 |     pl_buff[i].clear();
 108 |     pl_buff[i].reserve(plsize);
 109 |   }
 110 |   uint valid_num = 0;
 111 | 
 112 |   if (feature_enabled)
 113 |   {
 114 |     for (uint i = 1; i < plsize; i++)
 115 |     {
 116 |       if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
 117 |       {
 118 |         pl_full[i].x = msg->points[i].x;
 119 |         pl_full[i].y = msg->points[i].y;
 120 |         pl_full[i].z = msg->points[i].z;
 121 |         pl_full[i].intensity = msg->points[i].reflectivity;
 122 |         pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points
 123 | 
 124 |         bool is_new = false;
 125 |         if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7))
 126 |         {
 127 |           pl_buff[msg->points[i].line].push_back(pl_full[i]);
 128 |         }
 129 |       }
 130 |     }
 131 |     static int count = 0;
 132 |     static double time = 0.0;
 133 |     count++;
 134 |     double t0 = omp_get_wtime();
 135 |     for (int j = 0; j < N_SCANS; j++)
 136 |     {
 137 |       if (pl_buff[j].size() <= 5)
 138 |         continue;
 139 |       pcl::PointCloud<PointType> &pl = pl_buff[j];
 140 |       plsize = pl.size();
 141 |       vector<orgtype> &types = typess[j];
 142 |       types.clear();
 143 |       types.resize(plsize);
 144 |       plsize--;
 145 |       for (uint i = 0; i < plsize; i++)
 146 |       {
 147 |         types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
 148 |         vx = pl[i].x - pl[i + 1].x;
 149 |         vy = pl[i].y - pl[i + 1].y;
 150 |         vz = pl[i].z - pl[i + 1].z;
 151 |         types[i].dista = sqrt(vx * vx + vy * vy + vz * vz);
 152 |       }
 153 |       types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y);
 154 |       give_feature(pl, types);
 155 |       // pl_surf += pl;
 156 |     }
 157 |     time += omp_get_wtime() - t0;
 158 |     printf("Feature extraction time: %lf \n", time / count);
 159 |   }
 160 |   else
 161 |   {
 162 |     for (uint i = 1; i < plsize; i++)
 163 |     {
 164 |       if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
 165 |       {
 166 |         valid_num++;
 167 |         if (valid_num % point_filter_num == 0)
 168 |         {
 169 |           pl_full[i].x = msg->points[i].x;
 170 |           pl_full[i].y = msg->points[i].y;
 171 |           pl_full[i].z = msg->points[i].z;
 172 |           pl_full[i].intensity = msg->points[i].reflectivity;
 173 |           pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms
 174 |           // std::cout << "pl_full[i].curvature: " << pl_full[i].curvature << std::endl;
 175 | 
 176 |           if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7) && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind)))
 177 |           {
 178 |             pl_surf.push_back(pl_full[i]);
 179 |           }
 180 |         }
 181 |       }
 182 |     }
 183 |   }
 184 | }
 185 | 
 186 | void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
 187 | {
 188 |   pl_surf.clear();
 189 |   pl_corn.clear();
 190 |   pl_full.clear();
 191 |   pcl::PointCloud<ouster_ros::Point> pl_orig;
 192 |   pcl::fromROSMsg(*msg, pl_orig);
 193 |   int plsize = pl_orig.size();
 194 |   pl_corn.reserve(plsize);
 195 |   pl_surf.reserve(plsize);
 196 |   if (feature_enabled)
 197 |   {
 198 |     for (int i = 0; i < N_SCANS; i++)
 199 |     {
 200 |       pl_buff[i].clear();
 201 |       pl_buff[i].reserve(plsize);
 202 |     }
 203 | 
 204 |     for (uint i = 0; i < plsize; i++)
 205 |     {
 206 |       double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
 207 |       if (range < (blind * blind))
 208 |         continue;
 209 |       Eigen::Vector3d pt_vec;
 210 |       PointType added_pt;
 211 |       added_pt.x = pl_orig.points[i].x;
 212 |       added_pt.y = pl_orig.points[i].y;
 213 |       added_pt.z = pl_orig.points[i].z;
 214 |       added_pt.intensity = pl_orig.points[i].intensity;
 215 |       added_pt.normal_x = 0;
 216 |       added_pt.normal_y = 0;
 217 |       added_pt.normal_z = 0;
 218 |       double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
 219 |       if (yaw_angle >= 180.0)
 220 |         yaw_angle -= 360.0;
 221 |       if (yaw_angle <= -180.0)
 222 |         yaw_angle += 360.0;
 223 | 
 224 |       added_pt.curvature = pl_orig.points[i].t * time_unit_scale;
 225 |       if (pl_orig.points[i].ring < N_SCANS)
 226 |       {
 227 |         pl_buff[pl_orig.points[i].ring].push_back(added_pt);
 228 |       }
 229 |     }
 230 | 
 231 |     for (int j = 0; j < N_SCANS; j++)
 232 |     {
 233 |       PointCloudXYZI &pl = pl_buff[j];
 234 |       int linesize = pl.size();
 235 |       vector<orgtype> &types = typess[j];
 236 |       types.clear();
 237 |       types.resize(linesize);
 238 |       linesize--;
 239 |       for (uint i = 0; i < linesize; i++)
 240 |       {
 241 |         types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
 242 |         vx = pl[i].x - pl[i + 1].x;
 243 |         vy = pl[i].y - pl[i + 1].y;
 244 |         vz = pl[i].z - pl[i + 1].z;
 245 |         types[i].dista = vx * vx + vy * vy + vz * vz;
 246 |       }
 247 |       types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
 248 |       give_feature(pl, types);
 249 |     }
 250 |   }
 251 |   else
 252 |   {
 253 |     double time_stamp = msg->header.stamp.toSec();
 254 |     // cout << "===================================" << endl;
 255 |     // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
 256 |     for (int i = 0; i < pl_orig.points.size(); i++)
 257 |     {
 258 |       if (i % point_filter_num != 0)
 259 |         continue;
 260 | 
 261 |       double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
 262 | 
 263 |       if (range < (blind * blind))
 264 |         continue;
 265 | 
 266 |       Eigen::Vector3d pt_vec;
 267 |       PointType added_pt;
 268 |       added_pt.x = pl_orig.points[i].x;
 269 |       added_pt.y = pl_orig.points[i].y;
 270 |       added_pt.z = pl_orig.points[i].z;
 271 |       added_pt.intensity = pl_orig.points[i].intensity;
 272 |       added_pt.normal_x = 0;
 273 |       added_pt.normal_y = 0;
 274 |       added_pt.normal_z = 0;
 275 |       added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms
 276 | 
 277 |       pl_surf.points.push_back(added_pt);
 278 |     }
 279 |   }
 280 |   // pub_func(pl_surf, pub_full, msg->header.stamp);
 281 |   // pub_func(pl_surf, pub_corn, msg->header.stamp);
 282 | }
 283 | 
 284 | void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
 285 | {
 286 |   pl_surf.clear();
 287 |   pl_corn.clear();
 288 |   pl_full.clear();
 289 | 
 290 |   pcl::PointCloud<velodyne_ros::Point> pl_orig;
 291 |   pcl::fromROSMsg(*msg, pl_orig);
 292 |   int plsize = pl_orig.points.size();
 293 |   if (plsize == 0)
 294 |     return;
 295 |   pl_surf.reserve(plsize);
 296 | 
 297 |   /*** These variables only works when no point timestamps given ***/
 298 |   double omega_l = 0.361 * SCAN_RATE; // scan angular velocity
 299 |   std::vector<bool> is_first(N_SCANS, true);
 300 |   std::vector<double> yaw_fp(N_SCANS, 0.0);   // yaw of first scan point
 301 |   std::vector<float> yaw_last(N_SCANS, 0.0);  // yaw of last scan point
 302 |   std::vector<float> time_last(N_SCANS, 0.0); // last offset time
 303 |   /*****************************************************************/
 304 | 
 305 |   if (pl_orig.points[plsize - 1].time > 0)
 306 |   {
 307 |     given_offset_time = true;
 308 |   }
 309 |   else
 310 |   {
 311 |     given_offset_time = false;
 312 |     double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
 313 |     double yaw_end = yaw_first;
 314 |     int layer_first = pl_orig.points[0].ring;
 315 |     for (uint i = plsize - 1; i > 0; i--)
 316 |     {
 317 |       if (pl_orig.points[i].ring == layer_first)
 318 |       {
 319 |         yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
 320 |         break;
 321 |       }
 322 |     }
 323 |   }
 324 | 
 325 |   if (feature_enabled)
 326 |   {
 327 |     for (int i = 0; i < N_SCANS; i++)
 328 |     {
 329 |       pl_buff[i].clear();
 330 |       pl_buff[i].reserve(plsize);
 331 |     }
 332 | 
 333 |     for (int i = 0; i < plsize; i++)
 334 |     {
 335 |       PointType added_pt;
 336 |       added_pt.normal_x = 0;
 337 |       added_pt.normal_y = 0;
 338 |       added_pt.normal_z = 0;
 339 |       int layer = pl_orig.points[i].ring;
 340 |       if (layer >= N_SCANS)
 341 |         continue;
 342 |       added_pt.x = pl_orig.points[i].x;
 343 |       added_pt.y = pl_orig.points[i].y;
 344 |       added_pt.z = pl_orig.points[i].z;
 345 |       added_pt.intensity = pl_orig.points[i].intensity;
 346 |       added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // units: ms
 347 | 
 348 |       if (!given_offset_time)
 349 |       {
 350 |         double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
 351 |         if (is_first[layer])
 352 |         {
 353 |           // printf("layer: %d; is first: %d", layer, is_first[layer]);
 354 |           yaw_fp[layer] = yaw_angle;
 355 |           is_first[layer] = false;
 356 |           added_pt.curvature = 0.0;
 357 |           yaw_last[layer] = yaw_angle;
 358 |           time_last[layer] = added_pt.curvature;
 359 |           continue;
 360 |         }
 361 | 
 362 |         if (yaw_angle <= yaw_fp[layer])
 363 |         {
 364 |           added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
 365 |         }
 366 |         else
 367 |         {
 368 |           added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
 369 |         }
 370 | 
 371 |         if (added_pt.curvature < time_last[layer])
 372 |           added_pt.curvature += 360.0 / omega_l;
 373 | 
 374 |         yaw_last[layer] = yaw_angle;
 375 |         time_last[layer] = added_pt.curvature;
 376 |       }
 377 | 
 378 |       pl_buff[layer].points.push_back(added_pt);
 379 |     }
 380 | 
 381 |     for (int j = 0; j < N_SCANS; j++)
 382 |     {
 383 |       PointCloudXYZI &pl = pl_buff[j];
 384 |       int linesize = pl.size();
 385 |       if (linesize < 2)
 386 |         continue;
 387 |       vector<orgtype> &types = typess[j];
 388 |       types.clear();
 389 |       types.resize(linesize);
 390 |       linesize--;
 391 |       for (uint i = 0; i < linesize; i++)
 392 |       {
 393 |         types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
 394 |         vx = pl[i].x - pl[i + 1].x;
 395 |         vy = pl[i].y - pl[i + 1].y;
 396 |         vz = pl[i].z - pl[i + 1].z;
 397 |         types[i].dista = vx * vx + vy * vy + vz * vz;
 398 |       }
 399 |       types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
 400 |       give_feature(pl, types);
 401 |     }
 402 |   }
 403 |   else
 404 |   {
 405 |     for (int i = 0; i < plsize; i++)
 406 |     {
 407 |       PointType added_pt;
 408 |       // cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
 409 | 
 410 |       added_pt.normal_x = 0;
 411 |       added_pt.normal_y = 0;
 412 |       added_pt.normal_z = 0;
 413 |       added_pt.x = pl_orig.points[i].x;
 414 |       added_pt.y = pl_orig.points[i].y;
 415 |       added_pt.z = pl_orig.points[i].z;
 416 |       added_pt.intensity = pl_orig.points[i].intensity;
 417 |       added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // curvature unit: ms // cout<<added_pt.curvature<<endl;
 418 |       // std::cout << "added_pt.curvature:" << added_pt.curvature << std::endl;
 419 | 
 420 | 
 421 |       if (!given_offset_time)
 422 |       {
 423 |         int layer = pl_orig.points[i].ring;
 424 |         double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
 425 | 
 426 |         if (is_first[layer])
 427 |         {
 428 |           // printf("layer: %d; is first: %d", layer, is_first[layer]);
 429 |           yaw_fp[layer] = yaw_angle;
 430 |           is_first[layer] = false;
 431 |           added_pt.curvature = 0.0;
 432 |           yaw_last[layer] = yaw_angle;
 433 |           time_last[layer] = added_pt.curvature;
 434 |           continue;
 435 |         }
 436 | 
 437 |         // compute offset time
 438 |         if (yaw_angle <= yaw_fp[layer])
 439 |         {
 440 |           added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
 441 |         }
 442 |         else
 443 |         {
 444 |           added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
 445 |         }
 446 | 
 447 |         if (added_pt.curvature < time_last[layer])
 448 |           added_pt.curvature += 360.0 / omega_l;
 449 | 
 450 |         yaw_last[layer] = yaw_angle;
 451 |         time_last[layer] = added_pt.curvature;
 452 |       }
 453 | 
 454 |       if (i % point_filter_num == 0)
 455 |       {
 456 |         if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
 457 |         {
 458 |           pl_surf.points.push_back(added_pt);
 459 |         }
 460 |       }
 461 |     }
 462 |   }
 463 | }
 464 | 
 465 | void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
 466 | {
 467 |   int plsize = pl.size();
 468 |   int plsize2;
 469 |   if (plsize == 0)
 470 |   {
 471 |     printf("something wrong\n");
 472 |     return;
 473 |   }
 474 |   uint head = 0;
 475 | 
 476 |   while (types[head].range < blind)
 477 |   {
 478 |     head++;
 479 |   }
 480 | 
 481 |   // Surf
 482 |   plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
 483 | 
 484 |   Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
 485 |   Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
 486 | 
 487 |   uint i_nex = 0, i2;
 488 |   uint last_i = 0;
 489 |   uint last_i_nex = 0;
 490 |   int last_state = 0;
 491 |   int plane_type;
 492 | 
 493 |   for (uint i = head; i < plsize2; i++)
 494 |   {
 495 |     if (types[i].range < blind)
 496 |     {
 497 |       continue;
 498 |     }
 499 | 
 500 |     i2 = i;
 501 | 
 502 |     plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
 503 | 
 504 |     if (plane_type == 1)
 505 |     {
 506 |       for (uint j = i; j <= i_nex; j++)
 507 |       {
 508 |         if (j != i && j != i_nex)
 509 |         {
 510 |           types[j].ftype = Real_Plane;
 511 |         }
 512 |         else
 513 |         {
 514 |           types[j].ftype = Poss_Plane;
 515 |         }
 516 |       }
 517 | 
 518 |       // if(last_state==1 && fabs(last_direct.sum())>0.5)
 519 |       if (last_state == 1 && last_direct.norm() > 0.1)
 520 |       {
 521 |         double mod = last_direct.transpose() * curr_direct;
 522 |         if (mod > -0.707 && mod < 0.707)
 523 |         {
 524 |           types[i].ftype = Edge_Plane;
 525 |         }
 526 |         else
 527 |         {
 528 |           types[i].ftype = Real_Plane;
 529 |         }
 530 |       }
 531 | 
 532 |       i = i_nex - 1;
 533 |       last_state = 1;
 534 |     }
 535 |     else // if(plane_type == 2)
 536 |     {
 537 |       i = i_nex;
 538 |       last_state = 0;
 539 |     }
 540 |     // else if(plane_type == 0)
 541 |     // {
 542 |     //   if(last_state == 1)
 543 |     //   {
 544 |     //     uint i_nex_tem;
 545 |     //     uint j;
 546 |     //     for(j=last_i+1; j<=last_i_nex; j++)
 547 |     //     {
 548 |     //       uint i_nex_tem2 = i_nex_tem;
 549 |     //       Eigen::Vector3d curr_direct2;
 550 | 
 551 |     //       uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
 552 | 
 553 |     //       if(ttem != 1)
 554 |     //       {
 555 |     //         i_nex_tem = i_nex_tem2;
 556 |     //         break;
 557 |     //       }
 558 |     //       curr_direct = curr_direct2;
 559 |     //     }
 560 | 
 561 |     //     if(j == last_i+1)
 562 |     //     {
 563 |     //       last_state = 0;
 564 |     //     }
 565 |     //     else
 566 |     //     {
 567 |     //       for(uint k=last_i_nex; k<=i_nex_tem; k++)
 568 |     //       {
 569 |     //         if(k != i_nex_tem)
 570 |     //         {
 571 |     //           types[k].ftype = Real_Plane;
 572 |     //         }
 573 |     //         else
 574 |     //         {
 575 |     //           types[k].ftype = Poss_Plane;
 576 |     //         }
 577 |     //       }
 578 |     //       i = i_nex_tem-1;
 579 |     //       i_nex = i_nex_tem;
 580 |     //       i2 = j-1;
 581 |     //       last_state = 1;
 582 |     //     }
 583 | 
 584 |     //   }
 585 |     // }
 586 | 
 587 |     last_i = i2;
 588 |     last_i_nex = i_nex;
 589 |     last_direct = curr_direct;
 590 |   }
 591 | 
 592 |   plsize2 = plsize > 3 ? plsize - 3 : 0;
 593 |   for (uint i = head + 3; i < plsize2; i++)
 594 |   {
 595 |     if (types[i].range < blind || types[i].ftype >= Real_Plane)
 596 |     {
 597 |       continue;
 598 |     }
 599 | 
 600 |     if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16)
 601 |     {
 602 |       continue;
 603 |     }
 604 | 
 605 |     Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
 606 |     Eigen::Vector3d vecs[2];
 607 | 
 608 |     for (int j = 0; j < 2; j++)
 609 |     {
 610 |       int m = -1;
 611 |       if (j == 1)
 612 |       {
 613 |         m = 1;
 614 |       }
 615 | 
 616 |       if (types[i + m].range < blind)
 617 |       {
 618 |         if (types[i].range > inf_bound)
 619 |         {
 620 |           types[i].edj[j] = Nr_inf;
 621 |         }
 622 |         else
 623 |         {
 624 |           types[i].edj[j] = Nr_blind;
 625 |         }
 626 |         continue;
 627 |       }
 628 | 
 629 |       vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);
 630 |       vecs[j] = vecs[j] - vec_a;
 631 | 
 632 |       types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
 633 |       if (types[i].angle[j] < jump_up_limit)
 634 |       {
 635 |         types[i].edj[j] = Nr_180;
 636 |       }
 637 |       else if (types[i].angle[j] > jump_down_limit)
 638 |       {
 639 |         types[i].edj[j] = Nr_zero;
 640 |       }
 641 |     }
 642 | 
 643 |     types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
 644 |     if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista)
 645 |     {
 646 |       if (types[i].intersect > cos160)
 647 |       {
 648 |         if (edge_jump_judge(pl, types, i, Prev))
 649 |         {
 650 |           types[i].ftype = Edge_Jump;
 651 |         }
 652 |       }
 653 |     }
 654 |     else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista)
 655 |     {
 656 |       if (types[i].intersect > cos160)
 657 |       {
 658 |         if (edge_jump_judge(pl, types, i, Next))
 659 |         {
 660 |           types[i].ftype = Edge_Jump;
 661 |         }
 662 |       }
 663 |     }
 664 |     else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf)
 665 |     {
 666 |       if (edge_jump_judge(pl, types, i, Prev))
 667 |       {
 668 |         types[i].ftype = Edge_Jump;
 669 |       }
 670 |     }
 671 |     else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor)
 672 |     {
 673 |       if (edge_jump_judge(pl, types, i, Next))
 674 |       {
 675 |         types[i].ftype = Edge_Jump;
 676 |       }
 677 |     }
 678 |     else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor)
 679 |     {
 680 |       if (types[i].ftype == Nor)
 681 |       {
 682 |         types[i].ftype = Wire;
 683 |       }
 684 |     }
 685 |   }
 686 | 
 687 |   plsize2 = plsize - 1;
 688 |   double ratio;
 689 |   for (uint i = head + 1; i < plsize2; i++)
 690 |   {
 691 |     if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind)
 692 |     {
 693 |       continue;
 694 |     }
 695 | 
 696 |     if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8)
 697 |     {
 698 |       continue;
 699 |     }
 700 | 
 701 |     if (types[i].ftype == Nor)
 702 |     {
 703 |       if (types[i - 1].dista > types[i].dista)
 704 |       {
 705 |         ratio = types[i - 1].dista / types[i].dista;
 706 |       }
 707 |       else
 708 |       {
 709 |         ratio = types[i].dista / types[i - 1].dista;
 710 |       }
 711 | 
 712 |       if (types[i].intersect < smallp_intersect && ratio < smallp_ratio)
 713 |       {
 714 |         if (types[i - 1].ftype == Nor)
 715 |         {
 716 |           types[i - 1].ftype = Real_Plane;
 717 |         }
 718 |         if (types[i + 1].ftype == Nor)
 719 |         {
 720 |           types[i + 1].ftype = Real_Plane;
 721 |         }
 722 |         types[i].ftype = Real_Plane;
 723 |       }
 724 |     }
 725 |   }
 726 | 
 727 |   int last_surface = -1;
 728 |   for (uint j = head; j < plsize; j++)
 729 |   {
 730 |     if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane)
 731 |     {
 732 |       if (last_surface == -1)
 733 |       {
 734 |         last_surface = j;
 735 |       }
 736 | 
 737 |       if (j == uint(last_surface + point_filter_num - 1))
 738 |       {
 739 |         PointType ap;
 740 |         ap.x = pl[j].x;
 741 |         ap.y = pl[j].y;
 742 |         ap.z = pl[j].z;
 743 |         ap.intensity = pl[j].intensity;
 744 |         ap.curvature = pl[j].curvature;
 745 |         pl_surf.push_back(ap);
 746 | 
 747 |         last_surface = -1;
 748 |       }
 749 |     }
 750 |     else
 751 |     {
 752 |       if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane)
 753 |       {
 754 |         pl_corn.push_back(pl[j]);
 755 |       }
 756 |       if (last_surface != -1)
 757 |       {
 758 |         PointType ap;
 759 |         for (uint k = last_surface; k < j; k++)
 760 |         {
 761 |           ap.x += pl[k].x;
 762 |           ap.y += pl[k].y;
 763 |           ap.z += pl[k].z;
 764 |           ap.intensity += pl[k].intensity;
 765 |           ap.curvature += pl[k].curvature;
 766 |         }
 767 |         ap.x /= (j - last_surface);
 768 |         ap.y /= (j - last_surface);
 769 |         ap.z /= (j - last_surface);
 770 |         ap.intensity /= (j - last_surface);
 771 |         ap.curvature /= (j - last_surface);
 772 |         pl_surf.push_back(ap);
 773 |       }
 774 |       last_surface = -1;
 775 |     }
 776 |   }
 777 | }
 778 | 
 779 | void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)
 780 | {
 781 |   pl.height = 1;
 782 |   pl.width = pl.size();
 783 |   sensor_msgs::PointCloud2 output;
 784 |   pcl::toROSMsg(pl, output);
 785 |   output.header.frame_id = "livox";
 786 |   output.header.stamp = ct;
 787 | }
 788 | 
 789 | int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct)
 790 | {
 791 |   double group_dis = disA * types[i_cur].range + disB;
 792 |   group_dis = group_dis * group_dis;
 793 |   // i_nex = i_cur;
 794 | 
 795 |   double two_dis;
 796 |   vector<double> disarr;
 797 |   disarr.reserve(20);
 798 | 
 799 |   for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++)
 800 |   {
 801 |     if (types[i_nex].range < blind)
 802 |     {
 803 |       curr_direct.setZero();
 804 |       return 2;
 805 |     }
 806 |     disarr.push_back(types[i_nex].dista);
 807 |   }
 808 | 
 809 |   for (;;)
 810 |   {
 811 |     if ((i_cur >= pl.size()) || (i_nex >= pl.size()))
 812 |       break;
 813 | 
 814 |     if (types[i_nex].range < blind)
 815 |     {
 816 |       curr_direct.setZero();
 817 |       return 2;
 818 |     }
 819 |     vx = pl[i_nex].x - pl[i_cur].x;
 820 |     vy = pl[i_nex].y - pl[i_cur].y;
 821 |     vz = pl[i_nex].z - pl[i_cur].z;
 822 |     two_dis = vx * vx + vy * vy + vz * vz;
 823 |     if (two_dis >= group_dis)
 824 |     {
 825 |       break;
 826 |     }
 827 |     disarr.push_back(types[i_nex].dista);
 828 |     i_nex++;
 829 |   }
 830 | 
 831 |   double leng_wid = 0;
 832 |   double v1[3], v2[3];
 833 |   for (uint j = i_cur + 1; j < i_nex; j++)
 834 |   {
 835 |     if ((j >= pl.size()) || (i_cur >= pl.size()))
 836 |       break;
 837 |     v1[0] = pl[j].x - pl[i_cur].x;
 838 |     v1[1] = pl[j].y - pl[i_cur].y;
 839 |     v1[2] = pl[j].z - pl[i_cur].z;
 840 | 
 841 |     v2[0] = v1[1] * vz - vy * v1[2];
 842 |     v2[1] = v1[2] * vx - v1[0] * vz;
 843 |     v2[2] = v1[0] * vy - vx * v1[1];
 844 | 
 845 |     double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];
 846 |     if (lw > leng_wid)
 847 |     {
 848 |       leng_wid = lw;
 849 |     }
 850 |   }
 851 | 
 852 |   if ((two_dis * two_dis / leng_wid) < p2l_ratio)
 853 |   {
 854 |     curr_direct.setZero();
 855 |     return 0;
 856 |   }
 857 | 
 858 |   uint disarrsize = disarr.size();
 859 |   for (uint j = 0; j < disarrsize - 1; j++)
 860 |   {
 861 |     for (uint k = j + 1; k < disarrsize; k++)
 862 |     {
 863 |       if (disarr[j] < disarr[k])
 864 |       {
 865 |         leng_wid = disarr[j];
 866 |         disarr[j] = disarr[k];
 867 |         disarr[k] = leng_wid;
 868 |       }
 869 |     }
 870 |   }
 871 | 
 872 |   if (disarr[disarr.size() - 2] < 1e-16)
 873 |   {
 874 |     curr_direct.setZero();
 875 |     return 0;
 876 |   }
 877 | 
 878 |   if (lidar_type == AVIA)
 879 |   {
 880 |     double dismax_mid = disarr[0] / disarr[disarrsize / 2];
 881 |     double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2];
 882 | 
 883 |     if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin)
 884 |     {
 885 |       curr_direct.setZero();
 886 |       return 0;
 887 |     }
 888 |   }
 889 |   else
 890 |   {
 891 |     double dismax_min = disarr[0] / disarr[disarrsize - 2];
 892 |     if (dismax_min >= limit_maxmin)
 893 |     {
 894 |       curr_direct.setZero();
 895 |       return 0;
 896 |     }
 897 |   }
 898 | 
 899 |   curr_direct << vx, vy, vz;
 900 |   curr_direct.normalize();
 901 |   return 1;
 902 | }
 903 | 
 904 | bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir)
 905 | {
 906 |   if (nor_dir == 0)
 907 |   {
 908 |     if (types[i - 1].range < blind || types[i - 2].range < blind)
 909 |     {
 910 |       return false;
 911 |     }
 912 |   }
 913 |   else if (nor_dir == 1)
 914 |   {
 915 |     if (types[i + 1].range < blind || types[i + 2].range < blind)
 916 |     {
 917 |       return false;
 918 |     }
 919 |   }
 920 |   double d1 = types[i + nor_dir - 1].dista;
 921 |   double d2 = types[i + 3 * nor_dir - 2].dista;
 922 |   double d;
 923 | 
 924 |   if (d1 < d2)
 925 |   {
 926 |     d = d1;
 927 |     d1 = d2;
 928 |     d2 = d;
 929 |   }
 930 | 
 931 |   d1 = sqrt(d1);
 932 |   d2 = sqrt(d2);
 933 | 
 934 |   if (d1 > edgea * d2 || (d1 - d2) > edgeb)
 935 |   {
 936 |     return false;
 937 |   }
 938 | 
 939 |   return true;
 940 | }
 941 | 
 942 | void Preprocess::rs_handler(const sensor_msgs::PointCloud2_<allocator<void>>::ConstPtr &msg)
 943 | {
 944 |   pl_surf.clear();
 945 | 
 946 |   pcl::PointCloud<rslidar_ros::Point> pl_orig;
 947 |   pcl::fromROSMsg(*msg, pl_orig);
 948 |   int plsize = pl_orig.points.size();
 949 |   pl_surf.reserve(plsize);
 950 | 
 951 |   /*** These variables only works when no point timestamps given ***/
 952 |   double omega_l = 0.361 * SCAN_RATE; // scan angular velocity
 953 |   std::vector<bool> is_first(N_SCANS, true);
 954 |   std::vector<double> yaw_fp(N_SCANS, 0.0);   // yaw of first scan point
 955 |   std::vector<float> yaw_last(N_SCANS, 0.0);  // yaw of last scan point
 956 |   std::vector<float> time_last(N_SCANS, 0.0); // last offset time
 957 |   /*****************************************************************/
 958 | 
 959 |   if (pl_orig.points[plsize - 1].timestamp > 0) // todo check pl_orig.points[plsize - 1].time
 960 |   {
 961 |     given_offset_time = true;
 962 |     // std::cout << "given_offset_time = true " << std::endl;
 963 |   }
 964 |   else
 965 |   {
 966 |     given_offset_time = false;
 967 |     double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; // 记录第一个点(index 0)的yaw， to degree
 968 |     double yaw_end = yaw_first;
 969 |     int layer_first = pl_orig.points[0].ring; // 第一个点(index 0)的layer序号
 970 |     for (uint i = plsize - 1; i > 0; i--)     // 倒序遍历，找到与第一个点相同layer的最后一个点
 971 |     {
 972 |       if (pl_orig.points[i].ring == layer_first)
 973 |       {
 974 |         yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; // 与第一个点相同layer的最后一个点的yaw
 975 |         break;
 976 |       }
 977 |     }
 978 |   }
 979 | 
 980 |   for (int i = 0; i < plsize; i++)
 981 |   {
 982 |     PointType added_pt;
 983 | 
 984 |     added_pt.normal_x = 0;
 985 |     added_pt.normal_y = 0;
 986 |     added_pt.normal_z = 0;
 987 |     added_pt.x = pl_orig.points[i].x;
 988 |     added_pt.y = pl_orig.points[i].y;
 989 |     added_pt.z = pl_orig.points[i].z;
 990 |     added_pt.intensity = pl_orig.points[i].intensity;
 991 |     added_pt.curvature = (pl_orig.points[i].timestamp - pl_orig.points[0].timestamp) * 1000.0; // curvature unit: ms
 992 |     // std::cout << "added_pt.curvature:" << added_pt.curvature << std::endl;
 993 | 
 994 |     if (!given_offset_time)
 995 |     {
 996 |       int layer = pl_orig.points[i].ring;
 997 |       double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
 998 | 
 999 |       if (is_first[layer])
1000 |       {
1001 |         // printf("layer: %d; is first: %d", layer, is_first[layer]);
1002 |         yaw_fp[layer] = yaw_angle;
1003 |         is_first[layer] = false;
1004 |         added_pt.curvature = 0.0;
1005 |         yaw_last[layer] = yaw_angle;
1006 |         time_last[layer] = added_pt.curvature;
1007 |         continue;
1008 |       }
1009 | 
1010 |       // compute offset time
1011 |       if (yaw_angle <= yaw_fp[layer])
1012 |       {
1013 |         added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
1014 |       }
1015 |       else
1016 |       {
1017 |         added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
1018 |       }
1019 | 
1020 |       if (added_pt.curvature < time_last[layer])
1021 |         added_pt.curvature += 360.0 / omega_l;
1022 | 
1023 |       yaw_last[layer] = yaw_angle;
1024 |       time_last[layer] = added_pt.curvature;
1025 |     }
1026 | 
1027 |     if (i % point_filter_num == 0)
1028 |     {
1029 |       if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind) )
1030 |       {
1031 |         pl_surf.points.push_back(added_pt);
1032 |       }
1033 |     }
1034 |   }
1035 | }
1036 | 


--------------------------------------------------------------------------------
/src/laserMapping.cpp:
--------------------------------------------------------------------------------
  1 | #include <omp.h>
  2 | #include <mutex>
  3 | #include <math.h>
  4 | #include <thread>
  5 | #include <fstream>
  6 | #include <csignal>
  7 | #include <unistd.h>
  8 | #include <ros/ros.h>
  9 | #include <Eigen/Core>
 10 | #include <nav_msgs/Odometry.h>
 11 | #include <nav_msgs/Path.h>
 12 | #include <visualization_msgs/Marker.h>
 13 | #include <pcl_conversions/pcl_conversions.h>
 14 | #include <pcl/point_cloud.h>
 15 | #include <pcl/point_types.h>
 16 | #include <pcl/filters/voxel_grid.h>
 17 | #include <pcl/io/pcd_io.h>
 18 | #include <sensor_msgs/PointCloud2.h>
 19 | #include <tf/transform_datatypes.h>
 20 | #include <tf/transform_broadcaster.h>
 21 | #include <geometry_msgs/Vector3.h>
 22 | #include <livox_ros_driver/CustomMsg.h>
 23 | #include "preprocess.h"
 24 | #include <ikd-Tree/ikd_Tree.h>
 25 | 
 26 | #include "IMU_Processing.hpp"
 27 | 
 28 | #define INIT_TIME (0.1)
 29 | #define LASER_POINT_COV (0.001)
 30 | #define PUBFRAME_PERIOD (20)
 31 | 
 32 | /*** Time Log Variables ***/
 33 | int add_point_size = 0, kdtree_delete_counter = 0;
 34 | bool pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true, path_en = true;
 35 | int frame_count = 0;
 36 | double all_time = 0.0;
 37 | double all_update_time = 0.0;
 38 | string traj_log_file;
 39 | vector<int> iter_counts;
 40 | vector<double> update_time_vec;
 41 | /**************************/
 42 | 
 43 | float res_last[100000] = {0.0};
 44 | float DET_RANGE = 300.0f;
 45 | const float MOV_THRESHOLD = 1.5f;
 46 | double time_diff_lidar_to_imu = 0.0;
 47 | 
 48 | mutex mtx_buffer;
 49 | condition_variable sig_buffer;
 50 | 
 51 | string root_dir = ROOT_DIR;
 52 | string map_file_path, lid_topic, imu_topic;
 53 | 
 54 | double last_timestamp_lidar = 0, last_timestamp_imu = -1.0;
 55 | double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001;
 56 | double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0;
 57 | double cube_len = 0, lidar_end_time = 0, first_lidar_time = 0.0;
 58 | int scan_count = 0, publish_count = 0;
 59 | int feats_down_size = 0, NUM_MAX_ITERATIONS = 0, pcd_save_interval = -1, pcd_index = 0;
 60 | 
 61 | bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited;
 62 | bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
 63 | 
 64 | vector<BoxPointType> cub_needrm;
 65 | vector<PointVector> Nearest_Points;
 66 | vector<double> extrinT(3, 0.0);
 67 | vector<double> extrinR(9, 0.0);
 68 | deque<double> time_buffer;
 69 | deque<PointCloudXYZI::Ptr> lidar_buffer;
 70 | deque<sensor_msgs::Imu::ConstPtr> imu_buffer;
 71 | 
 72 | PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());
 73 | PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());
 74 | PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());  //畸变纠正后降采样的单帧点云，lidar系
 75 | PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，W系
 76 | 
 77 | pcl::VoxelGrid<PointType> downSizeFilterSurf;
 78 | pcl::VoxelGrid<PointType> downSizeFilterMap;
 79 | 
 80 | KD_TREE<PointType> ikdtree;
 81 | 
 82 | V3D Lidar_T_wrt_IMU(Zero3d);
 83 | M3D Lidar_R_wrt_IMU(Eye3d);
 84 | 
 85 | /*** EKF inputs and output ***/
 86 | MeasureGroup Measures;
 87 | 
 88 | esekfom::esekf kf;
 89 | 
 90 | state_ikfom state_point;
 91 | Eigen::Vector3d pos_lid; //估计的W系下的位置
 92 | 
 93 | nav_msgs::Path path;
 94 | nav_msgs::Odometry odomAftMapped;
 95 | geometry_msgs::PoseStamped msg_body_pose;
 96 | 
 97 | shared_ptr<Preprocess> p_pre(new Preprocess());
 98 | 
 99 | void SigHandle(int sig)
100 | {
101 |     flg_exit = true;
102 |     ROS_WARN("catch sig %d", sig);
103 |     sig_buffer.notify_all();
104 | }
105 | 
106 | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
107 | {
108 |     mtx_buffer.lock();
109 |     scan_count++;
110 |     double preprocess_start_time = omp_get_wtime();
111 |     if (msg->header.stamp.toSec() < last_timestamp_lidar)
112 |     {
113 |         ROS_ERROR("lidar loop back, clear buffer");
114 |         lidar_buffer.clear();
115 |     }
116 | 
117 |     PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
118 |     p_pre->process(msg, ptr);
119 |     lidar_buffer.push_back(ptr);
120 |     time_buffer.push_back(msg->header.stamp.toSec());
121 |     last_timestamp_lidar = msg->header.stamp.toSec();
122 |     mtx_buffer.unlock();
123 |     sig_buffer.notify_all();
124 | }
125 | 
126 | double timediff_lidar_wrt_imu = 0.0;
127 | bool timediff_set_flg = false;
128 | void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
129 | {
130 |     mtx_buffer.lock();
131 |     double preprocess_start_time = omp_get_wtime();
132 |     scan_count++;
133 |     if (msg->header.stamp.toSec() < last_timestamp_lidar)
134 |     {
135 |         ROS_ERROR("lidar loop back, clear buffer");
136 |         lidar_buffer.clear();
137 |     }
138 |     last_timestamp_lidar = msg->header.stamp.toSec();
139 | 
140 |     if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty())
141 |     {
142 |         printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n", last_timestamp_imu, last_timestamp_lidar);
143 |     }
144 | 
145 |     if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())
146 |     {
147 |         timediff_set_flg = true;
148 |         timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu;
149 |         printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu);
150 |     }
151 | 
152 |     PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
153 |     p_pre->process(msg, ptr);
154 |     lidar_buffer.push_back(ptr);
155 |     time_buffer.push_back(last_timestamp_lidar);
156 | 
157 |     mtx_buffer.unlock();
158 |     sig_buffer.notify_all();
159 | }
160 | 
161 | void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
162 | {
163 |     publish_count++;
164 |     // cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;
165 |     sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
166 | 
167 |     if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en)
168 |     {
169 |         msg->header.stamp =
170 |             ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec());
171 |     }
172 | 
173 |     msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_diff_lidar_to_imu);
174 | 
175 |     double timestamp = msg->header.stamp.toSec();
176 | 
177 |     mtx_buffer.lock();
178 | 
179 |     if (timestamp < last_timestamp_imu)
180 |     {
181 |         ROS_WARN("imu loop back, clear buffer");
182 |         imu_buffer.clear();
183 |     }
184 | 
185 |     last_timestamp_imu = timestamp;
186 | 
187 |     imu_buffer.push_back(msg);
188 |     mtx_buffer.unlock();
189 |     sig_buffer.notify_all();
190 | }
191 | 
192 | double lidar_mean_scantime = 0.0;
193 | int scan_num = 0;
194 | //把当前要处理的LIDAR和IMU数据打包到meas
195 | bool sync_packages(MeasureGroup &meas)
196 | {
197 |     if (lidar_buffer.empty() || imu_buffer.empty())
198 |     {
199 |         return false;
200 |     }
201 | 
202 |     /*** push a lidar scan ***/
203 |     if (!lidar_pushed)
204 |     {
205 |         meas.lidar = lidar_buffer.front();
206 |         meas.lidar_beg_time = time_buffer.front();
207 |         if (meas.lidar->points.size() <= 5) // time too little
208 |         {
209 |             lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
210 |             ROS_WARN("Too few input point cloud!\n");
211 |         }
212 |         else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime)
213 |         {
214 |             lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
215 |         }
216 |         else
217 |         {
218 |             scan_num++;
219 |             lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
220 |             lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;  //注意curvature中存储的是相对第一个点的时间
221 |         }
222 | 
223 |         meas.lidar_end_time = lidar_end_time;
224 | 
225 |         lidar_pushed = true;
226 |     }
227 | 
228 |     if (last_timestamp_imu < lidar_end_time)  //如果最新的imu时间戳都<雷达最终的时间，证明还没有收集足够的imu数据，break
229 |     {
230 |         return false;
231 |     }
232 | 
233 |     /*** push imu data, and pop from imu buffer ***/
234 |     double imu_time = imu_buffer.front()->header.stamp.toSec();
235 |     meas.imu.clear();
236 |     while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
237 |     {
238 |         imu_time = imu_buffer.front()->header.stamp.toSec();
239 |         if (imu_time > lidar_end_time)
240 |             break;
241 |         meas.imu.push_back(imu_buffer.front());
242 |         imu_buffer.pop_front();
243 |     }
244 | 
245 |     lidar_buffer.pop_front();
246 |     time_buffer.pop_front();
247 |     lidar_pushed = false;
248 |     return true;
249 | }
250 | 
251 | void pointBodyToWorld(PointType const *const pi, PointType *const po)
252 | {
253 |     V3D p_body(pi->x, pi->y, pi->z);
254 |     V3D p_global(state_point.rot.matrix() * (state_point.offset_R_L_I.matrix() * p_body + state_point.offset_T_L_I) + state_point.pos);
255 | 
256 |     po->x = p_global(0);
257 |     po->y = p_global(1);
258 |     po->z = p_global(2);
259 |     po->intensity = pi->intensity;
260 | }
261 | 
262 | template <typename T>
263 | void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
264 | {
265 |     V3D p_body(pi[0], pi[1], pi[2]);
266 |     V3D p_global(state_point.rot.matrix() * (state_point.offset_R_L_I.matrix() * p_body + state_point.offset_T_L_I) + state_point.pos);
267 | 
268 |     po[0] = p_global(0);
269 |     po[1] = p_global(1);
270 |     po[2] = p_global(2);
271 | }
272 | 
273 | BoxPointType LocalMap_Points;      // ikd-tree地图立方体的2个角点
274 | bool Localmap_Initialized = false; // 局部地图是否初始化
275 | void lasermap_fov_segment()
276 | {
277 |     cub_needrm.clear(); // 清空需要移除的区域
278 |     kdtree_delete_counter = 0;
279 | 
280 |     V3D pos_LiD = pos_lid; // W系下位置
281 |     //初始化局部地图范围，以pos_LiD为中心,长宽高均为cube_len
282 |     if (!Localmap_Initialized)
283 |     {
284 |         for (int i = 0; i < 3; i++)
285 |         {
286 |             LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
287 |             LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
288 |         }
289 |         Localmap_Initialized = true;
290 |         return;
291 |     }
292 | 
293 |     //各个方向上pos_LiD与局部地图边界的距离
294 |     float dist_to_map_edge[3][2];
295 |     bool need_move = false;
296 |     for (int i = 0; i < 3; i++)
297 |     {
298 |         dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
299 |         dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
300 |         // 与某个方向上的边界距离（1.5*300m）太小，标记需要移除need_move(FAST-LIO2论文Fig.3)
301 |         if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
302 |             need_move = true;
303 |     }
304 |     if (!need_move)
305 |         return; //如果不需要，直接返回，不更改局部地图
306 | 
307 |     BoxPointType New_LocalMap_Points, tmp_boxpoints;
308 |     New_LocalMap_Points = LocalMap_Points;
309 |     //需要移动的距离
310 |     float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1)));
311 |     for (int i = 0; i < 3; i++)
312 |     {
313 |         tmp_boxpoints = LocalMap_Points;
314 |         if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE)
315 |         {
316 |             New_LocalMap_Points.vertex_max[i] -= mov_dist;
317 |             New_LocalMap_Points.vertex_min[i] -= mov_dist;
318 |             tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
319 |             cub_needrm.push_back(tmp_boxpoints);
320 |         }
321 |         else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
322 |         {
323 |             New_LocalMap_Points.vertex_max[i] += mov_dist;
324 |             New_LocalMap_Points.vertex_min[i] += mov_dist;
325 |             tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
326 |             cub_needrm.push_back(tmp_boxpoints);
327 |         }
328 |     }
329 |     LocalMap_Points = New_LocalMap_Points;
330 | 
331 |     PointVector points_history;
332 |     ikdtree.acquire_removed_points(points_history);
333 | 
334 |     if (cub_needrm.size() > 0)
335 |         kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); //删除指定范围内的点
336 | }
337 | 
338 | void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
339 | {
340 |     V3D p_body_lidar(pi->x, pi->y, pi->z);
341 |     V3D p_body_imu(state_point.offset_R_L_I.matrix() * p_body_lidar + state_point.offset_T_L_I);
342 | 
343 |     po->x = p_body_imu(0);
344 |     po->y = p_body_imu(1);
345 |     po->z = p_body_imu(2);
346 |     po->intensity = pi->intensity;
347 | }
348 | 
349 | //根据最新估计位姿  增量添加点云到map
350 | void map_incremental()
351 | {
352 |     PointVector PointToAdd;
353 |     PointVector PointNoNeedDownsample;
354 |     PointToAdd.reserve(feats_down_size);
355 |     PointNoNeedDownsample.reserve(feats_down_size);
356 |     for (int i = 0; i < feats_down_size; i++)
357 |     {
358 |         //转换到世界坐标系
359 |         pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
360 | 
361 |         if (!Nearest_Points[i].empty() && flg_EKF_inited)
362 |         {
363 |             const PointVector &points_near = Nearest_Points[i];
364 |             bool need_add = true;
365 |             BoxPointType Box_of_Point;
366 |             PointType mid_point; //点所在体素的中心
367 |             mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min;
368 |             mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min;
369 |             mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min;
370 |             float dist = calc_dist(feats_down_world->points[i], mid_point);
371 |             if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min)
372 |             {
373 |                 PointNoNeedDownsample.push_back(feats_down_world->points[i]); //如果距离最近的点都在体素外，则该点不需要Downsample
374 |                 continue;
375 |             }
376 |             for (int j = 0; j < NUM_MATCH_POINTS; j++)
377 |             {
378 |                 if (points_near.size() < NUM_MATCH_POINTS)
379 |                     break;
380 |                 if (calc_dist(points_near[j], mid_point) < dist) //如果近邻点距离 < 当前点距离，不添加该点
381 |                 {
382 |                     need_add = false;
383 |                     break;
384 |                 }
385 |             }
386 |             if (need_add)
387 |                 PointToAdd.push_back(feats_down_world->points[i]);
388 |         }
389 |         else
390 |         {
391 |             PointToAdd.push_back(feats_down_world->points[i]);
392 |         }
393 |     }
394 | 
395 |     double st_time = omp_get_wtime();
396 |     add_point_size = ikdtree.Add_Points(PointToAdd, true);
397 |     ikdtree.Add_Points(PointNoNeedDownsample, false);
398 |     add_point_size = PointToAdd.size() + PointNoNeedDownsample.size();
399 | }
400 | 
401 | PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
402 | PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
403 | void publish_frame_world(const ros::Publisher &pubLaserCloudFull_)
404 | {
405 |     if (scan_pub_en)
406 |     {
407 |         PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
408 |         int size = laserCloudFullRes->points.size();
409 |         PointCloudXYZI::Ptr laserCloudWorld(
410 |             new PointCloudXYZI(size, 1));
411 | 
412 |         for (int i = 0; i < size; i++)
413 |         {
414 |             pointBodyToWorld(&laserCloudFullRes->points[i],
415 |                              &laserCloudWorld->points[i]);
416 |         }
417 | 
418 |         sensor_msgs::PointCloud2 laserCloudmsg;
419 |         pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
420 |         laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
421 |         laserCloudmsg.header.frame_id = "camera_init";
422 |         pubLaserCloudFull_.publish(laserCloudmsg);
423 |         publish_count -= PUBFRAME_PERIOD;
424 |     }
425 | 
426 |     /**************** save map ****************/
427 |     /* 1. make sure you have enough memories
428 |     /* 2. noted that pcd save will influence the real-time performences **/
429 |     if (pcd_save_en)
430 |     {
431 |         int size = feats_undistort->points.size();
432 |         PointCloudXYZI::Ptr laserCloudWorld(
433 |             new PointCloudXYZI(size, 1));
434 | 
435 |         for (int i = 0; i < size; i++)
436 |         {
437 |             pointBodyToWorld(&feats_undistort->points[i],
438 |                              &laserCloudWorld->points[i]);
439 |         }
440 | 
441 |         static int scan_wait_num = 0;
442 |         scan_wait_num++;
443 | 
444 |         if (scan_wait_num % 4 == 0)
445 |             *pcl_wait_save += *laserCloudWorld;
446 | 
447 |         if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval)
448 |         {
449 |             pcd_index++;
450 |             string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
451 |             pcl::PCDWriter pcd_writer;
452 |             cout << "current scan saved to /PCD/" << all_points_dir << endl;
453 |             pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
454 |             pcl_wait_save->clear();
455 |             scan_wait_num = 0;
456 |         }
457 |     }
458 | }
459 | 
460 | void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body)
461 | {
462 |     int size = feats_undistort->points.size();
463 |     PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
464 | 
465 |     for (int i = 0; i < size; i++)
466 |     {
467 |         RGBpointBodyLidarToIMU(&feats_undistort->points[i],
468 |                                &laserCloudIMUBody->points[i]);
469 |     }
470 | 
471 |     sensor_msgs::PointCloud2 laserCloudmsg;
472 |     pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
473 |     laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
474 |     laserCloudmsg.header.frame_id = "body";
475 |     pubLaserCloudFull_body.publish(laserCloudmsg);
476 |     publish_count -= PUBFRAME_PERIOD;
477 | }
478 | 
479 | void publish_map(const ros::Publisher &pubLaserCloudMap)
480 | {
481 |     sensor_msgs::PointCloud2 laserCloudMap;
482 |     pcl::toROSMsg(*featsFromMap, laserCloudMap);
483 |     laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);
484 |     laserCloudMap.header.frame_id = "camera_init";
485 |     pubLaserCloudMap.publish(laserCloudMap);
486 | }
487 | 
488 | template <typename T>
489 | void set_posestamp(T &out)
490 | {
491 |     out.pose.position.x = state_point.pos(0);
492 |     out.pose.position.y = state_point.pos(1);
493 |     out.pose.position.z = state_point.pos(2);
494 | 
495 |     auto q_ = Eigen::Quaterniond(state_point.rot.matrix());
496 |     out.pose.orientation.x = q_.coeffs()[0];
497 |     out.pose.orientation.y = q_.coeffs()[1];
498 |     out.pose.orientation.z = q_.coeffs()[2];
499 |     out.pose.orientation.w = q_.coeffs()[3];
500 | }
501 | 
502 | void publish_odometry(const ros::Publisher &pubOdomAftMapped)
503 | {
504 |     odomAftMapped.header.frame_id = "camera_init";
505 |     odomAftMapped.child_frame_id = "body";
506 |     odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);
507 |     set_posestamp(odomAftMapped.pose);
508 |     pubOdomAftMapped.publish(odomAftMapped);
509 | 
510 |     auto P = kf.get_P();
511 |     for (int i = 0; i < 6; i++)
512 |     {
513 |         int k = i < 3 ? i + 3 : i - 3;
514 |         odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3);
515 |         odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4);
516 |         odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5);
517 |         odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0);
518 |         odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1);
519 |         odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2);
520 |     }
521 | 
522 |     static tf::TransformBroadcaster br;
523 |     tf::Transform transform;
524 |     tf::Quaternion q;
525 |     transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x,
526 |                                     odomAftMapped.pose.pose.position.y,
527 |                                     odomAftMapped.pose.pose.position.z));
528 |     q.setW(odomAftMapped.pose.pose.orientation.w);
529 |     q.setX(odomAftMapped.pose.pose.orientation.x);
530 |     q.setY(odomAftMapped.pose.pose.orientation.y);
531 |     q.setZ(odomAftMapped.pose.pose.orientation.z);
532 |     transform.setRotation(q);
533 |     br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "body"));
534 | }
535 | 
536 | void publish_path(const ros::Publisher pubPath)
537 | {
538 |     set_posestamp(msg_body_pose);
539 |     msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
540 |     msg_body_pose.header.frame_id = "camera_init";
541 | 
542 |     /*** if path is too large, the rvis will crash ***/
543 |     // static int jjj = 0;
544 |     // jjj++;
545 |     // if (jjj % 10 == 0)
546 |     // {
547 |         path.poses.push_back(msg_body_pose);
548 |         pubPath.publish(path);
549 |     // }
550 | }
551 | 
552 | int main(int argc, char **argv)
553 | {
554 |     ros::init(argc, argv, "laserMapping");
555 |     ros::NodeHandle nh("~");
556 | 
557 |     nh.param<bool>("publish/path_en", path_en, true);
558 |     nh.param<bool>("publish/scan_publish_en", scan_pub_en, true);            // 是否发布当前正在扫描的点云的topic
559 |     nh.param<bool>("publish/dense_publish_en", dense_pub_en, true);          // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic
560 |     nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, true); // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，需要该变量和上一个变量同时为true才发布
561 |     nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4);                   // 卡尔曼滤波的最大迭代次数
562 |     nh.param<string>("map_file_path", map_file_path, "");                    // 地图保存路径
563 |     nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar");         // 雷达点云topic名称
564 |     nh.param<string>("common/imu_topic", imu_topic, "/livox/imu");           // IMU的topic名称
565 |     nh.param<bool>("common/time_sync_en", time_sync_en, false);              // 是否需要时间同步，只有当外部未进行时间同步时设为true
566 |     nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
567 |     nh.param<double>("filter_size_corner", filter_size_corner_min, 0.5); // VoxelGrid降采样时的体素大小
568 |     nh.param<double>("filter_size_surf", filter_size_surf_min, 0.5);
569 |     nh.param<double>("filter_size_map", filter_size_map_min, 0.5);
570 |     nh.param<double>("cube_side_length", cube_len, 200);    // 地图的局部区域的长度（FastLio2论文中有解释）
571 |     nh.param<float>("mapping/det_range", DET_RANGE, 300.f); // 激光雷达的最大探测范围
572 |     nh.param<double>("mapping/fov_degree", fov_deg, 180);
573 |     nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1);               // IMU陀螺仪的协方差
574 |     nh.param<double>("mapping/acc_cov", acc_cov, 0.1);               // IMU加速度计的协方差
575 |     nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001);        // IMU陀螺仪偏置的协方差
576 |     nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001);        // IMU加速度计偏置的协方差
577 |     nh.param<double>("preprocess/blind", p_pre->blind, 0.01);        // 最小距离阈值，即过滤掉0～blind范围内的点云
578 |     nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA); // 激光雷达的类型
579 |     nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);       // 激光雷达扫描的线数（livox avia为6线）
580 |     nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US);
581 |     nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
582 |     nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);           // 采样间隔，即每隔point_filter_num个点取1个点
583 |     nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); // 是否提取特征点（FAST_LIO2默认不进行特征点提取）
584 |     nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true);
585 |     nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false); // 是否将点云地图保存到PCD文件
586 |     nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
587 |     nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>()); // 雷达相对于IMU的外参T（即雷达在IMU坐标系中的坐标）
588 |     nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>()); // 雷达相对于IMU的外参R
589 |     nh.param<string>("save_path", traj_log_file, "~/traj_log.txt"); // 雷达相对于IMU的外参R
590 | 
591 |     cout << "Lidar_type: " << p_pre->lidar_type << endl;
592 |     cout << "save trajectory to: " << traj_log_file << endl;
593 |     // 初始化path的header（包括时间戳和帧id），path用于保存odemetry的路径
594 |     path.header.stamp = ros::Time::now();
595 |     path.header.frame_id = "camera_init";
596 | 
597 |     /*** ROS subscribe initialization ***/
598 |     ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
599 |     ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
600 |     ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>("cloud_registered", 100000);
601 |     ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>("cloud_registered_body", 100000);
602 |     ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>("cloud_effected", 100000);
603 |     // ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/Laser_map", 100000);
604 |     ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("Odometry", 100000);
605 |     ros::Publisher pubPath = nh.advertise<nav_msgs::Path>("path", 100000);
606 | 
607 |     downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
608 |     downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
609 | 
610 |     shared_ptr<ImuProcess> p_imu1(new ImuProcess());
611 |     Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT);
612 |     Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR);
613 |     p_imu1->set_param(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU, V3D(gyr_cov, gyr_cov, gyr_cov), V3D(acc_cov, acc_cov, acc_cov),
614 |                       V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov), V3D(b_acc_cov, b_acc_cov, b_acc_cov));
615 | 
616 |     signal(SIGINT, SigHandle); //当程序检测到signal信号（例如ctrl+c） 时  执行 SigHandle 函数
617 |     ros::Rate rate(5000);
618 | 
619 |     while (ros::ok())
620 |     {
621 |         if (flg_exit)
622 |             break;
623 |         ros::spinOnce();
624 | 
625 |         if (sync_packages(Measures)) //把一次的IMU和LIDAR数据打包到Measures
626 |         {
627 |             cout << "----------------------------------------------" << endl;
628 |             double t00 = omp_get_wtime();
629 |             frame_count++;
630 |             if (flg_first_scan)
631 |             {
632 |                 first_lidar_time = Measures.lidar_beg_time;
633 |                 p_imu1->first_lidar_time = first_lidar_time;
634 |                 flg_first_scan = false;
635 |                 continue;
636 |             }
637 |             double t_pred = omp_get_wtime();
638 |             p_imu1->Process(Measures, kf, feats_undistort);
639 |             double t_pred_1 = omp_get_wtime();
640 |             cout << "predict time: " << (t_pred_1 - t_pred) * 1000 << " ms" << endl;
641 | 
642 |             //如果feats_undistort为空 ROS_WARN
643 |             if (feats_undistort->empty() || (feats_undistort == NULL))
644 |             {
645 |                 ROS_WARN("No point, skip this scan!\n");
646 |                 continue;
647 |             }
648 | 
649 |             double step_2_1 = omp_get_wtime();
650 |             state_point = kf.get_x();
651 |             pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I;
652 | 
653 |             flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true;
654 | 
655 |             lasermap_fov_segment(); //更新localmap边界，然后降采样当前帧点云
656 | 
657 |             //点云下采样
658 |             downSizeFilterSurf.setInputCloud(feats_undistort);
659 |             downSizeFilterSurf.filter(*feats_down_body);
660 |             feats_down_size = feats_down_body->points.size();
661 | 
662 |             // std::cout << "feats_down_size :" << feats_down_size << std::endl;
663 |             if (feats_down_size < 5)
664 |             {
665 |                 ROS_WARN("No point, skip this scan!\n");
666 |                 continue;
667 |             }
668 |             double step_2_2 = omp_get_wtime();
669 |             // cout << "feats_down time: " << (step_2_2 - step_2_1) * 1000 << " ms" << endl;
670 | 
671 |             //初始化ikdtree(ikdtree为空时)
672 |             if (ikdtree.Root_Node == nullptr)
673 |             {
674 |                 ikdtree.set_downsample_param(filter_size_map_min);
675 |                 feats_down_world->resize(feats_down_size);
676 |                 for (int i = 0; i < feats_down_size; i++)
677 |                 {
678 |                     pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); // lidar坐标系转到世界坐标系
679 |                 }
680 |                 ikdtree.Build(feats_down_world->points); //根据世界坐标系下的点构建ikdtree
681 |                 continue;
682 |             }
683 | 
684 |             if (0) // If you need to see map point, change to "if(1)"
685 |             {
686 |                 PointVector().swap(ikdtree.PCL_Storage);
687 |                 ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
688 |                 featsFromMap->clear();
689 |                 featsFromMap->points = ikdtree.PCL_Storage;
690 |                 // std::cout << "ikdtree size: " << featsFromMap->points.size() << std::endl;
691 |             }
692 |             double t_update_0 = omp_get_wtime();
693 |             /*** iterated state estimation ***/
694 |             Nearest_Points.resize(feats_down_size); //存储近邻点的vector
695 |             int iter_num = 0;
696 |             kf.update_iterated_dyn_share_modified(LASER_POINT_COV, feats_down_body, ikdtree, Nearest_Points, NUM_MAX_ITERATIONS, extrinsic_est_en, iter_num);
697 |             double t_update = omp_get_wtime();
698 |             double update_time = (t_update - t_update_0) * 1000;
699 | 
700 |             update_time_vec.push_back(update_time);
701 |             iter_counts.push_back(iter_num + 1);
702 | 
703 |             cout << "update time: " << update_time << " ms" << endl;
704 |             all_update_time += update_time;
705 |             // cout << "predict estimation time: " << (t_update - t_pred) * 1000 << " ms" << endl;
706 |             state_point = kf.get_x();
707 |             pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I;
708 | 
709 |             /******* Publish odometry *******/
710 |             publish_odometry(pubOdomAftMapped);
711 | 
712 |             /*** add the feature points to map kdtree ***/
713 |             feats_down_world->resize(feats_down_size);
714 | 
715 |             // double t_map_0 = omp_get_wtime();
716 |             map_incremental();
717 |             // double t_map_1 = omp_get_wtime();
718 |             // cout << "update time: " << (t_update - t_update_0) * 1000 << " ms" << endl;
719 |             // cout << "map_incremental time: " << (t_map_1 - t_map_0) * 1000 << " ms" << endl;
720 | 
721 |             /******* Publish points *******/
722 |             if (path_en)
723 |                 publish_path(pubPath);
724 |             if (scan_pub_en || pcd_save_en)
725 |                 publish_frame_world(pubLaserCloudFull);
726 |             if (scan_pub_en && scan_body_pub_en)
727 |                 publish_frame_body(pubLaserCloudFull_body);
728 |             // publish_map(pubLaserCloudMap);
729 | 
730 |             double t11 = omp_get_wtime();
731 |             all_time += (t11 - t00) * 1000;
732 |             std::cout << "feats_down_size: " << feats_down_size << "  Whole mapping time(ms):  " << (t11 - t00) * 1000 << std::endl
733 |                       << std::endl;
734 |         }
735 | 
736 |         rate.sleep();
737 |     }
738 | 
739 |     /**************** save traj ****************/
740 |     printf("all_frames: %d, all_time(ms): %0.6f, average_time(ms): %0.6f\n, average_time(ms): %0.6f\n", frame_count, all_time, all_time / frame_count, all_update_time / frame_count);
741 |     // cout << "*******************average_time(ms)************************* " << all_time / frame_count << endl;
742 |     // string traj_file = "/home/zj/workspace/zc_paper_ws/traj_compare/inva-fast-lio-out/traj.txt";
743 |     cout << "finishing mapping" << endl;
744 |     std::ofstream ofs;
745 |     ofs.open(traj_log_file, std::ios::out);
746 |     if (!ofs.is_open()) {
747 |         cout << "Failed to open traj_file: " << traj_log_file;
748 |         return 0;
749 |     }
750 |     // ofs << "#timestamp x y z q_x q_y q_z q_w" << std::endl;
751 |     for (const auto &p : path.poses) {
752 |         ofs << std::fixed << std::setprecision(6) << p.header.stamp.toSec() << " " << std::setprecision(15)
753 |             << p.pose.position.x << " " << p.pose.position.y << " " << p.pose.position.z << " " << p.pose.orientation.x
754 |             << " " << p.pose.orientation.y << " " << p.pose.orientation.z << " " << p.pose.orientation.w << std::endl;
755 |     }
756 |     ofs.close();
757 | 
758 |     string iter_count_file = "/home/zj/workspace/zc_paper_ws/final_figure/inva_lio_iter_count.txt";
759 |     std::ofstream ofs_1;
760 |     ofs_1.open(iter_count_file, std::ios::out);
761 |     if (!ofs_1.is_open()) {
762 |         cout << "Failed to open iter_count_file: " << iter_count_file;
763 |         return 0;
764 |     }
765 |     for (const auto &cnt : iter_counts) {
766 |         ofs_1 << cnt << std::endl;
767 |     }
768 |     ofs_1.close();
769 | 
770 |     std::ofstream ofs_2;
771 |     string update_time_count_file = "/home/zj/workspace/zc_paper_ws/final_figure/inva_lio_average_update_time.txt";
772 |     ofs_2.open(update_time_count_file, std::ios::out);
773 |     if (!ofs_2.is_open()) {
774 |         cout << "Failed to open traj_file: " << update_time_count_file;
775 |         return 0;
776 |     }
777 |     // ofs << "#timestamp x y z q_x q_y q_z q_w" << std::endl;
778 |     for (const auto &p : update_time_vec) {
779 |         ofs_2 << std::fixed << std::setprecision(6) << p << std::endl;
780 |     }
781 |     ofs_2.close();
782 | 
783 |     /**************** save map ****************/
784 |     /* 1. make sure you have enough memories
785 |     /* 2. pcd save will largely influence the real-time performences **/
786 |     // if (pcl_wait_save->size() > 0 && pcd_save_en)
787 |     // {
788 |     //     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
789 |     //     for (size_t i = 1; i <= pcd_index; i++)
790 |     //     {
791 |     //         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
792 |     //         string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(i) + string(".pcd"));
793 |     //         pcl::PCDReader reader;
794 |     //         reader.read(all_points_dir, *cloud_temp);
795 |     //         *cloud = *cloud + *cloud_temp;
796 |     //     }
797 | 
798 |     //     string file_name = string("GlobalMap.pcd");
799 |     //     string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
800 |     //     pcl::PCDWriter pcd_writer;
801 |     //     cout << "current scan saved to /PCD/" << file_name << endl;
802 |     //     pcd_writer.writeBinary(all_points_dir, *cloud);
803 | 
804 |     //     //////////////////////////////////////
805 |     //     PointVector().swap(ikdtree.PCL_Storage);
806 |     //     ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
807 |     //     featsFromMap->clear();
808 |     //     featsFromMap->points = ikdtree.PCL_Storage;
809 |     //     std::cout << "ikdtree size: " << featsFromMap->points.size() << std::endl;
810 |     //     string file_name1 = string("GlobalMap_ikdtree.pcd");
811 |     //     pcl::PCDWriter pcd_writer1;
812 |     //     string all_points_dir1(string(string(ROOT_DIR) + "PCD/") + file_name1);
813 |     //     cout << "current scan saved to /PCD/" << file_name1 << endl;
814 |     //     pcd_writer1.writeBinary(all_points_dir1, *featsFromMap);
815 |     // }
816 | 
817 |     return 0;
818 | }
819 | 


--------------------------------------------------------------------------------