From 626df32cb62d7dc76b45a1a75a4c0c3c23ebfada Mon Sep 17 00:00:00 2001
From: cc <960732491@qq.com>
Date: Fri, 18 Nov 2022 17:02:20 +0800
Subject: [PATCH] support ouster and robosense lidar

---
 CMakeLists.txt            |   5 +-
 config/nclt.yaml          |   2 +-
 config/ouster.yaml        |  72 ++++++++++
 include/cloudMap.h        |   4 +-
 include/cloudProcessing.h |  38 ++++-
 include/lioOptimization.h |   2 +-
 include/utility.h         |   4 +
 launch/lio_ouster.launch  |  16 +++
 src/cloudProcessing.cpp   | 288 +++++++++++++++++++++++++++++++++++++-
 src/lioOptimization.cpp   |  25 ++--
 src/utility.cpp           |   9 ++
 11 files changed, 440 insertions(+), 25 deletions(-)
 create mode 100644 config/ouster.yaml
 create mode 100644 launch/lio_ouster.launch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42634e1..5da0b63 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,10 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(sr_lio)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
+set(CMAKE_BUILD_TYPE "Release")
+set (CMAKE_CXX_STANDARD 14)
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
+
 
 find_package(catkin REQUIRED COMPONENTS
   eigen_conversions
diff --git a/config/nclt.yaml b/config/nclt.yaml
index 1e5b060..55bb783 100644
--- a/config/nclt.yaml
+++ b/config/nclt.yaml
@@ -7,7 +7,7 @@ common:
     gravity_acc: [ 0.0, 0.0, 9.80416]
     
 lidar_parameter:
-    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for robosense
     N_SCANS: 32
     SCAN_RATE: 10                # only need to be set for velodyne, unit: Hz,
     time_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
diff --git a/config/ouster.yaml b/config/ouster.yaml
new file mode 100644
index 0000000..1b6a2b9
--- /dev/null
+++ b/config/ouster.yaml
@@ -0,0 +1,72 @@
+common:
+    lidar_topic:  "/os_cloud_node/points"
+    imu_topic:  "/os_cloud_node/imu"
+    point_filter_num: 6
+    sweep_cut_num: 3
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+    gravity_acc: [ 0.0, 0.0, 9.80416]
+    
+lidar_parameter:
+    lidar_type: 3                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for robosense
+    N_SCANS: 32
+    Horizon_SCAN: 1800
+    SCAN_RATE: 10                # only need to be set for velodyne, unit: Hz,
+    time_unit: 3            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+    blind: 2
+    fov_degree:    180
+    det_range:     100.0
+
+imu_parameter:
+    acc_cov: 0.1
+    gyr_cov: 0.1
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    time_diff_enable: false
+
+extrinsic_parameter:
+    extrinsic_enable:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_t: [ 0.0, 0.0, -0.0]
+    extrinsic_R: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, 1]
+
+
+odometry_options:
+    voxel_size: 0.5 # The voxel size for the grid sampling of the new frame (before keypoints extraction)
+    sample_voxel_size: 1.5 # The size of a voxel for the selection of `keypoints` by grid sampling
+    max_distance: 2000.0 # The threshold of the distance to suppress voxels from the map
+    max_num_points_in_voxel: 20 # The maximum number of points per voxel of the map
+    method_system_init: STATIC_INIT # LIO system initialization [MOTION_INIT, STATIC_INIT]
+    init_num_frames: 20
+    robust_registration: false
+    min_distance_points: 0.15
+    distance_error_threshold: 100.0 # The motion of the sensor between two frames which is considered erroneous (stops the odometry)
+    motion_compensation: CONTINUOUS # The profile of the motion compensation (NONE, CONSTANT_VELOCITY, ITERATIVE, CONTINUOUS)
+    initialization: INIT_IMU # [INIT_CONSTANT_VELOCITY, INIT_IMU]
+
+icp_options:
+    size_voxel_map: 1.0 # The voxel size of in the voxel map
+    num_iters_icp: 15 # The number of iterations of the ICP
+    min_number_neighbors: 20 # The minimum number of neighbor points to define a valid neighborhood
+    voxel_neighborhood: 1
+    max_number_neighbors: 20
+    max_dist_to_plane_ct_icp: 0.3
+    threshold_orientation_norm: 0.1 # Threshold on orientation changes (in degrees) for early termination of the ICP
+    threshold_translation_norm: 0.01 # Threshold on distance changes (in m) for early termination of the ICP
+    debug_print: false
+    point_to_plane_with_distortion: true
+    distance: CT_POINT_TO_PLANE # Options: [CT_POINT_TO_PLANE, POINT_TO_PLANE]
+    num_closest_neighbors: 1
+    beta_location_consistency: 1.0
+    beta_orientation_consistency: 1.0
+    beta_constant_velocity: 1.0 # Trajectory Regularisation Weight to constrain the optimization
+    beta_small_velocity: 0.00
+    solver: LIO # The type of SOLVER used in [LIO, LIDAR]
+    min_num_residuals: 200 # The minimum number of residuals for a valid ICP problem
+    max_num_residuals: 600 # The maximum number of residuals considered (if more keypoints exist, residuals are randomly sampled)
+
+    loss_function: HUBER # Options: [CAUCHY, STANDARD, HUBER, TOLERANT, TRUNCATED]
+    ls_max_num_iters: 5 # The number of steps performed by ceres for each iteration of the ICP
+    ls_num_threads: 16 # The number of threads to build and solve the least square system
+    ls_sigma: 0.5
+    ls_tolerant_min_threshold: 0.05
diff --git a/include/cloudMap.h b/include/cloudMap.h
index 175bc23..5fd1810 100644
--- a/include/cloudMap.h
+++ b/include/cloudMap.h
@@ -18,8 +18,8 @@ struct point3D {
 
     Eigen::Vector3d raw_point;
     Eigen::Vector3d point;
-    double alpha_time = 0.0;
-    double relative_time = 0.0;
+    double alpha_time = 0.0;    //  reference to last point of current frame
+    double relative_time = 0.0; //  feference to current frame
     double timestamp = 0.0;
     int index_frame = -1;
 
diff --git a/include/cloudProcessing.h b/include/cloudProcessing.h
index 7c37dce..086a8eb 100644
--- a/include/cloudProcessing.h
+++ b/include/cloudProcessing.h
@@ -19,7 +19,7 @@
 
 #include "cloudMap.h"
 
-enum LID_TYPE{AVIA = 1, VELO16 = 2, OUST64 = 3};
+enum LID_TYPE{AVIA = 1, VELO = 2, OUST = 3, ROBO = 4};
 enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
 enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
 enum Surround{Prev, Next};
@@ -63,6 +63,38 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
     (uint16_t, ring, ring)
 )
 
+namespace ouster_ros
+{
+	struct Point
+	{
+		PCL_ADD_POINT4D;
+		float intensity;
+		uint32_t t;
+		uint16_t reflectivity;
+		uint8_t ring;
+		// uint16_t noise;
+		uint16_t ambient;
+		uint32_t range;
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	} EIGEN_ALIGN16;
+}
+POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
+						    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint32_t, t, t)(uint16_t, reflectivity, reflectivity)(uint8_t, ring, ring)(uint16_t, ambient, ambient)(uint32_t, range, range))
+
+namespace robosense_ros
+{
+	struct Point
+	{
+		PCL_ADD_POINT4D;
+		uint8_t intensity;
+		uint16_t ring = 0;
+		double timestamp = 0;
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	} EIGEN_ALIGN16;
+}
+POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point,
+						    (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp))
+
 class cloudProcessing
 {
 private:
@@ -92,9 +124,9 @@ class cloudProcessing
 
 	// function
 	void resetVector();
-	void oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out);
+	void ousterHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out, std::vector<double> &v_dt_offset);
 	void velodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out, std::vector<double> &v_dt_offset);
-
+	void robosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out, std::vector<double> &v_dt_offset);
 
 public:
 
diff --git a/include/lioOptimization.h b/include/lioOptimization.h
index 37ded7e..ad0ef86 100644
--- a/include/lioOptimization.h
+++ b/include/lioOptimization.h
@@ -190,7 +190,7 @@ class lioOptimization{
 
     std::queue<std::pair<double, double>> time_buffer;
     std::queue<std::vector<pcl::PointCloud<pcl::PointXYZINormal>::Ptr>> feature_buffer;
-    std::queue<std::vector<std::vector<point3D>>> lidar_buffer;
+    std::queue<std::vector<std::vector<point3D>>> lidar_buffer; //  存储重构后的单帧数据
     std::queue<sensor_msgs::Imu::ConstPtr> imu_buffer;
 
     std::vector<cloudFrame*> all_cloud_frame;
diff --git a/include/utility.h b/include/utility.h
index 67eb6bb..0089a3a 100644
--- a/include/utility.h
+++ b/include/utility.h
@@ -38,6 +38,10 @@ bool time_list(point3D &point_1, point3D &point_2);
 
 bool time_list_velodyne(velodyne_ros::Point &point_1, velodyne_ros::Point &point_2);
 
+bool time_list_ouster(ouster_ros::Point &point_1, ouster_ros::Point &point_2);
+
+bool time_list_robosense(robosense_ros::Point &point_1, robosense_ros::Point &point_2);
+
 void point3DtoPCL(std::vector<point3D> &v_point_temp, pcl::PointCloud<pcl::PointXYZINormal>::Ptr &p_cloud_temp);
 
 Eigen::Matrix3d mat33FromArray(std::vector<double> &array);
diff --git a/launch/lio_ouster.launch b/launch/lio_ouster.launch
new file mode 100644
index 0000000..af38efc
--- /dev/null
+++ b/launch/lio_ouster.launch
@@ -0,0 +1,16 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find sr_lio)/config/ouster.yaml" />
+
+    <param name="debug_output" type="bool" value="0"/>
+    <param name="output_path" type="string" value="$(find sr_lio)/output"/>
+    <node pkg="sr_lio" type="lio_optimization" name="lio_optimization" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find sr_lio)/rviz_cfg/visualization.rviz" />
+    </group>
+
+</launch>
diff --git a/src/cloudProcessing.cpp b/src/cloudProcessing.cpp
index da88745..ebe9763 100644
--- a/src/cloudProcessing.cpp
+++ b/src/cloudProcessing.cpp
@@ -97,16 +97,21 @@ void cloudProcessing::process(const sensor_msgs::PointCloud2::ConstPtr &msg, std
 {
 	switch (lidar_type)
 	{
-	case OUST64:
-		ROS_ERROR("Only Velodyne LiDAR interface is supported currently.");
+	case OUST:
+		ousterHandler(msg, v_cloud_out, v_dt_offset);
+		// ROS_ERROR("Only Velodyne LiDAR interface is supported currently.");
 		break;
 
-	case VELO16:
+	case VELO:
 		velodyneHandler(msg, v_cloud_out, v_dt_offset);
 		break;
 
+	case ROBO:
+		robosenseHandler(msg, v_cloud_out, v_dt_offset);
+		break;
+
 	default:
-		ROS_ERROR("Only Velodyne LiDAR interface is supported currently.");
+		ROS_ERROR("Error Lidar Type, Please check.");
 		break;
 	}
 
@@ -145,9 +150,282 @@ void cloudProcessing::process(const sensor_msgs::PointCloud2::ConstPtr &msg, std
     sweep_id++;
 }
 
-void cloudProcessing::oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out)
+void cloudProcessing::ousterHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out, std::vector<double> &v_dt_offset)
+{
+	resetVector();
+
+	pcl::PointCloud<ouster_ros::Point> raw_cloud;
+	pcl::fromROSMsg(*msg, raw_cloud);
+	int size = raw_cloud.points.size();
+
+	double dt_last_point;
+
+	if (size == 0)
+	{
+		v_cloud_out = v_cut_sweep;
+		v_dt_offset.resize(sweep_cut_num);
+
+		for (int i = 0; i < sweep_cut_num; i++)
+		{
+			v_dt_offset[i] = (i + 1) * delta_cut_time;
+		}
+
+		return;
+	}
+
+	if (raw_cloud.points[size - 1].t > 0)
+		given_offset_time = true;
+	else
+		given_offset_time = false;
+
+	if (given_offset_time)
+	{
+		sort(raw_cloud.points.begin(), raw_cloud.points.end(), time_list_ouster);
+		dt_last_point = raw_cloud.points.back().t * time_unit_scale;
+		delta_cut_time = dt_last_point / sweep_cut_num;
+	}
+
+	double omega = 0.361 * SCAN_RATE;
+
+	std::vector<bool> is_first;
+	is_first.resize(N_SCANS);
+	fill(is_first.begin(), is_first.end(), true);
+
+	std::vector<double> yaw_first_point;
+	yaw_first_point.resize(N_SCANS);
+	fill(yaw_first_point.begin(), yaw_first_point.end(), 0.0);
+
+	std::vector<point3D> v_point_full;
+
+	for (int i = 0; i < size; i++)
+	{
+		point3D point_temp;
+
+		point_temp.raw_point = Eigen::Vector3d(raw_cloud.points[i].x, raw_cloud.points[i].y, raw_cloud.points[i].z);
+		point_temp.point = point_temp.raw_point;
+		point_temp.relative_time = raw_cloud.points[i].t * time_unit_scale;
+
+		if (!given_offset_time)
+		{
+			int layer = raw_cloud.points[i].ring;
+			double yaw_angle = atan2(point_temp.raw_point.y(), point_temp.raw_point.x()) * 57.2957;
+
+			if (is_first[layer])
+			{
+				yaw_first_point[layer] = yaw_angle;
+				is_first[layer] = false;
+				point_temp.relative_time = 0.0;
+
+				v_point_full.push_back(point_temp);
+
+				continue;
+			}
+
+			if (yaw_angle <= yaw_first_point[layer])
+			{
+				point_temp.relative_time = (yaw_first_point[layer] - yaw_angle) / omega;
+			}
+			else
+			{
+				point_temp.relative_time = (yaw_first_point[layer] - yaw_angle + 360.0) / omega;
+			}
+
+			point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec();
+			v_point_full.push_back(point_temp);
+		}
+
+		if (given_offset_time && i % point_filter_num == 0)
+		{
+			if (point_temp.raw_point.x() * point_temp.raw_point.x() + point_temp.raw_point.y() * point_temp.raw_point.y() + point_temp.raw_point.z() * point_temp.raw_point.z() > (blind * blind))
+			{
+				point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec();
+				point_temp.alpha_time = point_temp.relative_time / dt_last_point;
+
+				int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num;
+
+				id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id;
+
+				v_cut_sweep[id].push_back(point_temp);
+			}
+		}
+	}
+
+	if (!given_offset_time)
+	{
+		assert(v_point_full.size() == size);
+
+		sort(v_point_full.begin(), v_point_full.end(), time_list);
+		dt_last_point = v_point_full.back().relative_time;
+		delta_cut_time = dt_last_point / sweep_cut_num;
+
+		for (int i = 0; i < size; i++)
+		{
+			if (i % point_filter_num == 0)
+			{
+				point3D point_temp = v_point_full[i];
+				point_temp.alpha_time = (point_temp.relative_time / dt_last_point);
+
+				if (point_temp.alpha_time > 1)
+					point_temp.alpha_time = 1;
+				if (point_temp.alpha_time < 0)
+					point_temp.alpha_time = 0;
+
+				int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num;
+
+				id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id;
+
+				v_cut_sweep[id].push_back(point_temp);
+			}
+		}
+	}
+
+	for (int i = 1; i <= sweep_cut_num - 1; i++)
+		v_dt_offset.push_back(delta_cut_time * i);
+
+	v_dt_offset.push_back(dt_last_point);
+
+	assert(v_dt_offset.size() == sweep_cut_num);
+
+	v_cloud_out = v_cut_sweep;
+}
+
+void cloudProcessing::robosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out, std::vector<double> &v_dt_offset)
 {
+	resetVector();
+
+	pcl::PointCloud<robosense_ros::Point> raw_cloud;
+	pcl::fromROSMsg(*msg, raw_cloud);
+	int size = raw_cloud.points.size();
+
+	double dt_last_point;
+
+	if (size == 0)
+	{
+		v_cloud_out = v_cut_sweep;
+		v_dt_offset.resize(sweep_cut_num);
+
+		for (int i = 0; i < sweep_cut_num; i++)
+		{
+			v_dt_offset[i] = (i + 1) * delta_cut_time;
+		}
+
+		return;
+	}
+
+	if (raw_cloud.points[size - 1].timestamp > 0)
+		given_offset_time = true;
+	else
+		given_offset_time = false;
+
+	if (given_offset_time)
+	{
+		sort(raw_cloud.points.begin(), raw_cloud.points.end(), time_list_robosense);
+		dt_last_point = raw_cloud.points.back().timestamp * time_unit_scale;
+		delta_cut_time = dt_last_point / sweep_cut_num;
+	}
+
+	double omega = 0.361 * SCAN_RATE;
+
+	std::vector<bool> is_first;
+	is_first.resize(N_SCANS);
+	fill(is_first.begin(), is_first.end(), true);
+
+	std::vector<double> yaw_first_point;
+	yaw_first_point.resize(N_SCANS);
+	fill(yaw_first_point.begin(), yaw_first_point.end(), 0.0);
+
+	std::vector<point3D> v_point_full;
+
+	for (int i = 0; i < size; i++)
+	{
+		point3D point_temp;
+
+		point_temp.raw_point = Eigen::Vector3d(raw_cloud.points[i].x, raw_cloud.points[i].y, raw_cloud.points[i].z);
+		point_temp.point = point_temp.raw_point;
+		point_temp.relative_time = raw_cloud.points[i].timestamp * time_unit_scale;
+
+		if (!given_offset_time)
+		{
+			int layer = raw_cloud.points[i].ring;
+			double yaw_angle = atan2(point_temp.raw_point.y(), point_temp.raw_point.x()) * 57.2957;
+
+			if (is_first[layer])
+			{
+				yaw_first_point[layer] = yaw_angle;
+				is_first[layer] = false;
+				point_temp.relative_time = 0.0;
+
+				v_point_full.push_back(point_temp);
+
+				continue;
+			}
+
+			if (yaw_angle <= yaw_first_point[layer])
+			{
+				point_temp.relative_time = (yaw_first_point[layer] - yaw_angle) / omega;
+			}
+			else
+			{
+				point_temp.relative_time = (yaw_first_point[layer] - yaw_angle + 360.0) / omega;
+			}
+
+			point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec();
+			v_point_full.push_back(point_temp);
+		}
+
+		if (given_offset_time && i % point_filter_num == 0)
+		{
+			if (point_temp.raw_point.x() * point_temp.raw_point.x() + point_temp.raw_point.y() * point_temp.raw_point.y() + point_temp.raw_point.z() * point_temp.raw_point.z() > (blind * blind))
+			{
+				point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec();
+				point_temp.alpha_time = point_temp.relative_time / dt_last_point;
+
+				int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num;
+
+				id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id;
+
+				v_cut_sweep[id].push_back(point_temp);
+			}
+		}
+	}
+
+	if (!given_offset_time)
+	{
+		assert(v_point_full.size() == size);
+
+		sort(v_point_full.begin(), v_point_full.end(), time_list);
+		dt_last_point = v_point_full.back().relative_time;
+		delta_cut_time = dt_last_point / sweep_cut_num;
+
+		for (int i = 0; i < size; i++)
+		{
+			if (i % point_filter_num == 0)
+			{
+				point3D point_temp = v_point_full[i];
+				point_temp.alpha_time = (point_temp.relative_time / dt_last_point);
+
+				if (point_temp.alpha_time > 1)
+					point_temp.alpha_time = 1;
+				if (point_temp.alpha_time < 0)
+					point_temp.alpha_time = 0;
+
+				int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num;
+
+				id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id;
+
+				v_cut_sweep[id].push_back(point_temp);
+			}
+		}
+	}
+
+	for (int i = 1; i <= sweep_cut_num - 1; i++)
+		v_dt_offset.push_back(delta_cut_time * i);
+
+	v_dt_offset.push_back(dt_last_point);
+
+	assert(v_dt_offset.size() == sweep_cut_num);
 
+	v_cloud_out = v_cut_sweep;
 }
 
 void cloudProcessing::velodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector<std::vector<point3D>> &v_cloud_out, std::vector<double> &v_dt_offset)
diff --git a/src/lioOptimization.cpp b/src/lioOptimization.cpp
index bb0e519..f2546b9 100644
--- a/src/lioOptimization.cpp
+++ b/src/lioOptimization.cpp
@@ -371,23 +371,23 @@ void lioOptimization::standardCloudHandler(const sensor_msgs::PointCloud2::Const
     double sample_size = index_frame < options.init_num_frames ? options.init_voxel_size : options.voxel_size;
 
     assert(msg->header.stamp.toSec() > last_time_lidar);
-    
-    std::vector<std::vector<point3D>> v_cut_sweep;
-    std::vector<double> v_dt_offset;
 
-    cloud_pro->process(msg, v_cut_sweep, v_dt_offset);
+    std::vector<std::vector<point3D>> v_cut_sweep; //    最多缓存两帧数据,分割为多个小块
+    std::vector<double> v_dt_offset;               //  分块的时间戳
+
+    cloud_pro->process(msg, v_cut_sweep, v_dt_offset); //   v_dt_offset数据等分的时间戳
 
     for(int i = 1; i < sweep_cut_num + 1; i++)
     {
 
-        std::vector<std::vector<point3D>> v_buffer_temp;
+        std::vector<std::vector<point3D>> v_buffer_temp; //  降采样,重排数据,然后组成新一帧数据
 
         for(int j = i; j < i + sweep_cut_num; j++)
         {
             std::vector<point3D> frame(v_cut_sweep[j]);
 
             boost::mt19937_64 g;
-            std::shuffle(frame.begin(), frame.end(), g);
+            std::shuffle(frame.begin(), frame.end(), g); //  随机重排容器中的元素
 
             subSampleFrame(frame, sample_size);
 
@@ -399,7 +399,7 @@ void lioOptimization::standardCloudHandler(const sensor_msgs::PointCloud2::Const
         assert(v_buffer_temp.size() == sweep_cut_num);
         lidar_buffer.push(v_buffer_temp);
 
-        time_buffer.push(std::make_pair(msg->header.stamp.toSec(), v_dt_offset[i - 1] / (double)1000.0));
+        time_buffer.push(std::make_pair(msg->header.stamp.toSec(), v_dt_offset[i - 1] / (double)1000.0)); // 用以存储新构建数据的时间戳
     }
 
     assert(msg->header.stamp.toSec() > last_time_lidar);
@@ -437,7 +437,7 @@ std::vector<std::pair<std::pair<std::vector<sensor_msgs::ImuConstPtr>, std::vect
             return measurements;
         }
 
-        if (!(imu_buffer.front()->header.stamp.toSec() < time_buffer.front().first + time_buffer.front().second))
+        if (!(imu_buffer.front()->header.stamp.toSec() < time_buffer.front().first + time_buffer.front().second)) //  IMU大于重组建激光的时间戳
         {
             time_buffer.pop();
 
@@ -450,7 +450,7 @@ std::vector<std::pair<std::pair<std::vector<sensor_msgs::ImuConstPtr>, std::vect
             continue;
         }
 
-        double timestamp = time_buffer.front().first + time_buffer.front().second;
+        double timestamp = time_buffer.front().first + time_buffer.front().second; // 重组建激光的时间戳
         double timestamp_begin = time_buffer.front().first;
         double timestamp_offset = time_buffer.front().second;
         time_buffer.pop();
@@ -471,7 +471,8 @@ std::vector<std::pair<std::pair<std::vector<sensor_msgs::ImuConstPtr>, std::vect
 
         std::vector<std::vector<point3D>> v_cut_sweep = lidar_buffer.front();
 
-        for(int i = 0; i < lidar_buffer.front().size(); i++) std::vector<point3D>().swap(lidar_buffer.front()[i]);
+        for (int i = 0; i < lidar_buffer.front().size(); i++) // 移除lidar_buffer首个数据
+            std::vector<point3D>().swap(lidar_buffer.front()[i]);
         std::vector<std::vector<point3D>>().swap(lidar_buffer.front());
         assert(lidar_buffer.front().size() == 0);
         lidar_buffer.pop();
@@ -980,7 +981,7 @@ void lioOptimization::stateEstimation(std::vector<std::vector<point3D>> &v_cut_s
 
     std::vector<point3D> const_frame;
 
-    for(int i = 0; i < v_cut_sweep.size(); i++)
+    for (int i = 0; i < v_cut_sweep.size(); i++) //  组成一帧数据
         const_frame.insert(const_frame.end(), v_cut_sweep[i].begin(), v_cut_sweep[i].end());
 
     cloudFrame *p_frame = buildFrame(const_frame, imu_pro->current_state, timestamp_begin, timestamp_offset);
@@ -1006,7 +1007,7 @@ void lioOptimization::stateEstimation(std::vector<std::vector<point3D>> &v_cut_s
     std::cout << "translation_end: " << p_frame->p_state->translation.x() << " " << p_frame->p_state->translation.y() << " " << p_frame->p_state->translation.z() << std::endl;
 
     imu_pro->last_state = imu_pro->current_state;
-    imu_pro->current_state = new state(imu_pro->last_state, false);
+    imu_pro->current_state = new state(imu_pro->last_state, false); //   重置状态量
 
     publish_odometry(pub_odom,p_frame);
     publish_path(pub_path,p_frame);   
diff --git a/src/utility.cpp b/src/utility.cpp
index 374343f..2f145ff 100644
--- a/src/utility.cpp
+++ b/src/utility.cpp
@@ -22,6 +22,15 @@ bool time_list_velodyne(velodyne_ros::Point &point_1, velodyne_ros::Point &point
     return (point_1.time < point_2.time);
 }
 
+bool time_list_ouster(ouster_ros::Point &point_1, ouster_ros::Point &point_2)
+{
+    return (point_1.t < point_2.t);
+}
+
+bool time_list_robosense(robosense_ros::Point &point_1, robosense_ros::Point &point_2)
+{
+    return (point_1.timestamp < point_2.timestamp);
+}
 void point3DtoPCL(std::vector<point3D> &v_point_temp, pcl::PointCloud<pcl::PointXYZINormal>::Ptr &p_cloud_temp)
 {
     for(int i = 0; i < v_point_temp.size(); i++)