ICP 点云配准c++代码详细过程

ICP (Iterative Closest Point) 是一种常用的点云配准算法，它能够将两个或多个点云进行对齐，使它们尽可能地重合。下面是一个简单的 C++ 实现。

假设我们有两个点云 `cloud1` 和 `cloud2`，每个点云都由一组点组成，每个点包含 x、y、z 三个坐标。我们的目标是将 `cloud2` 对齐到 `cloud1` 上。

首先，我们需要定义一个点类：

class Point
{
public:
    float x, y, z;
};

然后，我们可以定义一个函数 `icp()` 来实现 ICP 算法：

void icp(std::vector<Point>& cloud1, std::vector<Point>& cloud2)
{
    int num_iterations = 10;    // 迭代次数
    float tolerance = 0.001;    // 容差
    float alpha = 0.5;          // 步长因子

    for (int iter = 0; iter < num_iterations; ++iter)
    {
        // Step 1: Find closest points
        std::vector<std::pair<int, int>> correspondences;
        for (int i = 0; i < cloud1.size(); ++i)
        {
            float min_dist = std::numeric_limits<float>::max();
            int min_idx = -1;
            for (int j = 0; j < cloud2.size(); ++j)
            {
                float dist = std::sqrt(std::pow(cloud1[i].x - cloud2[j].x, 2.0) +
                                       std::pow(cloud1[i].y - cloud2[j].y, 2.0) +
                                       std::pow(cloud1[i].z - cloud2[j].z, 2.0));
                if (dist < min_dist)
                {
                    min_dist = dist;
                    min_idx = j;
                }
            }
            correspondences.push_back(std::make_pair(i, min_idx));
        }

        // Step 2: Compute transformation
        Eigen::Matrix3f R;
        Eigen::Vector3f t;
        compute_transform(cloud1, cloud2, correspondences, R, t);

        // Step 3: Apply transformation
        for (int i = 0; i < cloud2.size(); ++i)
        {
            Eigen::Vector3f p(cloud2[i].x, cloud2[i].y, cloud2[i].z);
            p = R * p + t;
            cloud2[i].x = p(0);
            cloud2[i].y = p(1);
            cloud2[i].z = p(2);
        }

        // Step 4: Check convergence
        float error = compute_error(cloud1, cloud2, correspondences);
        if (error < tolerance)
        {
            break;
        }

        // Step 5: Update parameters
        alpha *= 0.5;
    }
}

下面是 `compute_transform()` 和 `compute_error()` 函数的实现：

void compute_transform(const std::vector<Point>& cloud1,
                        const std::vector<Point>& cloud2,
                        const std::vector<std::pair<int, int>>& correspondences,
                        Eigen::Matrix3f& R,
                        Eigen::Vector3f& t)
{
    R.setZero();
    t.setZero();

    Eigen::MatrixXf A(correspondences.size(), 3);
    Eigen::MatrixXf B(correspondences.size(), 3);
    for (int i = 0; i < correspondences.size(); ++i)
    {
        A.row(i) << cloud1[correspondences[i].first].x,
                    cloud1[correspondences[i].first].y,
                    cloud1[correspondences[i].first].z;
        B.row(i) << cloud2[correspondences[i].second].x,
                    cloud2[correspondences[i].second].y,
                    cloud2[correspondences[i].second].z;
    }

    Eigen::Vector3f centroid1 = A.colwise().mean();
    Eigen::Vector3f centroid2 = B.colwise().mean();
    for (int i = 0; i < correspondences.size(); ++i)
    {
        A.row(i) -= centroid1.transpose();
        B.row(i) -= centroid2.transpose();
    }

    Eigen::MatrixXf H = A.transpose() * B;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXf U = svd.matrixU();
    Eigen::MatrixXf V = svd.matrixV();
    R = V * U.transpose();
    if (R.determinant() < 0)
    {
        R.col(2) *= -1;
    }
    t = centroid1 - R * centroid2;
}

float compute_error(const std::vector<Point>& cloud1,
                    const std::vector<Point>& cloud2,
                    const std::vector<std::pair<int, int>>& correspondences)
{
    float error = 0.0;
    for (const auto& c : correspondences)
    {
        Eigen::Vector3f p1(cloud1[c.first].x, cloud1[c.first].y, cloud1[c.first].z);
        Eigen::Vector3f p2(cloud2[c.second].x, cloud2[c.second].y, cloud2[c.second].z);
        float dist = (p1 - p2).norm();
        error += dist * dist;
    }
    return std::sqrt(error);
}

上面的代码中，`compute_transform()` 函数用于计算从 `cloud2` 到 `cloud1` 的旋转矩阵和平移向量，`compute_error()` 函数用于计算配准误差。这两个函数都使用了 Eigen 库进行矩阵计算。

在 `icp()` 函数中，我们首先定义了一些参数，包括迭代次数、容差和步长因子。然后，在每次迭代中，我们执行以下步骤：

1. 找到最近的点对；
2. 计算变换矩阵；
3. 应用变换矩阵；
4. 检查收敛；
5. 更新参数。

在第一步中，我们使用暴力搜索方法找到每个点在另一个点云中的最近邻。在实际应用中，可以使用更高效的方法，如 kd-tree 或 octree。

在第二步中，我们使用 SVD 分解计算从 `cloud2` 到 `cloud1` 的最优旋转矩阵和平移向量。具体方法可以参考 PCL 库中的实现。

在第三步中，我们将 `cloud2` 应用变换矩阵，使它尽可能地与 `cloud1` 重合。

在第四步中，我们检查配准误差是否达到容差，如果达到，则认为算法已经收敛，退出迭代。

在第五步中，我们更新步长因子，以便在后续迭代中更快地收敛。

在实际应用中，ICP 算法还有很多变体和改进，如 ICP with Point-to-Plane 和 ICP with Feature。如果需要更高效、更稳定的点云配准算法，可以使用 PCL 或 Open3D 等开源库。