茂之钳
74 lines
2.1 KiB
C++
74 lines
2.1 KiB
C++
#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
|