ViewDesignEngine/include/vde/core/icp.h

#pragma once
#include "vde/core/point.h"
#include "vde/core/transform.h"
#include <vector>

namespace vde::core {
using foundation::Point2D;
using foundation::Point3D;
using foundation::Vector2D;
using foundation::Vector3D;

/**
 * @brief ICP（迭代最近点）配准结果
 *
 * 包含刚体变换、误差度量、迭代信息和收敛状态。
 *
 * @ingroup core
 */
struct ICPResult {
    /// 将 source 对齐到 target 的刚体变换
    Transform3D transform;

    /**
     * @brief 配准后的均方根误差
     *
     * RMS = sqrt(Σ ||target_i - T(source_i)||² / N)
     */
    double rms_error;

    /// 实际使用的迭代次数
    int iterations;

    /// 是否在达到最大迭代次数前收敛
    bool converged;
};

/**
 * @brief 迭代最近点（ICP）刚体点云配准
 *
 * 使用标准 ICP 算法将 source 点云配准到 target 点云。
 * 每步迭代：
 * 1. 对每个 source 点找 target 中的最近邻
 * 2. 用 SVD 求解最优刚体变换
 * 3. 应用变换，检查收敛
 *
 * @param source 源点云（将被移动）
 * @param target 目标点云（固定参考）
 * @param max_iter 最大迭代次数（默认 50）
 * @param tolerance 收敛容差：相邻两次 RMS 变化的阈值（默认 1e-6）
 * @return 配准结果，包含最终变换和误差
 *
 * @note source 和 target 点数可以不同
 * @note 初始对齐越好，收敛越快；若初始偏差太大，可能收敛到局部最优
 *
 * @code{.cpp}
 * std::vector<Point3D> src = // ...;
 * std::vector<Point3D> tgt = // ...;
 * auto result = icp_register(src, tgt, 100, 1e-8);
 * if (result.converged) {
 *     std::cout << "RMS error: " << result.rms_error << "\n";
 *     // 应用变换
 *     for (auto& p : src) p = result.transform * p;
 * }
 * @endcode
 *
 * @ingroup core
 */
[[nodiscard]] ICPResult icp_register(const std::vector<Point3D>& source,
                                     const std::vector<Point3D>& target,
                                     int max_iter = 50,
                                     double tolerance = 1e-6);

}  // namespace vde::core