Incorrect concatenation of point clouds with differing point_step and row_step

generush commented 4 years ago

Hello,

Thank you for providing such a powerful ros package. Unfortunately, I believe I found an issue, and wanted to share it in case a solution is available.

Big picture, I am trying to concatenate three point clouds data streams from ROS (PointCloud2 topics), but they come from different types of sensors (1 x Ouster, 2 x Pico Flexx) and have different data types. My application is mobile robotics, and need a single point cloud in the base_link frame for mapping/planning algorithms, they do no accept multiple input clouds.

I attempted to use the PointCloudConcatenateDataSynchronizer nodelet, which is very handy when the two point clouds are of the same data type. I start a nodelet manager along with launch my two Pico Flexx, make sure the necessary transforms between the sensors and the base_link are set up. Then I run this:

<launch>
  <node name="point_cloud_concat"
        pkg="nodelet" type="nodelet"
        args="load pcl/PointCloudConcatenateDataSynchronizer $(arg MANAGER)"
        output="screen">
    <rosparam>
      approximate_sync: true
      queue_size: 20
      output_frame: base_link
      input_topics:
        - /os1_cloud_node/points
        - /up/up_camera/stream/1/cloud
        - /down/down_camera/stream/1/cloud
    </rosparam>
  </node>

</launch>

When I only try merging the two Pico Flexx, it works fine, I can see the concatenated point cloud in RViz, all is well. BUT, when I try to also merge the Ouster, the nodelet outputs a gibberish point cloud. I know that the point cloud is not merged properly because I cannot visualize it in RViz (see the error below), and I even tried to visualize it in the pcl_viewer/print values a pcd_reader script. The points are far from correct varying from 1e-50 to 1e50 see here--> test_pcd.txt, so it is clear that the data parsing is incorrect.

RViz Error

I believe what is happening is the Ouster has a different point_step than the Pico Flexx, and therefore, the concatenation is done incorrectly. Based off of the information below, when I "rostopic echo --noarr" the three input sensor topics, and the concatenated output topic, I observe:

The height, width, and data length of the concatenated cloud appears correct (it is the summation of the three input cloud)
_BUT, the pointstep is 48 and row step is 49152 for the Ouster, which is larger than the point step of 32 and row step of 7168 for the Pico Flexx. _The concatenated cloud has the same point and row step as the Ouster, which I believe must be incorrect and is causing the parsing of the point cloud to be incorrect._

I believe this has to do with the root cause of the issue. I look forward to a proposed solution, since this would be such a convenient and efficient way to merge asynchronous point clouds from multiple sources!

Best,

Gene

OUSTER:

header: seq: 2704 stamp: secs: 1580537344 nsecs: 119789258 frame_id: "os1_lidar" height: 64 width: 1024 fields: "<array type: sensor_msgs/PointField, length: 4>" is_bigendian: False point_step: 48 row_step: 49152 data: "<array type: uint8, length: 3145728>" is_dense: True

Pico Flexx 1:

header: seq: 0 stamp: secs: 1580537314 nsecs: 187153000 frame_id: "up_camera_link" height: 171 width: 224 fields: "<array type: sensor_msgs/PointField, length: 4>" is_bigendian: False point_step: 32 row_step: 7168 data: "<array type: uint8, length: 1225728>" is_dense: True

Pico Flexx 2:

header: seq: 0 stamp: secs: 1580537314 nsecs: 187153000 frame_id: "up_camera_link" height: 171 width: 224 fields: "<array type: sensor_msgs/PointField, length: 4>" is_bigendian: False point_step: 32 row_step: 7168 data: "<array type: uint8, length: 1225728>" is_dense: True

CONCATENATED PCL:

header: seq: 1459 stamp: secs: 1580537355 nsecs: 419741839 frame_id: "base_link" height: 1 width: 142144 fields: "<array type: sensor_msgs/PointField, length: 4>" is_bigendian: False point_step: 48 row_step: 49152 data: "<array type: uint8, length: 5597184>" is_dense: True

SteveMacenski commented 4 years ago

PRs are appreciated. That sounds like a relatively straight forward fix if that is the issue.

I do happen to have both of those sensors available to me, but I won’t have time to address this in the immediate future myself. I can help gather data or answer questions if that helps you in resolving the issue

generush commented 4 years ago

Thanks for responding so fast. Do you have any suggestions for solving? I do not now how to solve it, I just discovered the problem when I realized I couldn't do what I wanted. Since I needed a solution in the short term, I just made my own custom solution (https://github.com/erush91/pcl_merge.git), but I know that it uses significantly more compute than the pcl_ros library. Let me know if you figure out the solution, I would be interested!

ros-perception / perception_pcl