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算法的应用都是比较复杂的，需要不断的尝试和优化，才能得到一个比较好的效果。
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());
}
