基于表面的刚体配准¶

1 点云配准¶

点云配准的核心任务，是将两个在不同坐标系或视角下扫描得到的点云（通常称为源点云 P 和目标点云 Q）对齐。这需要找到一个最优的刚体变换（旋转矩阵R和平移向量t），使得两片点云在重叠区域内的整体误差最小。

常见点云配准算法框架有两类：ICP（Iterative Closest Point，迭代最近点）和 NDT（Normal Distributions Transform，正态分布变换）。

特性	ICP (迭代最近点)	NDT (正态分布变换)
核心思想	最小化点对距离：迭代地寻找最近点并最小化对应点间的距离。	最大化概率似然：将目标点云表示为概率场，寻找使源点云在该场中概率最大的变换。
数据结构	依赖KD-Tree等加速最近邻搜索。	将空间划分为固定网格（体素），每个网格存储一个概率分布参数。
对应关系	显式对应：为每个源点找到目标点云中的一个对应点。	隐式对应：源点通过概率密度函数与目标点云的局部区域相关联，无一一对应的点。
初值要求	较高。容易因错误的最近点对应而陷入局部最优。	相对较低。概率表示更为平滑，对初始位置的容忍度更好。
计算速度	每次迭代需搜索最近点，计算量较大。	预处理（建网格、计算分布）后，配准过程通常更快，因为无需动态搜索最近邻。
抗噪性	对噪声和离群点较敏感，通常需要搭配鲁棒策略。	抗噪性较好，因为概率分布本身对离群点不敏感。
对点密度	敏感，点密度差异大会影响匹配质量。	不敏感，只要网格内有足够点来估算分布即可。
适用场景	1. 高精度工业检测。 2. 计算机辅助医疗。 3. 三维重建后期精配准。	1. 机器人定位与SLAM。 2. 自动驾驶环境感知。

ICP 更直观，讲究“精准对位”，适合数据干净、目标明确、有大致初始位置的场景，比如：医疗手术导航（把实时影像对准术前CT）和工业零件检测（把扫描的工件和CAD模型对齐）中。
NDT 擅长处理“不可控、嘈杂、大范围、初值未知”，通过概率表示，提供了更平滑、更鲁棒的配准场，适合在环境“不可控”、数据“又吵又乱”、范围还很大的场景，比如：自动驾驶的激光雷达定位和机器人同步建图与定位（SLAM）。

2 基于表面的刚体配准¶

在计算机辅助医疗导航中，一个非常典型的配准问题是术前影像与术中物理空间之间的对齐。

术前 → 通过 CT 扫描重建出患者骨骼的三维表面模型（通常以 STL 或三角网格形式表示）
术中 → 通过带有定位系统的探针，在真实骨表面上采集一组离散的空间点。

这类问题通常被建模为 基于表面的刚体配准（surface-based rigid registration）：在刚体假设下，估计一个空间变换，使术中采集的点集能够与术前重建的骨表面在几何意义上最佳对齐。

由于术中探针只能接触到骨表面的局部区域，且采集到的是稀疏、不规则分布的点集（点数稀疏、覆盖范围有限、噪声较大），而术前模型则为高分辨率的完整骨表面，二者在初始状态下往往存在较大的位姿差异。在此条件下，单一的局部配准算法（如 ICP）容易陷入局部最优，难以保证稳定收敛。

因此，实际系统中通常采用分层式配准策略：首先通过解剖标志点获得一个几何意义明确的粗配准，将两坐标系对齐到相同数量级；随后使用对初始误差更具鲁棒性的 NDT 方法进行概率意义下的粗到中等精度配准；最后在较小误差范围内，利用 ICP 算法进行高精度的局部优化，从而实现稳定而精确的刚体配准结果。

2.1 术前模型构建¶

通过 CT 扫描重建患者骨骼的三维表面模型（STL 或三角网格），并在术前影像空间中完成必要的标注与模型准备，包括解剖标志点的选取与定义，为后续的术中初始配准提供参考。

2.2 基于标志点的初始配准¶

通过在术前模型与术中物理空间中分别采集对应的解剖标志点，建立两坐标系之间的初始对应关系。基于这些稀疏但具有明确语义一致性的点对，可直接求解一个刚体变换，用作后续表面配准的初始位姿估计。

解剖标志点仅用于初始位姿估计，其数量有限且不参与后续基于表面误差的优化过程。

bool SurgaceReg::CoarseRegistration(
  vtkSmartPointer<vtkPoints> sourcePoints, vtkSmartPointer<vtkPoints> targetPoints,
  vtkSmartPointer<vtkMatrix4x4> coarseMatrix)
{
    int nbSourcePoints = sourcePoints->GetNumberOfPoints();
    int nbTargetPoints = targetPoints->GetNumberOfPoints();

    if (nbSourcePoints != nbTargetPoints || nbSourcePoints < 3) {
        return false;
    }

    vtkNew<vtkLandmarkTransform> landmarkTrans;
    landmarkTrans->SetModeToRigidBody();
    landmarkTrans->SetSourceLandmarks(sourcePoints);
    landmarkTrans->SetTargetLandmarks(targetPoints);
    landmarkTrans->Update();

    coarseMatrix->DeepCopy(landmarkTrans->GetMatrix());

    return true;
}

2.3 术中表面点采集¶

使用带定位系统的探针在真实骨表面进行扫描，采集一组稀疏、局部的三维表面点，作为待配准的源点云。

2.4 预处理¶

对术前模型进行平移归一化，将其置于计算中心。
根据术中点的范围裁剪感兴趣区域（ROI），排除无关结构干扰。
对裁剪后的网格进行表面加密采样，生成用于配准的稠密表面点云。
基于探针点到最近表面点的方向关系，进行若干次仅含平移的对齐，减小初始位姿误差并提升后续算法的收敛稳定性。

// 在粗配准结果基础上，通过最近邻搜索估计点云间的平均平移偏差，并迭代修正位姿中的平移分量，以降低初始配准的系统性偏移。
Eigen::Matrix4f RegistrationMathUtils::RefineTranslationByNearestNeighbor(
  pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
  double distanceThreshold, int maxIterations)
{
    Eigen::Matrix4f initTransform = Eigen::Matrix4f::Identity();

    if (!source || !target || source->empty() || target->empty()) {
        return initTransform;
    }

    // KD-tree on target cloud
    pcl::search::KdTree<pcl::PointXYZ> targetTree;
    targetTree.setInputCloud(target);

    std::vector<int> indices(1);
    std::vector<float> sqrDistances(1);

    // 当前变换（只更新平移）
    Eigen::Matrix4f currentTransform = initTransform;

    pcl::PointCloud<pcl::PointXYZ> transformedSource;
    pcl::transformPointCloud(*source, transformedSource, currentTransform);

    int iter = 0;
    double meanDistance = std::numeric_limits<double>::max();

    while (meanDistance > distanceThreshold && iter < maxIterations) {

        pcl::PointCloud<pcl::PointXYZ> directionCloud;
        directionCloud.reserve(transformedSource.size());

        std::vector<float> distances(transformedSource.size());

        // 最近邻 + 位移向量
        for (size_t i = 0; i < transformedSource.size(); ++i) {
            if (targetTree.nearestKSearch(transformedSource[i], 1, indices, sqrDistances) > 0) {

                const pcl::PointXYZ & nearest = target->points[indices[0]];

                pcl::PointXYZ dir;
                dir.x = nearest.x - transformedSource[i].x;
                dir.y = nearest.y - transformedSource[i].y;
                dir.z = nearest.z - transformedSource[i].z;
                directionCloud.push_back(dir);

                distances[i] = std::sqrt(sqrDistances[0]);
            }
        }

        if (directionCloud.empty()) {
            break;
        }

        // 平均位移
        Eigen::Vector4f meanShift;
        pcl::compute3DCentroid(directionCloud, meanShift);

        // 只更新 translation
        currentTransform(0, 3) += meanShift[0];
        currentTransform(1, 3) += meanShift[1];
        currentTransform(2, 3) += meanShift[2];

        // 重新变换 source
        pcl::transformPointCloud(*source, transformedSource, currentTransform);

        // 计算 mean NN distance
        meanDistance = std::accumulate(distances.begin(), distances.end(), 0.0) / distances.size();

        ++iter;
    }

    return currentTransform;
}

2.5 基于概率模型的中等精度配准（NDT）¶

采用正态分布变换（NDT）方法，在概率意义下对两组点云进行对齐，实现对初始误差较为鲁棒的全局或半全局配准。为减小参数选择对结果的影响，可在不同模型尺度下并行执行 NDT，并筛选误差较小的若干候选位姿。

QList<RegistrationCandidate> NDTMultiScaleRegistrar::Register()
{
    qInfo() << "NDTMultiScaleRegistrar start.";

    QElapsedTimer timer;
    timer.start();

    double sourceCloudResolution = ComputeCloudResolution(m_SourceCloud, m_Config.kForResolution);
    double targetCloudResolution = ComputeCloudResolution(m_TargetCloud, m_Config.kForResolution);

    double minResolutionRatio = m_Config.minResolution;
    double maxResolutionRatio = m_Config.maxResolution;

    minResolutionRatio = std::floor(targetCloudResolution * 10.0) / 10.0 + 0.2;
    if (minResolutionRatio > maxResolutionRatio) {
        maxResolutionRatio = minResolutionRatio + 1.0;
    }

    double resolutionStep = m_Config.resolutionStep;
    int resolutionNum = static_cast<int>(std::floor((maxResolutionRatio - minResolutionRatio) / resolutionStep));
    resolutionNum = std::max(1, resolutionNum + 1);

    qInfo() << "Source Cloud Resolution: " << sourceCloudResolution;
    qInfo() << "Target Cloud Resolution: " << targetCloudResolution;
    qInfo() << "NDT Multi-Scale minResolution: " << minResolutionRatio
            << ", maxResolution: " << maxResolutionRatio
            << ", resolutionStep: " << resolutionStep
            << ", resolutionNum: " << resolutionNum;

    QList<NDTJob> jobList;
    for (int i = 0; i < resolutionNum; i++) {
        NDTJob job;
        job.resolution = minResolutionRatio + i * resolutionStep;
        job.stepSize = job.resolution / 10.0;
        pcl::copyPointCloud(*m_SourceCloud, job.sourceCloud);
        pcl::copyPointCloud(*m_TargetCloud, job.targetCloud);
        job.initMatrix = m_InitialTransform;
        jobList.append(job);
    }

    QFuture<QList<NDTResult>> future = QtConcurrent::mappedReduced(jobList, RunNDTRegistration, CollectValidNDTResult);
    future.waitForFinished();

    QList<NDTResult> ndtResults = future.result();
    std::sort(ndtResults.begin(), ndtResults.end(), CompareNDTResultByScore);

    QList<RegistrationCandidate> candidates;
    double minScore = ndtResults[0].score;
    for (int i = 0; i < m_Config.acceptResultMaxNum; i++) {
        if (ndtResults[i].score < minScore * 1.5) {
            RegistrationCandidate candidate;
            candidate.matrix = ndtResults[i].matrix;
            candidate.score = ndtResults[i].score;
            candidates.append(candidate);
        } else {
            break;
        }
    }

    qint64 elapsedMs = timer.elapsed();
    qInfo() << "NDTMultiScaleRegistrar time(ms): " << elapsedMs;

    return candidates;
}

NDTResult RunNDTRegistration(NDTJob paras)
{
    NDTResult result;

    pcl::PointCloud<pcl::PointXYZ>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZ>);

    try {
        pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
        ndt.setTransformationEpsilon(1e-10); // 为终止条件设置最小转换差异
        ndt.setTransformationRotationEpsilon(1 - 1e-5);
        ndt.setMaxCorrespondenceDistance(1.0);
        ndt.setOulierRatio(0.9);
        ndt.setEuclideanFitnessEpsilon(1e-5);
        ndt.setMaximumIterations(10000);
        ndt.setInputSource(paras.sourceCloud.makeShared()); // 源点云
        ndt.setInputTarget(paras.targetCloud.makeShared()); // 目标点云
        ndt.setStepSize(paras.stepSize); // more-thuente 线搜索最大步长
        ndt.setResolution(paras.resolution); // NDT网格网格结构的分辨率
        ndt.align(*outputCloud, paras.initMatrix);

        result.valid = true;
        result.matrix = ndt.getFinalTransformation();
        result.resolution = paras.resolution;
        result.stepSize = paras.stepSize;
        result.score = ndt.getFitnessScore();
    } catch (const std::exception &) {
        result.valid = false;
    }

    return result;
}

2.6 基于最近邻优化的高精度配准（ICP）¶

在 NDT 输出的候选位姿基础上，使用基于最近邻的点到点 ICP 算法进行局部精配准。通过对多个初始位姿并行优化，进一步降低陷入局部最优的风险，从而获得高精度的刚体配准结果。

ICPResult RunICPRegistration(const ICPJob & job, const ICPMultiInitRegistrar::ICPConfig & cfg)
{
    ICPResult result;

    try {
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        pcl::PointCloud<pcl::PointXYZ> aligned;

        icp.setInputSource(job.sourceCloud.makeShared());
        icp.setInputTarget(job.targetCloud.makeShared());

        icp.setMaxCorrespondenceDistance(cfg.maxCorrespondenceDistance);
        icp.setMaximumIterations(cfg.maxIterations);
        icp.setTransformationEpsilon(cfg.transformationEpsilon);
        icp.setTransformationRotationEpsilon(cfg.rotationEpsilon);
        icp.setEuclideanFitnessEpsilon(cfg.fitnessEpsilon);

        icp.align(aligned, job.initMatrix);

        if (icp.hasConverged()) {
            result.valid = true;
            result.matrix = icp.getFinalTransformation();
            result.score = icp.getFitnessScore();
        }
    } catch (const std::exception &) {
        result.valid = false;
    }

    return result;
}

RegistrationCandidate ICPMultiInitRegistrar::Register()
{
    QElapsedTimer timer;
    timer.start();

    QList<ICPJob> jobs;
    for (const auto & init : m_InitialTransforms) {
        ICPJob job;
        pcl::copyPointCloud(*m_SourceCloud, job.sourceCloud);
        pcl::copyPointCloud(*m_TargetCloud, job.targetCloud);
        job.initMatrix = init;
        jobs.append(job);
    }

    auto mapper = [&](const ICPJob & job) {
        return RunICPRegistration(job, m_Config);
    };

    QFuture<QList<ICPResult>> future =
      QtConcurrent::mappedReduced(jobs, mapper, CollectValidICPResult);

    future.waitForFinished();

    QList<ICPResult> results = future.result();
    std::sort(results.begin(), results.end(), CompareICPResultByScore);

    RegistrationCandidate candidate;
    if (results.size() > 0) {
        candidate.matrix = results[0].matrix;
        candidate.score = results[0].score;
    }

    qInfo() << "ICPMultiInitRegistrar time(ms): " << timer.elapsed();
    return candidate;
}

2.7 配准误差评估¶

通过计算探针点到骨表面最近点的均方根误差（RMS），对最终配准结果的精度进行定量评估。

double RegistrationMathUtils::ComputeRMSDistance(
  pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
  const Eigen::Matrix4f & matrix, double trimRatio)
{
    pcl::search::KdTree<pcl::PointXYZ> tree;
    tree.setInputCloud(source);

    pcl::PointCloud<pcl::PointXYZ> transformed;
    pcl::transformPointCloud(*target, transformed, matrix);

    std::vector<double> dists;
    dists.reserve(transformed.size());

    std::vector<int> idx(1);
    std::vector<float> sqr(1);

    for (const auto & p : transformed) {
        if (!pcl::isFinite(p)) {
            continue;
        }
        if (tree.nearestKSearch(p, 1, idx, sqr) > 0) {
            dists.push_back(sqr[0]);
        }
    }

    if (dists.empty()) {
        return std::numeric_limits<double>::infinity();
    }

    std::sort(dists.begin(), dists.end());

    size_t keep = static_cast<size_t>(dists.size() * (1.0 - trimRatio));
    double sum = 0.0;
    for (size_t i = 0; i < keep; ++i) {
        sum += dists[i];
    }

    return std::sqrt(sum / keep);
}