[Filters] PointCloud::reserve should be added in VoxelGridCovariance::getDisplayCloud

[KotaYonezawa]

Describe the bug

Thanks a lot for great library.
In VoxelGridCovariance::getDisplayCloud, very huge number of points are added to PointCloud by push_back function.
To improve performance, PointCloud::reserve should be added, I think.

Context

Following is current VoxelGridCovariance::getDisplayCloud loop part code. (in voxel_grid_covariance.hpp)

  // Generate points for each occupied voxel with sufficient points.
  for (typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
  {
    Leaf& leaf = it->second;

    if (leaf.nr_points >= min_points_per_voxel_)
    {
      cell_mean = leaf.mean_;
      llt_of_cov.compute (leaf.cov_);
      cholesky_decomp = llt_of_cov.matrixL ();

      // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
      for (int i = 0; i < pnt_per_cell; i++)
      {
        rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
        dist_point = cell_mean + cholesky_decomp * rand_point;
        cell_cloud.push_back (PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
      }
    }
  }

In this loop, many points are created and added to PointCloud instance (cell_cloud) using push_back function.
But on above code, many memory allocation will be occurred by push_back.
I think it is common technique that it is better to reserve memory before many push_back call.

  cell_cloud.reserve (pnt_per_cell * leaves_.size ()); // *** Should be Added ***

  // Generate points for each occupied voxel with sufficient points.
  for (typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
  {
    Leaf& leaf = it->second;

In addition, in our other environment using Ubuntu, ROS and Docker,
getDisplayCloud sometimes causes application crash.
Above modification seems to prevent it.
We are not sure why push_back call without reserve can cause application crash...
But anyway, adding reserve is basically correct, I think.

Expected behavior

Memory allocation in VoxelGridCovariance::getDisplayCloud can finish faster, without memory fragmentation.

Current Behavior

Memory allocation in VoxelGridCovariance::getDisplayCloud is not efficient. (time consuming, and memory can be fragmented)

Your Environment (please complete the following information):

• OS: Windows10
• Compiler: Compiler: Visual Studio 2019 Build Tools
• PCL Version: HEAD

[mvieth]

Yes, adding a reserve call is definitely a good idea. Ideally, we would not use leaves_.size(), but check, how many leaves actually pass the leaf.nr_points >= min_points_per_voxel_ test. I think something like this would work:
std::count_if(leaves_.begin(), leaves_.end(), [](auto& l){return (l.nr_points >= min_points_per_voxel_);}).
Would you open a pull request to add this? It would also be great if you could measure the time getDisplayCloud() takes with and without the reserve call, so we know how much faster it is with reserve.

[KotaYonezawa]

Thanks a lot for your comment!
I replaced my original proposed code:

  cell_cloud.reserve (pnt_per_cell * leaves_.size ()); // *** Should be Added ***

to your proposed code:

  cell_cloud.reserve (pnt_per_cell * std::count_if (leaves_.begin (), leaves_.end (), [this] (auto& l) { return (l.second.nr_points >= min_points_per_voxel_); }));

(In fact, I'm not so familiar with lambda function.. Is above code OK? It could run on my Windows PC...)

I simply measured time on my Windows 10 PC, using timeGetTime() function like as follows,

    DWORD start = timeGetTime ();
    ndt.getTargetGridPtr()->getDisplayCloud (*display_cloud);
    DWORD end = timeGetTime ();

using "box.pcd" attached on this article. (#4930)

Without this modification (ms)
Duration = 308
Duration = 308
Duration = 388
Duration = 303
Duration = 317
Duration = 329
Duration = 326
Duration = 304
Duration = 360
Duration = 318

After this modification (ms)
Duration = 266
Duration = 224
Duration = 247
Duration = 238
Duration = 218
Duration = 269
Duration = 221
Duration = 234
Duration = 221
Duration = 239

[mvieth]

About the lambda function: is it necessary to capture the whole object ([this])? Or would it be sufficient to only capture the one variable ([&min_points_per_voxel_])?
The measured durations look promising!
Would you create a pull request with your code changes?

[KotaYonezawa]

Thanks for your comment.
When I modified the code to [&min_points_per_voxel_], compile error was occurred on my PC.
(error C4573)
https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warnings-c4400-through-c4599?view=msvc-160

[KotaYonezawa]

According to internet information (https://cpplover.blogspot.com/2010/06/lambda_15.html (sorry, in Japanese))
min_points_per_voxel_ is VoxelGridCovariance class member variable and it cannot be captured on lambda expression...
So, if [this] is not preferred, we may need to add additional local variable to copy min_points_per_voxel_.

  auto min_points = min_points_per_voxel_;
  cell_cloud.reserve (pnt_per_cell * std::count_if (leaves_.begin (), leaves_.end (), [&min_points] (auto& l) { return (l.second.nr_points >= min_points); }));

I'm not so sure PCL coding rule... but is above code better than before?

[mvieth]

Ok, if [&min_points_per_voxel_] does not work, then let's use [this]. It is captured by reference (not copy), so that is fine. Thanks for the explanation!

[KotaYonezawa]

Thanks a lot for your comments!
I created a pull request.
#4938
Could you check it?