降采样pcl::PCLPointCloud2时如何保留有序结构或重新组织点云？

Great question—this is a common gotcha when working with organized point clouds from depth cameras like the Kinect One. The issue with pcl::VoxelGrid is that it reorganizes points based on voxel clusters, which destroys the original row-column structure (hence the height=1 output). Here are two reliable approaches to solve this:

1. Structured Downsampling (Preserve Organized Structure Directly) #

Since your input is an organized point cloud (512x424, matching the Kinect's depth resolution), the simplest and most efficient fix is to downsample by skipping rows and columns. This maintains the ordered structure perfectly, which is ideal for subsequent normal estimation tools optimized for organized data (like IntegralImageNormalEstimation).

Code Example #

// Assume input_cloud is your original organized pcl::PCLPointCloud2::Ptr
pcl::PCLPointCloud2::Ptr downsampled_organized(new pcl::PCLPointCloud2);

// Set your downsampling step (e.g., step=2 reduces resolution by 75%)
int step = 2;
downsampled_organized->width = input_cloud->width / step;
downsampled_organized->height = input_cloud->height / step;
downsampled_organized->is_dense = input_cloud->is_dense;
downsampled_organized->fields = input_cloud->fields;
downsampled_organized->point_step = input_cloud->point_step;
downsampled_organized->row_step = downsampled_organized->width * downsampled_organized->point_step;

// Allocate memory for the downsampled cloud
size_t total_points = downsampled_organized->width * downsampled_organized->height;
downsampled_organized->data.resize(total_points * downsampled_organized->point_step);

// Copy points in a grid pattern
for (int y = 0; y < downsampled_organized->height; ++y) {
    for (int x = 0; x < downsampled_organized->width; ++x) {
        // Calculate index in the original cloud
        size_t original_idx = (y * step) * input_cloud->width + (x * step);
        // Copy the point data
        memcpy(
            &downsampled_organized->data[y * downsampled_organized->row_step + x * downsampled_organized->point_step],
            &input_cloud->data[original_idx * input_cloud->point_step],
            input_cloud->point_step
        );
    }
}

This outputs an organized cloud with height=424/step and width=512/step, ready for normal estimation tools that leverage ordered data for speed and accuracy.

2. Reorganize VoxelGrid Output (If You Need Voxel-Based Downsampling) #

If you specifically need the uniform point distribution that VoxelGrid provides (instead of simple row/column skipping), you can track each point's original row/column position before downsampling, then re-map the output back to an organized structure.

Step-by-Step Code Implementation #

First, add row/column metadata to your original cloud:

pcl::PCLPointCloud2::Ptr cloud_with_indices(new pcl::PCLPointCloud2);
pcl::copyPointCloud(*input_cloud, *cloud_with_indices);

// Add fields to store original row and column
pcl::PCLPointField row_field, col_field;
row_field.name = "row";
row_field.datatype = pcl::PCLPointField::UINT16;
row_field.count = 1;
row_field.offset = cloud_with_indices->point_step;

col_field.name = "col";
col_field.datatype = pcl::PCLPointField::UINT16;
col_field.count = 1;
col_field.offset = cloud_with_indices->point_step + sizeof(uint16_t);

cloud_with_indices->fields.push_back(row_field);
cloud_with_indices->fields.push_back(col_field);
cloud_with_indices->point_step += 2 * sizeof(uint16_t);
cloud_with_indices->row_step = cloud_with_indices->width * cloud_with_indices->point_step;

// Resize data and fill row/column values
cloud_with_indices->data.resize(cloud_with_indices->width * cloud_with_indices->height * cloud_with_indices->point_step);
for (int y = 0; y < input_cloud->height; ++y) {
    for (int x = 0; x < input_cloud->width; ++x) {
        size_t original_idx = y * input_cloud->width + x;
        size_t new_idx = y * cloud_with_indices->width + x;

        // Copy original point data
        memcpy(
            &cloud_with_indices->data[new_idx * cloud_with_indices->point_step],
            &input_cloud->data[original_idx * input_cloud->point_step],
            input_cloud->point_step
        );

        // Write row and column values
        uint16_t* row_ptr = reinterpret_cast<uint16_t*>(&cloud_with_indices->data[new_idx * cloud_with_indices->point_step + row_field.offset]);
        uint16_t* col_ptr = reinterpret_cast<uint16_t*>(&cloud_with_indices->data[new_idx * cloud_with_indices->point_step + col_field.offset]);
        *row_ptr = y;
        *col_ptr = x;
    }
}

Next, run VoxelGrid downsampling:

pcl::VoxelGrid<pcl::PCLPointCloud2> voxel_grid;
voxel_grid.setInputCloud(cloud_with_indices);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // Adjust leaf size to your needs
pcl::PCLPointCloud2::Ptr voxel_downsampled(new pcl::PCLPointCloud2);
voxel_grid.filter(*voxel_downsampled);

Finally, rebuild the organized cloud (fill missing positions with NaN):

pcl::PCLPointCloud2::Ptr organized_output(new pcl::PCLPointCloud2);
organized_output->width = input_cloud->width;
organized_output->height = input_cloud->height;
organized_output->fields = input_cloud->fields;
organized_output->point_step = input_cloud->point_step;
organized_output->row_step = organized_output->width * organized_output->point_step;
organized_output->is_dense = false; // Mark as non-dense since some points will be missing

// Initialize all points to NaN
organized_output->data.resize(organized_output->width * organized_output->height * organized_output->point_step);
std::fill(organized_output->data.begin(), organized_output->data.end(), 0);
for (size_t i = 0; i < organized_output->width * organized_output->height; ++i) {
    float* x = reinterpret_cast<float*>(&organized_output->data[i * organized_output->point_step]);
    float* y = reinterpret_cast<float*>(&organized_output->data[i * organized_output->point_step + 4]);
    float* z = reinterpret_cast<float*>(&organized_output->data[i * organized_output->point_step + 8]);
    *x = std::numeric_limits<float>::quiet_NaN();
    *y = std::numeric_limits<float>::quiet_NaN();
    *z = std::numeric_limits<float>::quiet_NaN();
}

// Map downsampled points back to their original row/column positions
for (size_t i = 0; i < voxel_downsampled->width; ++i) {
    uint16_t row = 0, col = 0;
    // Extract row and column from the metadata fields
    for (const auto& field : voxel_downsampled->fields) {
        if (field.name == "row") {
            memcpy(&row, &voxel_downsampled->data[i * voxel_downsampled->point_step + field.offset], sizeof(uint16_t));
        } else if (field.name == "col") {
            memcpy(&col, &voxel_downsampled->data[i * voxel_downsampled->point_step + field.offset], sizeof(uint16_t));
        }
    }
    // Copy the point to the organized cloud
    size_t output_idx = row * organized_output->width + col;
    memcpy(
        &organized_output->data[output_idx * organized_output->point_step],
        &voxel_downsampled->data[i * voxel_downsampled->point_step],
        input_cloud->point_step
    );
}

Note on Normal Estimation #

For organized point clouds, use pcl::IntegralImageNormalEstimation instead of the standard NormalEstimation. It’s optimized for ordered data, runs much faster, and doesn’t require setting a search radius—perfect for depth camera clouds.

内容的提问来源于stack exchange，提问作者Dhindhimathai