一、算法原理

二、代码实现

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/impl/common.hpp> // for isFinite
#include <type_traits>

// 一侧的平均最近邻距离和（可选平方）
template <typename PointT>
static double oneSidedChamfer(
    const typename pcl::PointCloud<PointT>::ConstPtr& from,
    const typename pcl::PointCloud<PointT>::ConstPtr& to,
    bool squared)
{
    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud(to);

    std::vector<int>    knn_indices(1);
    std::vector<float>  knn_sq_dists(1);

    double sum = 0.0;
    std::size_t valid_cnt = 0;

    for (const auto& p : *from)
    {
        if (!pcl::isFinite(p)) continue; // 跳过NaN/Inf
        if (kdtree.nearestKSearch(p, 1, knn_indices, knn_sq_dists) > 0)
        {
            const double d2 = static_cast<double>(knn_sq_dists[0]);
            sum += squared ? d2 : std::sqrt(d2);
            ++valid_cnt;
        }
    }

    return (valid_cnt > 0) ? (sum / static_cast<double>(valid_cnt)) : 0.0;
}

/**
 * @brief 计算双向倒角距离（Chamfer Distance）
 * @param cloud_a, cloud_b  两个点云
 * @param squared           若为 true，则返回“平方倒角”（L2^2），无需开根号，速度更快；false 则为标准倒角（L2）
 * @return 倒角距离标量
 */
template <typename PointT>
double computeChamferDistance(
    const typename pcl::PointCloud<PointT>::ConstPtr& cloud_a,
    const typename pcl::PointCloud<PointT>::ConstPtr& cloud_b,
    bool squared = false)
{
    if (!cloud_a || !cloud_b || cloud_a->empty() || cloud_b->empty())
        return std::numeric_limits<double>::quiet_NaN();

    // 双向平均
    const double ab = oneSidedChamfer<PointT>(cloud_a, cloud_b, squared);
    const double ba = oneSidedChamfer<PointT>(cloud_b, cloud_a, squared);
    return ab + ba;
}

int main()
{
    using PointT = pcl::PointXYZ;
    pcl::PointCloud<PointT>::Ptr cloudA(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloudB(new pcl::PointCloud<PointT>);

    if (pcl::io::loadPCDFile<PointT>("E:/data/1.pcd", *cloudA) != 0) {
        std::cerr << "读取 E:/data/1.pcd 失败\n";
        return 1;
    }
    if (pcl::io::loadPCDFile<PointT>("E:/data/2.pcd", *cloudB) != 0) {
        std::cerr << "读取 E:/data/2.pcd 失败\n";
        return 1;
    }

    // 标准倒角（L2）
    double chamfer_l2 = computeChamferDistance<PointT>(cloudA, cloudB, /*squared=*/false);
    std::cout << "Chamfer (L2): " << chamfer_l2 << "\n";

    // 平方倒角（L2^2）——更快，常用于优化目标
    double chamfer_l2_sq = computeChamferDistance<PointT>(cloudA, cloudB, /*squared=*/true);
    std::cout << "Chamfer (L2^2): " << chamfer_l2_sq << "\n";

    return 0;
}

三、应用场景

倒角距离（Chamfer Distance, CD）虽然概念简单（最近邻平均距离的对称形式），但在点云、计算机视觉、图形学等领域用得非常多。我分几个常见场景给你讲清楚：

---

1. 点云相似度评价

• 场景：机器人/三维重建/配准时，需要衡量两片点云是否“对齐”。

• 做法：配准后，把源点云和目标点云计算倒角距离，数值越小说明对齐越好。

• 特点：比简单的欧式距离直观，因为它考虑了“每个点的最近邻覆盖度”。

• 例子：ICP 或 NDT 配准完成后，用 CD 评价注册精度。

---

2. 三维重建与深度学习

• 场景：神经网络生成或补全点云时，需要一个 loss 函数来度量预测点云与真实点云的差异。

• 做法：常用 Chamfer Distance 或 Earth Mover’s Distance (EMD) 作为损失。

• 特点：

  • CD 易于计算（只需最近邻查询）。

  • 可微分，适合反向传播。

• 例子：

  • PointNet / AtlasNet / 3D GAN 等方法里常用 CD 来训练网络。

  • 点云补全任务：预测的点集和真实点集用 CD 衡量差异。

---

3. 计算机图形学与形状比较

• 场景：比较两个 3D 模型（mesh/点云）的几何相似性。

• 做法：取模型表面点采样，计算 CD。

• 用途：

  • 形状检索（shape retrieval）：检索数据库中与查询模型最接近的物体。

  • 模型简化（decimation）后检查保真度。

---

4. 三维检测 / 分割 / 配准精度评估

• 场景：3D 检测或分割任务，需要把预测的物体表面点集与 GT（ground truth）表面对比。

• 用途：

  • 评估预测物体边界与真实边界的接近程度。

  • 可替代 IoU（体素级）作为几何精度指标。

---

5. 医学影像与体素/曲面对比

• 场景：医学图像分割，常需要比较预测分割表面与医生标注表面。

• 做法：提取两个分割结果的边界点集，计算倒角距离。

• 用途：

  • 辅助评估分割算法精度。

  • 在 3D 器官/肿瘤表面拟合中常见。

---

6. 机器人感知与规划

• 场景：机器人在感知（视觉、激光雷达）得到环境点云后，需要和已有地图或目标模型比对。

• 用途：

  • 比较环境扫描与 CAD 模型的接近程度。

  • 评估路径规划中候选位姿的表面对齐度。

• 优势：计算快，适合实时性要求。

---

7. 其他应用

• 生成式对抗网络 (GAN) 图像/点云生成：用 CD 衡量生成分布与真实分布的差异。

• 形状补全与编辑：衡量局部修改前后是否保留整体形状。

• 三维匹配评价：PPF、特征匹配后，计算 CD 作为整体匹配效果指标。

---

✅ 总结：
Chamfer Distance 广泛用于 点云配准评价、深度学习的损失函数、3D 模型对比、分割评估、机器人感知 等场景。

它的优点是 直观、对点数不要求一致、可微分、速度快。
缺点是 对密度不均衡敏感，容易受离群点影响（这也是为什么有时要用 截断 CD 或 分位数 CD，或者改用 EMD/Hausdorff 距离）。