# 迭代最近点算法`ICP`及`SVD`求解

# `ICP`问题

迭代最近点算法Iterative Closet Points,ICP算法，是根据两组已知的三维空间点来估计采集两组点时传感器位姿发生的旋转和平移变化。

在使用相机时，我们可以通过图像的特征匹配，找到两组三维点之间每个点的匹配关系。而在使用激光雷达时，由于点云是稀疏的，但是采集频率很高，通常认为两帧之间离的最近的点就是匹配点，因此称此算法为迭代最近点算法。

使用代数式描述为:

假设我们得到,

时刻传感器位置一采集到的一组三维空间点

时刻传感器运动到位置二采集到的一组三维空间点

这里，和中的点是传感器坐标系中的表示，这两组点对应的是世界坐标系下相同的个点，且对应,对应，依次类推。传感器坐标系是运动坐标系，从位置1到位置2姿态和位置发生的变化分别是，则可以得到如下关系，

p_{i} = R q_{i} + t, i = 1, . . ., n

观察上式，我们有对匹配点就能得到个方程，是待求量，一般情况下可以得到的匹配点对都会远远多于求解所需的量，所以得到的方程组都是超定的，只能求解其最小二乘解。

实际为了求解定义误差项，

对于第个点

e_{i} = p_{i} - (R q_{i} + t)

对于所有个点，求解变成如下最优化问题，

min_{R, t} \frac{1}{2} \sum_{i}^{n} | | p_{i} - (R q_{i} + t) | |_{2}^{2}

要求解此最小二乘问题，有两种解法，一种使用SVD分解，一种使用非线性优化算法。

本文中，我们来一起学习SVD方法。

# SVD分解来求解`ICP`

定义两组点的质心为

p_{c} = \frac{1}{n} \sum_{i}^{n} p_{i}

q_{c} = \frac{1}{n} \sum_{i}^{n} q_{i}

将质心代入误差函数，

\begin{aligned} \frac{1}{2} \sum_{i}^{n} | | p_{i} - (R q_{i} + t) | |_{2}^{2} = & \frac{1}{2} \sum_{i}^{n} | | p_{i} - R q_{i} - t - p_{c} + R q_{c} + p_{c} - R q_{c} | |_{2}^{2} \\ = & \frac{1}{2} \sum_{i}^{n} | | p_{i} - p_{c} - R (q_{i} - q_{c}) + (p_{c} - R q_{c} - t) | |_{2}^{2} \\ = & \frac{1}{2} \sum_{i}^{n} (| | p_{i} - p_{c} - R (q_{i} - q_{c}) | |_{2}^{2} + | | p_{c} - R q_{c} - t | |_{2}^{2} + 2 (p_{i} - p - R (q_{i} - q))^{T} (p_{c} - R q_{c} - t)) \end{aligned}

最后一项求和后结果为零，因此：

min_{R, t} J = \frac{1}{2} \sum_{i}^{n} | | p_{i} - p_{c} - R (q_{i} - q_{c}) | |_{2}^{2} + | | p_{c} - R q_{c} - t | |_{2}^{2}

观察变换后的优化问题形式，第一项只和有关，因此只要能求得，然后令第二项为零即可求得

如何求解旋转矩阵R呢？

取优化问题的第一项，

\frac{1}{2} \sum_{i}^{n} | | p_{i} - p_{c} - R (q_{i} - q_{c}) | |_{2}^{2}

令,

\begin{matrix}u_i=p_i-p_c\\v_i=q_i-q_c\end{matrix}

则，

\frac{1}{2} \sum_{i}^{n} | | p_{i} - p_{c} - R (q_{i} - q_{c}) | |_{2}^{2} = \frac{1}{2} \sum_{i}^{n} | | u_{i} - R v_{i} | |_{2}^{2}

根据二范数展开

\frac{1}{2} \sum_{i}^{n} | | u_{i} - R v_{i} | |_{2}^{2} = \frac{1}{2} \sum_{i}^{n} (u_{i}^{T} u_{i} + v_{i}^{T} R^{T} R v_{i} - 2 u_{i}^{T} R v_{i})

展开后的三项中前两项都和无关，因此只需要关注最后一项，

- \sum_{i}^{n} u_{i}^{T} R v_{i}

使其最小即可，上式中是三维空间中的点差，因此是的矩阵，根据矩阵迹的性质，

- \sum_{i}^{n} u_{i}^{T} R v_{i} = - \sum_{i}^{n} t r (R v_{i} u_{i}^{T}) = - t r (R \sum_{i}^{n} v_{i} u_{i}^{T})

引入一点数学知识，对于正定矩阵，对于任何正交矩阵都有,关于此的证明可参考论文 (opens new window)。

令

W = \sum_{i}^{n} v_{i} u_{i}^{T}

是一个的矩阵，对进行SVD分解，

W = U Σ V^{T}

是的正交矩阵，是的元素非负的对角矩阵。

R = V U^{T} $ $ 就 是 以 上 问 题 的 解 ， 可 根 据 $ R $ 再 求 出 $ t $ 。 关 于 为 什 么 $ R = V U^{T} $ 就 是 以 上 问 题 的 解 ， 可 结 合 $ t r (A A^{T}) \geq t r (B A A^{T}) $ 来 看 ， 若 $ R = V U^{T} $ $ $ R W = V U^{T} U Σ V^{T} = V Σ V^{T}

是正定对称矩阵，根据，对于任何正交矩阵，都有,而旋转矩阵一定是正交矩阵，因此可以使取最大值。若求得的的行列式为负，就取作为最后的旋转矩阵。关于此的详细说明可参考论文 (opens new window)。

# `SVD`求解`ICP`实战

代码参考自视觉SLAM十四讲。

示例使用的RGB-D图像，先使用RGB图像找到两个位置图像中匹配点，再计算匹配点的三维坐标，然后利用SVD来求解R和t。

关键的两个函数，一个是特征点匹配，

void ICPSolver::findMatchFeatures(cv::Mat &image1,
                                    cv::Mat &image2,
                                    std::vector<cv::KeyPoint> &kpt1,
                                    std::vector<cv::KeyPoint> &kpt2,
                                    std::vector<cv::DMatch> &matches)
{
    cv::Mat desc1, desc2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    detector->detectAndCompute(image1, cv::Mat(), kpt1, desc1);
    detector->detectAndCompute(image2, cv::Mat(), kpt2, desc2);

    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    std::vector<cv::DMatch> match;
    matcher->match(desc1, desc2, match);

    double min_dist = 10000, max_dist = 0;
    for(auto &m : match) {
        auto d = m.distance;
        if(d < min_dist) min_dist = d;
        if(d > max_dist) max_dist = d;
    }
    std::cout << "Max Distance: " << max_dist << std::endl;
    std::cout << "Min Distance: " << min_dist << std::endl;

    for(auto &m : match) {
        if(m.distance < 2 * min_dist || m.distance < 30)
            continue;
        matches.push_back(m);
    }
}

SVD求解R和t的代码，

void ICPSolver::svdSolver(std::vector<cv::Point3f> &pts1,
                std::vector<cv::Point3f> &pts2,
                cv::Mat &R, cv::Mat &t)
{
    cv::Point3f pc1, pc2; // center point
    int N = pts1.size();
    for(int i = 0; i < N; i++) {
        pc1 += pts1[i];
        pc2 += pts2[i];
    }
    pc1 = cv::Point3f(cv::Vec3f(pc1) / N);
    pc2 = cv::Point3f(cv::Vec3f(pc2) / N);

    cv::Point3f u, v;
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i = 0; i < N; i++) {
        u = pts1[i] - pc1;
        v = pts2[i] - pc2;

        W += Eigen::Vector3d(u.x, u.y, u.z) * Eigen::Vector3d(v.x, v.y, v.z).transpose();
    }

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    std::cout << "W=" << W << std::endl;
    std::cout << "U=" << U << std::endl;
    std::cout << "V=" << V << std::endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    std::cout <<"det(R_)=" << R_.determinant() << std::endl;
    if(R_.determinant() < 0) {
        R_ = -R_;
    }
    std::cout <<"R_R_^T=" << R_ * R_.transpose() << std::endl;

    Eigen::Vector3d t_ = Eigen::Vector3d(pc1.x, pc1.y, pc1.z) 
                            - R_ * Eigen::Vector3d(pc2.x, pc2.y, pc2.z);
    std::cout << "t_ = " << t_ << std::endl;

    R = (cv::Mat_<double>(3,  3) << R_(0, 0), R_(0, 1), R_(0, 2),
                                    R_(1, 0), R_(1, 1), R_(1, 2),
                                    R_(2, 0), R_(2, 1), R_(2, 2));
    t = (cv::Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

代码运行的结果可以发现

# det(R_)=1
# R_R_^T= [[1 -2.13805e-16    2.498e-16]
#          [-2.13805e-16            1  4.16334e-16],
#         [2.498e-16  4.22405e-16            1]]

完整可运行代码见https://gitee.com/lx_r/basic_cplusplus_examples (opens new window)

1.https://www.math.pku.edu.cn/teachers/yaoy/Fall2011/arun.pdf (opens new window)

2.视觉SLAM十四讲第二版p196-198 (opens new window)

# 迭代最近点算法ICP及SVD求解

# ICP问题

# SVD分解来求解ICP

# SVD求解ICP实战

# 迭代最近点算法`ICP`及`SVD`求解

# `ICP`问题

# SVD分解来求解`ICP`

# `SVD`求解`ICP`实战