Merge pull request #27 from ethz-asl/feature/destagger

Feature/destagger

README.md

@@ -61,7 +61,7 @@ This package was developed on Ubuntu 20.04 using ROS Noetic. Other versions shou
     ```
 4. Build COIN-LIO:
     ```bash
-    catkin build coin_lio
+    catkin build coin_lio --force-cmake
     ```
 
 ## Alternative Installation: Docker
@@ -104,8 +104,11 @@ If you are not using the built-in IMU in the Ouster LiDAR, you need to adapt the
 ### Run COIN-LIO with your own data
 Launch with settings for your data:
   ```bash
-  roslaunch coin_lio mapping.launch metadata_file:=<metadata_path.json> column_shift:=<parameter from calibration> point_topic:=<pointcloud_topic> imu_topic:=<imu_topic>
+  roslaunch coin_lio mapping.launch metadata_file:=<metadata_path.json> column_shift:=<parameter from calibration> point_topic:=<pointcloud_topic> imu_topic:=<imu_topic> destagger:=<true/false>
   ```
+  If your data already contains [destaggered point clouds from the ouster driver](https://github.com/ouster-lidar/ouster-ros/blob/55519ed2b8a7dd7d4ae13a968b0ec88e5cada7dd/launch/common.launch#L45), set `destagger:=false`, otherwise use `destagger:=true`.
+
+
 Play your data:
   ```bash
   rosbag play <bag_path.bag>

include/projector.h

@@ -34,6 +34,7 @@ class Projector {
     std::vector<int> idx_to_v_;
     std::vector<int> idx_to_u_;
     M3D K_;
+    bool destagger_;
 };
 
 #endif  // COIN_LIO_PROJECTOR_H_

launch/mapping.launch

@@ -1,6 +1,7 @@
 <launch>
     <arg name="rviz" default="true" />
     <arg name="bag_file" default="" />
+    <arg name="destagger" default="true" />
     <arg name="metadata_file"/>
     <arg name="column_shift"/>
     <arg name="point_topic"/>
@@ -13,6 +14,7 @@
     <param name="common/lid_topic" type="string" value="$(arg point_topic)"/>
     <param name="common/imu_topic" type="string" value="$(arg imu_topic)"/>
     <param name="image/u_shift" type="int" value="$(arg column_shift)"/>
+    <param name="image/destagger" type="bool" value="$(arg destagger)"/>
 
     <node pkg="coin_lio" type="coin_lio_mapping" name="laserMapping" output="screen" launch-prefix=""/>
 

src/preprocess.cpp

@@ -23,7 +23,7 @@ void Preprocess::ouster_handler(const sensor_msgs::PointCloud2::ConstPtr &msg, P
   pcl::uint64_t max_time = 0;
   for (size_t i = 0; i < pl_orig.points.size(); i++)
   {
-
+    if(isnan(pl_orig.points[i].x) || isnan(pl_orig.points[i].y) || isnan(pl_orig.points[i].z)) continue;
     double range = pl_orig.points[i].getVector3fMap().norm();
 
     if (range < blind) continue;

src/projector.cpp

@@ -90,6 +90,10 @@ void Projector::loadParameters(ros::NodeHandle nh) {
         return;
     }
     u_shift_ = static_cast<int>(u_shift);
+
+    // true: destagger the pointcloud data 
+    // https://static.ouster.dev/sdk-docs/reference/lidar-scan.html#staggering-and-destaggering
+    nh.param<bool>("image/destagger", destagger_, true);
 }
 
 size_t Projector::vectorIndexFromRowCol(const size_t row, const size_t col) const {
@@ -150,7 +154,7 @@ void Projector::createImages(LidarFrame& frame) const {
 
 size_t Projector::indexFromPixel(const V2D& px) const {
     const int vv = (int(px(1)) + cols_ - offset_lut_[int(px(0))]) % cols_;
-    const size_t index = px(0)* cols_ + vv;
+    const size_t index = px(0)* cols_ + (destagger_ ? vv : int(px(1)));
     return index;
 };