ICP算法

迭代最近点（Iterative Closest Point，ICP）算法是一种用于两个三维形状之间几何对齐（也叫做配准）的计算方法。通常，这两个形状至少有一个是点云数据。ICP算法用于最小化源点云与目标点云之间点到点的距离，从而寻找两者之间的最佳匹配变换（通常是刚体变换，但也可能是仿射或其他形式的变换）。

算法流程

初始化：设定初始猜测的变换参数（通常为单位矩阵）。

关联点：对于源点云中的每一个点，找到目标点云中的最近点。

最小化误差：找到一个变换，该变换能最小化源点到对应目标点之间的距离。

更新变换：将找到的变换应用于源点云。

收敛检查：检查误差或变换是否在一定阈值内收敛。如果是，则算法停止。否则，返回步骤2。

数学公式

设源点云 P 和目标点云Q 由 n 个点组成，其中pi∈P 和 qi∈Q。我们想要找到一个变换 T，使得总体误差 E 最小。

对于刚体变换，T 可以由旋转矩阵 R 和平移矢量 t 组成：

这个算法的优点是简单、直观、实现相对容易，缺点是容易收敛到局部最优解和计算复杂性较高。经常应用到机器人导航、对象识别、三维建模等场景。

代码流程

计算模型的质心：使用 VTK 的 vtkCenterOfMass 类计算 STL 模型的质心。这个质心用于后续找出模型中距质心最远的几个点。
找出四个距离质心最远的点：迭代遍历模型所有的点，并使用 VTK 的 vtkMath::Distance2BetweenPoints 方法计算每个点与质心的距离。找出四个距离质心最远的点，并保存它们的索引。
创建一个新的 vtkPolyData 对象，包含这四个点：使用 vtkPoints 和 vtkCellArray 创建一个新的 vtkPolyData 对象，这个对象只包括距离质心最远的四个点。
创建另一个 vtkPolyData 对象，包含在 3D 模型中选定的点：这个对象是算法的目标点集，也是使用 vtkPoints 和 vtkCellArray 创建的。
应用 ICP 算法：使用 VTK 的 vtkIterativeClosestPointTransform 类进行 ICP 对齐。设置源点（STL模型中的四个点）和目标点（3D模型中选定的点），并启动算法。
应用变换到 STL 模型：通过 vtkTransformPolyDataFilter 将 ICP 算法得出的变换应用到原始的 STL 模型上。
更新渲染：最后，更新模型的渲染，以反映应用了 ICP 变换后的新状态。

代码的核心是使用 ICP 算法进行两组点之间的最优对齐。它首先选取 STL 模型中四个特定的点（距离质心最远的点），然后用这些点与 3D 模型中预先选定的点进行对齐。通过这种方式，算法能够找到一个最佳的刚体变换，将 STL 模型与 3D 模型对齐。在这里面，所做的只是一个简单的配准示例。因为两个模型特征点的选取和ICP算法的应用都是比较复杂的，需要不断的尝试和优化，才能得到一个比较好的效果。

cpp 复制代码

   void performModelAlignment() {

    
    // Calculate Model Center
    double modelCenter[3];
    auto centerCalculator = vtkSmartPointer<vtkCenterOfMass>::New();
    centerCalculator->SetInputData(reader->GetOutput());
    centerCalculator->SetUseScalarsAsWeights(false);
    centerCalculator->Update();
    centerCalculator->GetCenter(modelCenter);

    // Find Distant Points from Center
    auto modelPoints = reader->GetOutput()->GetPoints();
    std::vector<vtkIdType> distantPoints = findDistantPoints(modelPoints, modelCenter, 4);

    // Construct PolyData for Distant Points in STL
    auto stlSelectedPointsData = createPolyDataFromPoints(modelPoints, distantPoints);

    // Construct PolyData for Selected Points in 3D Model
    auto modelSelectedPointsData = createPolyDataFromPoints(featurePoints);

    // Perform ICP
    auto icpTransform = vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
    icpTransform->SetSource(stlSelectedPointsData);
    icpTransform->SetTarget(modelSelectedPointsData);
    icpTransform->GetLandmarkTransform()->SetModeToRigidBody();
    icpTransform->Modified();
    icpTransform->Update();

    // Apply Transformation
    applyTransformationToModel(icpTransform, reader->GetOutput());

}

std::vector<vtkIdType> findDistantPoints(vtkSmartPointer<vtkPoints> points, double center[3], int numPoints) {
    std::vector<vtkIdType> distantPoints;
    // ... (Same logic to find distant points)
    return distantPoints;
}

vtkSmartPointer<vtkPolyData> createPolyDataFromPoints(vtkSmartPointer<vtkPoints> points, std::vector<vtkIdType> &selectedIds) {
    auto polyData = vtkSmartPointer<vtkPolyData>::New();
    auto selectedPoints = vtkSmartPointer<vtkPoints>::New();
    auto vertices = vtkSmartPointer<vtkCellArray>::New();
    // ... (Same logic to create PolyData)
    return polyData;
}

void applyTransformationToModel(vtkSmartPointer<vtkIterativeClosestPointTransform> icpTransform, vtkSmartPointer<vtkPolyData> originalData) {
    auto transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    transformFilter->SetInputData(originalData);
    transformFilter->SetTransform(icpTransform);
    transformFilter->Update();
    vtkPolyDataMapper::SafeDownCast(m_modelActor->GetMapper())->SetInputData(transformFilter->GetOutput());
}

VTK——使用ICP算法进行模型配准

ICP算法

算法流程

数学公式

代码流程