两点云相减并保存结果的C++代码实现

C++中实现两个点云相减并保存相减结果，可以使用点云库（PCL, Point Cloud Library）。代码示例展示了如何进行点云相减，并将结果保存为一个新的点云文件。

这个例子使用了PCL中的pcl::KdTreeFLANN来查找一个点云中的点在另一个点云中的最近邻点。如果最近邻点的距离超过一个预设的阈值，则认为该点是两个点云之间的差异，并将其保存到结果点云中。

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void subtractPointClouds(const PointCloud::Ptr &cloud1, const PointCloud::Ptr &cloud2, PointCloud::Ptr &cloud_diff, double threshold) {
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud2);

    std::vector<int> pointIdxNKNSearch(1);
    std::vector<float> pointNKNSquaredDistance(1);

    for (size_t i = 0; i < cloud1->points.size(); ++i) {
        if (kdtree.nearestKSearch(cloud1->points[i], 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
            if (pointNKNSquaredDistance[0] > threshold * threshold) {
                cloud_diff->points.push_back(cloud1->points[i]);
            }
        }
    }
}

int main() {
    PointCloud::Ptr cloud1(new PointCloud);
    PointCloud::Ptr cloud2(new PointCloud);
    PointCloud::Ptr cloud_diff(new PointCloud);

    // 加载点云文件
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("cloud1.pcd", *cloud1) == -1 ||
        pcl::io::loadPCDFile<pcl::PointXYZ>("cloud2.pcd", *cloud2) == -1) {
        PCL_ERROR("Couldn't read file \n");
        return -1;
    }

    double threshold = 0.05;  // 设置差异阈值
    subtractPointClouds(cloud1, cloud2, cloud_diff, threshold);

    cloud_diff->width = cloud_diff->points.size();
    cloud_diff->height = 1;
    cloud_diff->is_dense = false;
    cloud_diff->points.resize(cloud_diff->width * cloud_diff->height);

    // 保存结果点云
    pcl::io::savePCDFileASCII("cloud_diff.pcd", *cloud_diff);
    std::cout << "Resulting point cloud saved to 'cloud_diff.pcd'." << std::endl;

    return 0;
}

在这段代码中，函数subtractPointClouds负责计算两个点云的差异。它将点云cloud1中的每个点与点云cloud2进行比较，如果在cloud2中找不到接近的点（基于设定的阈值），则这个点被认为是两个点云的差异，并被加入到结果点云cloud_diff中。最后，使用pcl::io::savePCDFileASCII函数将结果点云保存到PCD文件中。

例子中假设有两个名为cloud1.pcd和cloud2.pcd的点云文件。需要根据实际情况调整文件名和路径。此外，代码中的阈值设置（threshold）可能需要根据你的具体应用进行调整。