Add direct support for Livox xfer_format: 0

Author: Kenny Chen

README.md

@@ -12,7 +12,9 @@ DLIO is a new lightweight LiDAR-inertial odometry algorithm with a novel coarse-
 ## Instructions
 
 ### Sensor Setup & Compatibility
-DLIO has been extensively tested using a variety of sensor configurations and currently supports Ouster, Velodyne, Hesai, and Livox LiDARs. The point cloud should be of input type `sensor_msgs::PointCloud2` and the 6-axis IMU input type of `sensor_msgs::Imu`. For Livox sensors specifically, please use the `feature/livox-support` branch and the latest [`livox_ros_driver2`](https://github.com/Livox-SDK/livox_ros_driver2) package with `xfer_format: 1` and point cloud of input type `livox_ros_driver2::CustomMsg` (see [here](https://github.com/vectr-ucla/direct_lidar_inertial_odometry/issues/5) for more information).
+DLIO has been extensively tested using a variety of sensor configurations and currently supports Ouster, Velodyne, Hesai, and Livox LiDARs. The point cloud should be of input type `sensor_msgs::PointCloud2` and the 6-axis IMU input type of `sensor_msgs::Imu`. 
+
+For Livox sensors specifically, you can use the `master` branch directly if it is of type `sensor_msgs::PointCloud2` (`xfer_format: 0`), or the `feature/livox-support` branch and the latest [`livox_ros_driver2`](https://github.com/Livox-SDK/livox_ros_driver2) package if it is of type `livox_ros_driver2::CustomMsg` (`xfer_format: 1`) (see [here](https://github.com/vectr-ucla/direct_lidar_inertial_odometry/issues/5) for more information).
 
 For best performance, extrinsic calibration between the LiDAR/IMU sensors and the robot's center-of-gravity should be inputted into `cfg/dlio.yaml`. If the exact values of these are unavailable, a rough LiDAR-to-IMU extrinsics can also be used (note however that performance will be degraded).
 
@@ -56,7 +58,7 @@ roslaunch direct_lidar_inertial_odometry dlio.launch \
   imu_topic:=/robot/imu
 ```
 
-for Ouster, Velodyne, or Hesai sensors, or 
+for Ouster, Velodyne, Hesai, or Livox (`xfer_format: 0`) sensors, or 
 
 ```sh
 roslaunch direct_lidar_inertial_odometry dlio.launch \
@@ -65,7 +67,7 @@ roslaunch direct_lidar_inertial_odometry dlio.launch \
   imu_topic:=/robot/imu
 ```
 
-for Livox sensors.
+for Livox sensors (`xfer_format: 1`).
 
 Be sure to change the topic names to your corresponding topics. Alternatively, edit the launch file directly if desired. If successful, you should see the following output in your terminal:
 <br>

cfg/params.yaml

@@ -28,6 +28,7 @@ dlio:
   odom:
 
     gravity: 9.80665
+    computeTimeOffset: true
 
     imu:
       approximateGravity: false

include/dlio/dlio.h

@@ -78,7 +78,7 @@ std::string to_string_with_precision(const T a_value, const int n = 6)
 #include <direct_lidar_inertial_odometry/save_pcd.h>
 
 namespace dlio {
-  enum class SensorType { OUSTER, VELODYNE, HESAI, UNKNOWN };
+  enum class SensorType { OUSTER, VELODYNE, HESAI, LIVOX, UNKNOWN };
 
   class OdomNode;
   class MapNode;
@@ -89,9 +89,10 @@ namespace dlio {
     PCL_ADD_POINT4D;
     float intensity; // intensity
     union {
-      std::uint32_t t; // time since beginning of scan in nanoseconds
-      float time; // time since beginning of scan in seconds
-      double timestamp; // absolute timestamp in seconds
+    std::uint32_t t;   // (Ouster) time since beginning of scan in nanoseconds
+    float time;        // (Velodyne) time since beginning of scan in seconds
+    double timestamp;  // (Hesai) absolute timestamp in seconds
+                       // (Livox) absolute timestamp in (seconds * 10e9)
     };
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   } EIGEN_ALIGN16;

include/dlio/odom.h

@@ -286,6 +286,8 @@ class dlio::OdomNode {
 
   double gravity_;
 
+  bool time_offset_;
+
   bool adaptive_params_;
 
   double obs_submap_thresh_;

src/dlio/odom.cc

@@ -189,6 +189,9 @@ void dlio::OdomNode::getParams() {
   // Gravity
   ros::param::param<double>("~dlio/odom/gravity", this->gravity_, 9.80665);
 
+  // Compute time offset between lidar and imu
+  ros::param::param<bool>("~dlio/odom/computeTimeOffset", this->time_offset_, false);
+
   // Keyframe Threshold
   ros::param::param<double>("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1);
   ros::param::param<double>("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0);
@@ -510,9 +513,12 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc)
     } else if (field.name == "time") {
       this->sensor = dlio::SensorType::VELODYNE;
       break;
-    } else if (field.name == "timestamp") {
+    } else if (field.name == "timestamp" && original_scan_->points[0].timestamp < 1e14) {
       this->sensor = dlio::SensorType::HESAI;
       break;
+    } else if (field.name == "timestamp" && original_scan_->points[0].timestamp > 1e14) {
+      this->sensor = dlio::SensorType::LIVOX;
+      break;
     }
   }
 
@@ -629,6 +635,14 @@ void dlio::OdomNode::deskewPointcloud() {
     extract_point_time = [&sweep_ref_time](boost::range::index_value<PointType&, long> pt)
       { return pt.value().timestamp; };
 
+  } else if (this->sensor == dlio::SensorType::LIVOX) {
+    point_time_cmp = [](const PointType& p1, const PointType& p2)
+      { return p1.timestamp < p2.timestamp; };
+    point_time_neq = [](boost::range::index_value<PointType&, long> p1,
+                        boost::range::index_value<PointType&, long> p2)
+      { return p1.value().timestamp != p2.value().timestamp; };
+    extract_point_time = [&sweep_ref_time](boost::range::index_value<PointType&, long> pt)
+      { return pt.value().timestamp * 1e-9f; };
   }
 
   // copy points into deskewed_scan_ in order of timestamp
@@ -643,8 +657,16 @@ void dlio::OdomNode::deskewPointcloud() {
   // extract timestamps from points and put them in their own list
   std::vector<double> timestamps;
   std::vector<int> unique_time_indices;
+
+  // compute offset between sweep reference time and first point timestamp
+  double offset = 0.0;
+  if (this->time_offset_) {
+    offset = sweep_ref_time - extract_point_time(*points_unique_timestamps.begin());
+  }
+
+  // build list of unique timestamps and indices of first point with each timestamp
   for (auto it = points_unique_timestamps.begin(); it != points_unique_timestamps.end(); it++) {
-    timestamps.push_back(extract_point_time(*it));
+    timestamps.push_back(extract_point_time(*it) + offset);
     unique_time_indices.push_back(it->index());
   }
   unique_time_indices.push_back(deskewed_scan_->points.size());
@@ -1905,6 +1927,11 @@ void dlio::OdomNode::debug() {
       << "Sensor Rates: Hesai @ " + to_string_with_precision(avg_lidar_rate, 2)
                                   + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz"
       << "|" << std::endl;
+  } else if (this->sensor == dlio::SensorType::LIVOX) {
+    std::cout << "| " << std::left << std::setfill(' ') << std::setw(66)
+      << "Sensor Rates: Livox @ " + to_string_with_precision(avg_lidar_rate, 2)
+                                  + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz"
+      << "|" << std::endl;
   } else {
     std::cout << "| " << std::left << std::setfill(' ') << std::setw(66)
       << "Sensor Rates: Unknown LiDAR @ " + to_string_with_precision(avg_lidar_rate, 2)