Pcl: Voxel Grid Rewrite Ideas

[SergioRAgostinho]

A follow-up on the issues raised in https://github.com/PointCloudLibrary/pcl/pull/2171#issuecomment-364926779 .

Ideas

• Start boilerplating on pcl::experimental::VoxelGrid so that we can spread the work over multiple PRs without breaking current functionalities.
• Does it still make sense to call it a grid or even voxel for that matter? VoxelFilter?
• Drop the PointCloud2 support. Use the conversion functions during the deprecation period.

The swappable partitioning strategy seems like something that makes sense at compile time. Since different coordinate systems in the partitioning imply different parameters and therefore the corresponding getters and setters, this immediately invokes templated multiple inheritance in my head. Something along the lines of

class Partition1D
{
    public:

        virtual size_t computeLinearIndex (const float coord) const = 0;
};

class Cartesian1DPartitioning : public Partition1D
{
    public:

        void setMin (const float min) { min_ = min; }

        float getMin () const { return min_; }

        size_t computeLinearIndex (const float coord) const
        {
            // return something meaningful
            return 3.f;
        }

    protected:

        float min_;
        float max_;
        float division_;
};


// Whatever is responsible for receiving the list of points in a voxel and applying some decimation action over it
class SomeVoxelMethod
{

};

template<typename PartitionT, typename VoxelMethodT>
class VoxelFilter : public PartitionT, public VoxelMethodT
{
};

typedef VoxelFilter<Cartesian1DPartitioning, SomeVoxelMethod> VoxelGrid;

See it running here.

[sddavis14]

I can provide an initial implementation of this system with the Cartesian and spherical partitioning methods. It will take me a few weeks to complete this. What is the process for developing an experimental feature like this? Are the documentation/unit testing requirements loosened for a feature in this stage?

[SergioRAgostinho]

I can provide an initial implementation of this system with the Cartesian and spherical partitioning methods. It will take me a few weeks to complete this.

Before you embark on this journey I would like to discuss here a coarse class design.

I know it's impossible to predict everything in advance and there will be of course changes throughout the development, but I would like to have "everyone" coming up with a consensus on the approach to take.

What is the process for developing an experimental feature like this? Are the documentation/unit testing requirements loosened for a feature in this stage?

There will be 4 VoxelGrid classes, 2 of these you're already pretty familiar with but the other 2 will definitely require more work. Additionally, the amount of code that will be written requires that we split things over multiple PRs.

In order to make your development experience easier while taking into account the points I mentioned above, it's definitely better if you start from a situation where you can freely code without "fear" of breaking the current implementations and hence my suggestion of using the experimental namespace.

This namespace was something we recently introduced when we deprecated some things in the Euclidean Cluster Comparator class and the idea is to use it as an "intermediate stage" in which your classes can reside before they're "swapped" to public scope under the normal pcl namespace.

The work flow I suggest for the current endeavor:

1. double check unit test coverage for the voxel grid class and add whatever we feel it's missing. We need to cover all public methods the class exposes.
2. dependent on the final design we decide upon, start writing the new partitioning, filtering classes plus the actual filter class which aggregates both, everything in the experimental namespace.
3. apply the same VoxelGrid unit tests to the new class in the experimental namespace and make the appropriate modification until the tests run green.
4. at this point the aim is to perform the swap between pcl::experimental::VoxelGrid -> pcl::VoxelGrid. Since pcl::VoxelGridCovariance and pcl::VoxelGridOcclusionEstimation inherit from pcl::VoxelGrid this swapping might not be clean, especially because unit tests just cover public methods. For this reason the following namespace moving might also be required pcl::VoxelGrid - > pcl::deprecated::VoxelGrid. This gives you a little more room to then work individually on the new implementations of pcl::VoxelGridCovariance and pcl::VoxelGridOcclusionEstimation.
5. For each of these classes, then repeat steps 1. to 4.
6. At the end of this, everything which is on the deprecated namespace is to be deleted.

Not sure if there's anything else to add from @PointCloudLibrary/maintainers .

[jefft255]

Hi,

Something I would appreciate, and other working on large point clouds (terrestrial laser scanning and mobile laser scanning for example), is supporting small leaf sizes on large point clouds. See discussion in #585 where a possible approach is discussed.

[sddavis14]

Here is a more fleshed out class design: http://cpp.sh/7sxhq

All of the base classes need to be templated on the point type. I'm using template template parameters to propagate the chosen point type to VoxelStructure and PointMergeMethod. Let me know if you guys see any issues with this.

[sddavis14]

Some performance benchmarks:

Old sort method

Input size: 8592880
Output size: 21520
Time: 416ms

Input size: 8592880
Output size: 92046
Time: 484ms

Input size: 8592880
Output size: 331596
Time: 550ms

Input size: 85928800
Output size: 211960
Time: 4380ms

Input size: 85928800
Output size: 912216
Time: 5304ms

Input size: 85928800
Output size: 3295116
Time: 6283ms

unordered_map method

Input size: 8592880
Output size: 21520
Time: 192ms

Input size: 8592880
Output size: 92046
Time: 206ms

Input size: 8592880
Output size: 331596
Time: 274ms

Input size: 85928800
Output size: 211960
Time: 1904ms

Input size: 85928800
Output size: 912216
Time: 2242ms

Input size: 85928800
Output size: 3295116
Time: 2809ms

Source for the new method:

pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
{
  // Has the input dataset been set already?
  if (!input_)
  {
    PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  // Copy the header (and thus the frame_id) + allocate enough space for points
  output.height       = 1;                    // downsampling breaks the organized structure
  output.is_dense     = true;                 // we filter out invalid points

  Eigen::Vector4f min_p, max_p;
  // Get the minimum and maximum dimensions
  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
    getMinMax3D<PointT> (input_, *indices_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
  else
    getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);

  // Check that the leaf size is not too small, given the size of the data
  uint64_t dx = static_cast<uint64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
  uint64_t dy = static_cast<uint64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
  uint64_t dz = static_cast<uint64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;

  if ((dx*dy*dz) > static_cast<uint64_t>(std::numeric_limits<int>::max()))
  {
    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
    output = *input_;
    return;
  }

  // Compute the minimum and maximum bounding box values
  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
  max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));

  // Compute the number of divisions needed along all axis
  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
  div_b_[3] = 0;

  // Set up the division multiplier
  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);

  // Storage for mapping leaf and pointcloud indexes

  std::unordered_map<int, pcl::CentroidPoint<PointT>> voxelMap;

  // First pass: go over all points and insert them into the index_vector vector
  // with calculated idx. Points with the same idx value will contribute to the
  // same point of resulting CloudPoint
  for (int i = 0; i < input_->points.size(); i++)
  {
    if (!input_->is_dense)
      // Check if the point is invalid
      if (!pcl_isfinite (input_->points[i].x) ||
          !pcl_isfinite (input_->points[i].y) ||
          !pcl_isfinite (input_->points[i].z))
        continue;

    int ijk0 = static_cast<int> (floor (input_->points[i].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
    int ijk1 = static_cast<int> (floor (input_->points[i].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
    int ijk2 = static_cast<int> (floor (input_->points[i].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));

    // Compute the centroid leaf index
    int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];

    voxelMap[idx].add(input_->points[i]);
  }

  for (auto iter = std::begin(voxelMap); iter != std::end(voxelMap); iter++)
  {
    PointT point;
    iter->second.get(point);
    output.push_back(point);
  }

  output.width = static_cast<uint32_t> (output.points.size ());
}

This removes some of the old functionality (filtering, etc..)

[SergioRAgostinho]

Here is a more fleshed out class design: http://cpp.sh/7sxhq

All of the base classes need to be templated on the point type. I'm using template template parameters to propagate the chosen point type to VoxelStructure and PointMergeMethod. Let me know if you guys see any issues with this.

So far looks pretty cool 👍

I just assembled your results on a table

| Input size / Output Size | Old sort method | unordered_map method |
|-------------------------|----------------------|--------------------------------|
| 8592880 / 21520 | 416ms | 192ms |
| 8592880 / 92046 | 484ms | 206ms |
| 8592880 / 331596 | 550ms | 274ms |
| 85928800 / 211960 | 4380ms | 1904ms |
| 85928800 / 912216 | 5304ms | 2242ms |
| 85928800 / 3295116 | 6283ms | 2809ms |

They look very promising. I have to say I'm a little intrigued at the results between row 3 and 4. They are definitely counter intuitive. Why is the time increasing so suddenly?

The applyFilter method is also looking clean.

(Sorry for the absence.)

[sddavis14]

Thanks for the feedback. I will continue working towards a pull request as I have time.

They look very promising. I have to say I'm a little intrigued at the results between row 3 and 4. They are definitely counter intuitive. Why is the time increasing so suddenly?

If you look back at my results you'll notice that the number of input points is increasing by an order of magnitude for those last 3 tests (85928800 points). I probably should have denoted that I was also varying the input point cloud size.

[SergioRAgostinho]

Apologies. I totally missed. Do you have the code online somewhere I can keep track or is everything local for now?

[sddavis14]

No problem at all. Things are currently local. However, I can open a pull request and we could work from there if preferable.

[SergioRAgostinho]

It's too early for that. I would suggest we work on a specific branch of your fork. I would like to chime in and actively help on this if you don't mind.

[sddavis14]

Sure, I'll push what I have to a branch on my fork in the next few days. I'll update you here when that is complete. Your help would be much appreciated.

[sddavis14]

@SergioRAgostinho I apologize for the delay on this - my fork and branch is now available at https://github.com/sddavis14/pcl/tree/new-voxel-filter

This contains the following changes:

• New voxel_filter files (not completely structured correctly) along with CMakeLists changes
• unordered_map implementation of original voxel_grid filter (commented out in voxel_grid.hpp)
• Change to compile with C++14 (C++11 should also work) in main CMakeLists

Note that the voxel_filter files will probably cause compilation to fail right now. I could use some help structuring the classes according to PCL standards. I'm not exactly sure how to manage the use of template template parameters.

Thanks for the help!

[kunaltyagi]

Has this been merged? I assume no since no new work has been done on the #2171 thread.

[taketwo]

The effort died down, as it often happens...

[sddavis14]

I would love to see this get fully implemented and merged, but my time constraints make this difficult as the sole developer. If anyone in the PCL community would like to jump in and provide some assistance, we can probably revive this and get an initial version merged relatively quickly.

[kunaltyagi]

VoxelGrid is giving a lot of issues to me personally. I might be able to help on this

Right now, there are a few PR on this, and the situation seems messy. If someone can provide me with an overview, I might get started on this slowly, but surely.

[sddavis14]

@kunaltyagi

We want to modularize the filter so that we can interchange partitioning and decimation methods. This way we can downsample using different coordinate systems such as spherical, log-polar, etc without writing an entirely new filter.

Can you reference the PRs? Since this will be a rewrite of the filter we should start out by considering some of the existing issues. My reason for starting this was needing different coordinate systems, but I know other issues such as overflowing the leaf indices need to be addressed.

[kunaltyagi]

In reverse chronological order (grouped by issue type):

• Small index size: #3260 (almost a PR), #585 and others closed
• Inconsistency (small ones, can skip them): #2527, #2525
• Related to self: #2211, #2044, #2171 (PR)

we can interchange partitioning and decimation methods

In this case, is SuperVoxel a special case of VoxelGrid?

[sddavis14]

Thanks @kunaltyagi

With regards to the inconsistencies, my opinion is that pcl::VoxelGrid should never have included the ability to pre-filter points. If that is desired, pcl::PassThrough should be used, then pcl::VoxelGrid. So agreed, I think it's a non-issue.

Clearly, the index error needs to be fixed. Seems like we may be able to just use a 64-bit unsigned integer instead of a 32-bit signed integer to prevent overflow.

In this case, is SuperVoxel a special case of VoxelGrid?

I think that SuperVoxel is unrelated to VoxelGrid, even in this case. VoxelGrid is a filter while (to my limited understanding) SuperVoxel is a segmentation algorithm. With our updated design, the filter will just be downsampling with better control over the downsampling method.

[kunaltyagi]

Great. I've groked existing code of VoxelGrid. Where should I begin?

[stale[bot]]

Marking this as stale due to 30 days of inactivity. It will be closed in 7 days if no further activity occurs.