Staggered/destaggered LidarScan vs unorganized/organized point clouds?

[vebjornjr]

Is what you call a destaggered representation the same as an organized point cloud?

www.mathworks.com/help/lidar/ug/organized-and-unorganized-point-clouds.html

[vebjornjr]

And can you confirm if this is the correct way to produce a pcl::PointCloud<pcl::PointXYZI> from an ouster::LidarScan? I removed some of our application specific code. I'm unsure on the indexing and if the reflectivity values are connected to the correct xyz values, and if the LidarScan object must created fresh after each complete scan?

while (!stop.load()) {
    // Raw packet data buffer
    array<uint8_t, udpPacketBufferSize> packetBuffer{};

    // Wait on sensor data
    ouster::sensor::client_state state = ouster::sensor::poll_client(*sensor);

    if (state & ouster::sensor::CLIENT_ERROR) {
        // Exceptions
    }

    if (state & ouster::sensor::IMU_DATA) {
        // Ignore for now
    }

    if (state & ouster::sensor::LIDAR_DATA) {
        if (!ouster::sensor::read_lidar_packet(*sensor, packetBuffer.data(), packetFormat)) {
            // Error handling
        }

        // Batcher returns true on complete scan
        if (batcher(packetBuffer.data(), scan)) {
            // Check validity of measurements
            const auto invalids = count_if(scan.status().begin(), scan.status().end(), [](uint32_t s) {
                return !(s & 0x01);
            });

            // Check if scan is valid (accounting for azimuth window)
            if (invalids > (W - columnWindowSize)) {
                continue;
            }

            // We have a valid scan
            ouster::LidarScan::Points points = ouster::cartesian(scan, lookupTable);
            const auto reflectivity = scan.field<uint32_t>(ouster::sensor::ChanField::REFLECTIVITY);

            // Create PCL point cloud
            // Staggered representation - each column is a measurement (timestamp)
            pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
            cloud->width = W;
            cloud->height = H;
            cloud->resize(W * H);
            for (int u = 0; u < H; u++) {
                for (int v = 0; v < W; v++) {
                    const auto xyz = points.row(u * W + v);
                    cloud->at(v, u) = pcl::PointXYZI(xyz(0), xyz(1), xyz(2), reflectivity(u, v)); // TODO: Reflectivity indexing opposite of cloud indexing?
                }
            }

            // Publish the point cloud to other code
            // [...]

            // Reset scan (TODO: needed?)
            scan = ouster::LidarScan(W, H, info.format.udp_profile_lidar);
        }
    }
}

[kairenw]

Hi @vebjornjr,

It's better to think of both staggered and destaggered representations as being organized, in that they both represent the data in a HxW array where the columns represent either values that share the same timestamp ("staggeerd") or values that share the same azimuth angle ("destaggered"). I'm not sure if you've seen this, but I do think these pictures here in our documentation of staggered and destaggered range images are probably the easiest way to understand the difference. While those pictures depict the range image, I think it's clear that if one were to have a HxW point cloud, it could similarly be staggered or destaggered, and these would both be organized in concept (although only destaggered would match the traditional understanding of 'organization').

Regarding the code --

On reflectivity indexing: Yeah, the PCL PointCloud constructor takes (W,H) in that order; and cloud(col, row). So you've got it exactly right there. Are you seeing something weird when you write it that way?

On resetting scan: You don't need to reset scan! the ScanBatcher will zero out everything you need so you shouldn't get contamination between frames.

Other notes: So, I notice that you're doing that point conversion right in the polling loop. I'd recommend against doing this, or if you do, make sure you test to see if you're dropping packets. Eventually there's too much calculation in the loop. There's a few things you can do to avoid this. You can manage your own packet buffers; you can use two scans and swap them as needed; or if you're on ROS, you can make sure your queues are large enough there.

[vebjornjr]

Thank you, that image really explained it well. I think I understand it now. I will take a look at how we might move the calculation away from the polling loop.