support ouster and robosense lidar

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

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.

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

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;
 

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:
 

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;

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);

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>