Organizing point cloud from HDL-32E

edit

TL;DR: Use the function I wrote here: organize_velodyne_cloud

I had to solve this problem recently in order to use the cloud for mesh creation with the Point Cloud Library (PCL). At the time I couldn't find anyone else who had solved this problem, so I had to write the code myself following the suggestions here from @joq. I've added to core functions to the repo linked above, as they're a bit too long to put here. Hopefully this will save anyone after me ~1-2 weeks effort.

How it works is this: every Velodyne LiDAR unit fires its lasers in a pre-defined sequence that repeats. E.g. 0, 16, 1, 17, 2, 18... But not every laser beam returns to the unit, so some parts of the cloud need to be filled in with points whose coordinates are NaN to preserve the organized nature of the cloud. My function creates a 2D organized cloud where there is one row for each "ring" of velodyne data. It then fills in each row, tracking how many points are in each row, i.e. what "column" of data is currently being filled in. When it moves from one column of data to the next, it fills in any gaps with NaN points.

Be careful how you use the organized cloud after this, as some PCL functions will crash if you input a cloud with NaNs, or worse, silently destroy the organized nature of the cloud without warning you.

An organized point cloud is just a point cloud where some information about the sensor that captured the "depth image" is given. E.g. for a kinect with a resolution of 640 x 480 pixels, the properties of an organized point cloud would be set to

width: 640
height 480
isOrganized = true.

Internally, the point cloud stores all 3D points in a vector that contains

point(0,0), point(1,0), ..., point(638, 0), point(639,0),
point(0,1), point(1,1), ..., point(638, 1), point(639,1),
...
point(0,479), point(1,479), ..., point(638, 479), point(639,479)

The important bit is that all measured points have to be stored in the point cloud. As soon as some (invalid, ...) points are filtered, the cloud is no longer organized.

So if you have a laser that e.g. provides 1.000 points per line and scans 500 lines per "picture" (?), you'd just push_back the data points into the point cloud in the order which they were acquired (starting with the first one of the first line, ending with the last on eof the last line). Then set width to 1.000, height to 500, and voila!

Disclaimer: I haven't worked with lasers much, so apologies for the poor terminology! The above of course only makes sense when there's no motion in the scene during the time all lines are captured...