c++loopspoint-cloud-librarypoint-cloudspcl

PCL: Populate Organized PointCloud by Index


I have worked with PCL for a few days now, but cannot get around one Problem: I got a dense, organized PointCloud<PointT> cloud_1 and want to populate a second, new PointCoud PointCloud<PointT> cloud_2 with processed Points. So my idea was (in pseudocode, but of course I can provide MWE if it helps):

//cloud_1 is also a Ptr that gets a Cloud loaded from PCD File
PointCloud<PointT>::Ptr cloud_2(new PointCloud<PointT>)

void populateSecondCoud(PointCloud<PointT>& cloud_1, PointCloud<PointT>& cloud_2){
  cloud_2.width = cloud_1.width;
  cloud_2.height = cloud_1.height;

  for (i in cloud_1.height){
    for(j in cloud_1.width){
      PointT p = cloud_1.at(i,j);
      // do processing with the point...
      cloud_2.at(i,j) = p
    }
  }
}
populateSecondCloud(*cloud_1, *cloud_2)

This terminates with :

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 0) >= this->size() (which is 0)

I guess, because the points-vector of cloud_2 is still completely empty. Is there any way to iteratively populate an organized PointCloud?

All of that happens for a lot of PointClouds, that is why I try to prevent copying cloud_2 from cloud_1 before processing the points.

Any ideas are greatly appreciated. Of course I can provide a compiling code snippet, but I thought the problem gets clear from above pseudocode.

Edit: Clarified how cloud_2 is initialized.


Solution

  • There are 2 issues in your code:

    1. Memory allocation:
    You need to allocate cloud_2 with the proper size.
    There is a pcl::PointCloud constructor that accepts the width and height and allocates the data accordingly, e.g.:

    PointCloud<PointT>::Ptr cloud_2 = PointCloud<PointT>::Ptr cloud(
                   new PointCloud<PointT>(cloud_1.width, cloud_1.height));
    

    You can also use the pcl::PointCloud::resize method to resize cloud_2 with new width and height inside populateSecondCoud:

    cloud_2.resize(cloud_1.width, cloud_1.height);
    

    2. Proper indexing:
    As you can see in the pcl::PointCloud::at documentation,
    the arguments for at are column, row (in that order).
    You actually pass them in the reverse order, because your i in the row index and j the column index.

    Therefore change lines containing:

    at(i, j)
    

    To:

    at(j, i)