实时拼接RealSense D415点云的两大技术问题求助

一、重叠区域重复点去除方案 #

既然Voxel下采样不适用你的场景，推荐以下两种精准去重方式：

1. PCL内置坐标去重函数 #

使用pcl::removeDuplicatePoints，基于点的空间坐标（可包含RGB信息）设置距离阈值，将小于阈值的点判定为重复点过滤。代码示例：

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_no_duplicates(new pcl::PointCloud<pcl::PointXYZRGB>);
// 0.001米阈值，可根据实际精度需求调整
pcl::removeDuplicatePoints(*Final, *cloud_no_duplicates, 0.001); 
*Final = *cloud_no_duplicates;

2. KD-Tree近邻过滤（针对重叠区精准筛选） #

利用ICP得到的变换矩阵，通过KD-Tree搜索第二个点云中与第一个点云距离过近的点，只保留非重叠部分：

pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
kdtree.setInputCloud(cloud_in);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);

float threshold = 0.001; // 距离阈值，单位米
for (const auto& point : cloud2_in->points) {
    std::vector<int> indices;
    std::vector<float> distances;
    if (kdtree.nearestKSearch(point, 1, indices, distances) == 1) {
        if (distances[0] > threshold) {
            cloud2_filtered->points.push_back(point);
        }
    } else {
        cloud2_filtered->points.push_back(point);
    }
}
// 合并去重后的点云
*Final = *cloud_in + *cloud2_filtered;
// 保留原有序结构属性
cloud2_filtered->width = cloud2_in->width;
cloud2_filtered->height = cloud2_in->height;
cloud2_filtered->is_dense = cloud2_in->is_dense;

二、有序点云生成实现 #

有序点云的核心是保持width×height的网格结构，每个点对应相机图像的像素位置。你当前直接相加width并resize的方式会破坏原有有序结构，正确实现方式如下：

1. 前提校验 #

先确认输入的两个点云都是有序的（RealSense D415默认输出有序点云，isOrganized()返回true），否则无法生成有效有序拼接结果。

2. 有序点云拼接（以左右拼接为例） #

假设两个相机为左右摆放，拼接后高度取两者最大值（若分辨率一致则取相同值），宽度为两者宽度之和，逐个填充点并处理无效区域：

if (!cloud_in->isOrganized() || !cloud2_in->isOrganized()) {
    NODELET_WARN("Input clouds are not organized, cannot generate organized output");
    return;
}

// 确定拼接后尺寸
int final_height = std::max(cloud_in->height, cloud2_in->height);
int final_width = cloud_in->width + cloud2_in->width;

// 初始化有序点云
Final->height = final_height;
Final->width = final_width;
Final->is_dense = false; // 边缘存在无效点，需设为false
Final->points.resize(final_height * final_width);
Final->header = cloud_in->header;

// 填充第一个点云的像素点
for (int h = 0; h < cloud_in->height; ++h) {
    memcpy(&Final->points[h * final_width], &cloud_in->points[h * cloud_in->width], cloud_in->width * sizeof(pcl::PointXYZRGB));
    // 填充右侧无效点（NaN）
    for (int w = cloud_in->width; w < final_width; ++w) {
        auto& p = Final->points[h * final_width + w];
        p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
    }
}

// 填充第二个点云的像素点
for (int h = 0; h < cloud2_in->height; ++h) {
    memcpy(&Final->points[h * final_width + cloud_in->width], &cloud2_in->points[h * cloud2_in->width], cloud2_in->width * sizeof(pcl::PointXYZRGB));
}

// 填充下方无效点（NaN）
for (int h = cloud_in->height; h < final_height; ++h) {
    for (int w = 0; w < final_width; ++w) {
        auto& p = Final->points[h * final_width + w];
        p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
    }
}

3. 注意事项 #

• 若两个相机分辨率不同，需先将其中一个点云重采样至相同分辨率（使用pcl::resize），再进行拼接，否则有序结构无实际意义。
• 有序点云的is_dense必须设为false，因为拼接边缘必然存在无效点。

修改后的核心代码片段 #

将去重与有序拼接整合到你的原有逻辑中：

if (cloud1RW_ != 0 && cloud2RW_ != 0) {
    // Transformation matrix for cloud 2 to align with cloud 1 (obtained via PCL ICP)
    static Eigen::Matrix4f transformationMatrix = (Eigen::Matrix4f() << 0.999949,0.0058447,0.00823151,-0.0132738,-0.00563437,0.999663,-0.025349,0.360606,-0.0083768,0.0253014,0.999645,0.0215044,0,0,0,1).finished();
    pcl::transformPointCloud( *cloud2_in, *cloud2_in, transformationMatrix);

    if ((cloud1RW_->width != 0) && (cloud2RW_->width != 0)) { 
        // --- 第一步：去除重叠区域重复点 ---
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
        kdtree.setInputCloud(cloud_in);
        float dist_threshold = 0.001; // 1mm阈值，可调整

        for (const auto& p : cloud2_in->points) {
            std::vector<int> nn_idx;
            std::vector<float> nn_dist;
            if (kdtree.nearestKSearch(p, 1, nn_idx, nn_dist) == 1 && nn_dist[0] <= dist_threshold) {
                continue; // 跳过重复点
            }
            cloud2_filtered->points.push_back(p);
        }
        cloud2_filtered->width = cloud2_in->width;
        cloud2_filtered->height = cloud2_in->height;
        cloud2_filtered->is_dense = cloud2_in->is_dense;

        // --- 第二步：生成有序点云 ---
        if (!cloud_in->isOrganized() || !cloud2_filtered->isOrganized()) {
            NODELET_WARN("Input clouds are not organized, falling back to unorganized merge");
            *Final = *cloud_in + *cloud2_filtered;
        } else {
            int final_h = std::max(cloud_in->height, cloud2_filtered->height);
            int final_w = cloud_in->width + cloud2_filtered->width;

            Final->height = final_h;
            Final->width = final_w;
            Final->is_dense = false;
            Final->points.resize(final_h * final_w);
            Final->header = cloud_in->header;

            // 填充cloud_in的点
            for (int h = 0; h < cloud_in->height; ++h) {
                memcpy(&Final->points[h * final_w], &cloud_in->points[h * cloud_in->width], cloud_in->width * sizeof(pcl::PointXYZRGB));
                // 填充右侧无效点
                for (int w = cloud_in->width; w < final_w; ++w) {
                    auto& p = Final->points[h * final_w + w];
                    p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
                }
            }
            // 填充cloud2_filtered的点
            for (int h = 0; h < cloud2_filtered->height; ++h) {
                memcpy(&Final->points[h * final_w + cloud_in->width], &cloud2_filtered->points[h * cloud2_filtered->width], cloud2_filtered->width * sizeof(pcl::PointXYZRGB));
            }
            // 填充下方无效点
            for (int h = cloud_in->height; h < final_h; ++h) {
                for (int w = 0; w < final_w; ++w) {
                    auto& p = Final->points[h * final_w + w];
                    p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
                }
            }
        }

        pcl::toROSMsg(*Final, output_);
        NODELET_INFO("Final height %d", output_.height);
        output_.header.frame_id = target_frame_;
        output_.header.stamp = ros::Time::now();
        output_.is_bigendian = cloud1RW_->is_bigendian;
        output_.is_dense = Final->is_dense;
    }
}

内容的提问来源于stack exchange，提问作者Pran-7