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

---

欢迎访问个人网络日志🌹🌹知行空间🌹🌹

---

ICP问题

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

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

使用代数式描述为:

假设我们得到,

$t_1$时刻传感器位置一采集到的一组三维空间点 P = { p 1 , . . . , p n } P=\{p_1,...,p_n\} P={p1​,...,pn​}

$t_2$时刻传感器运动到位置二采集到的一组三维空间点 Q = { q 1 , . . . , q n } Q=\{q_1,...,q_n\} Q={q1​,...,qn​}

这里，$P$和$Q$中的点是传感器坐标系中的表示，这两组点对应的是世界坐标系 { W } \{W\} {W}下相同的$n$个点 { w 1 , . . . . , w 2 } \{w_1,....,w_2\} {w1​,....,w2​}，且$p_1,q_1$对应$w_1$,$p_2,q_2$对应$w_2$，依次类推。传感器坐标系是运动坐标系，从位置1到位置2姿态和位置发生的变化分别是$R,t$，则可以得到如下关系，

$$p_i=R{q}_i+t,i=1,...,n$$

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

实际为了求解$R,t$定义误差项，

对于第$i$个点

$$e_i=p_i-(R{q}_i+t)$$

对于所有$n$个点，求解$R,t$变成如下最优化问题，

$$\underset{R,t}{\min }\frac{1}{2}\sum _i^n||p_i-(R{q}_i+t)|{|}_2^2$$

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

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

SVD分解来求解ICP

定义两组点的质心为$p_c,q_c$

$$p_c=\frac{1}{n}\sum _i^n{p}_i$$

$$q_c=\frac{1}{n}\sum _i^n{q}_i$$

将质心代入误差函数，

$$\begin{array}{rl}\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{array}$$

最后一项$p_i-p-R(q_i-q)$求和后结果为零，因此：

$$\underset{R,t}{\min }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$有关，因此只要能求得$R$，然后令第二项为零即可求得$t$

如何求解旋转矩阵R呢？

取优化问题的第一项，

$$\frac{1}{2}\sum _i^n||p_i-p_c-R(q_i-q_c)|{|}_2^2$$

令,$$\begin{array}{c}{u}_i=p_i-p_c\\ v_i=q_i-q_c\end{array}$$

则，

$$\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-2u_i^{T}R{v}_i)$$

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

$$-\sum _i^n{u}_i^{T}R{v}_i$$

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

$$-\sum _i^n{u}_i^{T}R{v}_i=-\sum _i^{n}tr(R{v}_i{u}_i^T)=-tr(R\sum _i^n{v}_i{u}_i^T)$$

引入一点数学知识，对于正定矩阵$A{A}^T$，对于任何正交矩阵$B$都有$tr(A{A}^T)\ge tr(BA{A}^T)$,关于此的证明可参考论文。

令

$$W=\sum _i^n{v}_i{u}_i^T$$

$W$是一个$3\times 3$的矩阵，对$W$进行SVD分解，

$$W=U\Sigma V^T$$

$U/V$是$3\times 3$的正交矩阵，$\Sigma$是$3\times 3$的元素非负的对角矩阵。

$$R=V{U}^T$$就是以上问题的解，可根据$R$再求出$t$。

关于为什么$R=V{U}^T$就是以上问题的解，可结合$tr(A{A}^T)\ge tr(BA{A}^T)$来看，若$R=V{U}^T$

$$RW=V{U}^{T}U\Sigma V^T=V\Sigma V^T$$

$RW$是正定对称矩阵，根据$tr(A{A}^T)\ge tr(BA{A}^T)$，对于任何正交矩阵$B$，都有$tr(RW)\ge tr(BRW)$,而旋转矩阵一定是正交矩阵，因此$R=V{U}^T$可以使$tr(RW)$取最大值。若求得的$R$的行列式为负，就取$-R$作为最后的旋转矩阵。关于此的详细说明可参考论文。

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\approx I$

# 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

• 1.https://www.math.pku.edu.cn/teachers/yaoy/Fall2011/arun.pdf
• 2.视觉SLAM十四讲第二版p196-198

---

欢迎访问个人网络日志🌹🌹知行空间🌹🌹

---