Filter out points below/above a certain range

[lio94]

Is there currently a convenient way to filter points below certain threshold, in a similar manner than the min_range parameter in velodyne_driver?

The sensor is often positioned so that the laser beams hit parts of the vehicle itself. This might disturb odometry/localization/SLAM algorithms, which think that these static points are part of the observed environment. This calls for having a functionality that could neglect all points that are e.g. at less than 3 meters distance from the sensor frame.

Implementing filters on the PointCloud2 topic could be a hassle in terms of getting them work at a reasonable performance, so I think it would preferably need to be implemented in the driver, closer to the original data format, not on the xyz pointcloud ouput.

[Samahu]

I agree, it would be much faster to filter-out the points directly at the time of composing the PointCloud2 message that applying filters later on on the composed message.

[thorstink]

FYI currently I simply hack it like this:

void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls,
                        std::chrono::nanoseconds scan_ts, const PointT& points,
                        const ouster::img_t<RangeT>& range,
                        const ouster::img_t<ReflectivityT>& reflectivity,
                        const ouster::img_t<NearIrT>& near_ir,
                        const ouster::img_t<SignalT>& signal) {
    auto timestamp = ls.timestamp();
    const auto rg = range.data();
    const auto rf = reflectivity.data();
    const auto nr = near_ir.data();
    const auto sg = signal.data();
    const auto t_zero = std::chrono::duration<long int, std::nano>::zero();
    cloud.clear(); // new
#ifdef __OUSTER_UTILIZE_OPENMP__
#pragma omp parallel for collapse(2)
#endif
    for (auto u = 0; u < ls.h; u++) {
        for (auto v = 0; v < ls.w; v++) {
            const auto col_ts = std::chrono::nanoseconds(timestamp[v]);
            const auto ts = col_ts > scan_ts ? col_ts - scan_ts : t_zero;
            const auto idx = u * ls.w + v;
            const auto xyz = points.row(idx);
            if (static_cast<uint32_t>(rg[idx]) > 1200) // new, if >1200mm add point
            {
                cloud.points.push_back(ouster_ros::Point{
                {static_cast<float>(xyz(0)), static_cast<float>(xyz(1)),
                 static_cast<float>(xyz(2)), 1.0f},
                static_cast<float>(sg[idx]),
                static_cast<uint32_t>(ts.count()),
                static_cast<uint16_t>(rf[idx]),
                static_cast<uint16_t>(u),
                static_cast<uint16_t>(nr[idx]),
                static_cast<uint32_t>(rg[idx]),
            });
        }
        }
    }
}

and comment out the check:

    // assert(cloud.width == static_cast<std::uint32_t>(ls.w) &&
    //        cloud.height == static_cast<std::uint32_t>(ls.h) &&
    //        "point cloud and lidar scan size mismatch");

• as far as I can tell, cloud is a pcl::PointCloud and clear only resets the data and sizes, but does not release the memory.
• I do not know how this affects the images. I only use the pointclouds.

[thorstink]

for the record, this approach does not work anymore with the destaggered version.

[Samahu]

@thorstink The copy_scan_to_cloud was replaced by copy_scan_to_cloud_destaggered method. So you may need to modify the new method to achieve the same results.

[thorstink]

@Samahu I tried applying the same approach to the new method briefly, but for some reason it gave me weird results (it would run, but the pointcloud would be empty or something)...

I couldn't pin-point why, I also thought it would just work, but I probably messed something up. For now I'm sticking to an old commit and will revisit once I have some cycles to look at it again. Would you be interested in a pull-request at some point in time? Or do you prefer to do it yourself/not do it at all?

[Samahu]

@thorstink I might be able to pinpoint the problem if you share your updated code.

I don't recommend that you submit a pull request since this part is being actively developed.