通过ICP配准两组尺度不同点云, 统一坐标系(附代码)

网上大部分的ICP配准点云方法, 是在假定两组点云间尺度一致的情况下, 计算出两组点云间的旋转矩阵R和平移向量t, 若两组点云间还存在尺度不一致, 则无法准确估计. 本文介绍了两种方法, 配准两组“尺度”不同点云, 计算出它们之间的相似变换矩阵, 统一它们的坐标.

Liber-coder

10281人浏览 · 2020-03-04 12:59:54

Liber-coder · 2020-03-04 12:59:54 发布

摘要

注意：本文提出的方法效果不一定好, 如果大家知道更好的方法, 欢迎留言讨论.

本文主要讲了如何估计尺度，对于尺度相同的点云ICP算法的原理细节，可以看另一篇博文《【动手学MVG】ICP算法原理和代码实现》

假设我有3D点集合 $S_1$ , 对该点集进行缩放, 旋转以及平移操作(即三维空间中的相似变换)后得到3D点集合 $S_2$ . 现在问题是:
如果给定上述的点集S1和S2, 并且已知点的匹配关系(即已知点集S1中每个点对应点集S2中的哪个点), 如何估计出一个相似变换, 使得点集S2进行相似变换后与点集1重合?

方法一

分别求解缩放倍数s和刚体变换Rt。
上述问题中, 如果点集 $S_1$ 和 $S_2$ 之间不存在缩放关系时(即尺度相同时), 可以用经典ICP( Iterative Closest Point )方法求解得到旋转矩阵R和平移向量t来进行点集对齐. 如果我们能首先估计出点集S1和S2之间的缩放倍数s(或称为尺度因子, 本文中两个称呼混用), 我们就可以利用ICP算法求解剩下的问题.

估计缩放倍数s

为了配准两组三维点集合, 我们需要找到这么一个相似变换矩阵 $\begin{bmatrix}sR & t \\ 0^T & 1\end{bmatrix}$ , 使得
$\begin{bmatrix} X_1^{S_1} \\ Y_1^{S_1} \\ Z_1^{S_1} \\ 1 \end{bmatrix}= \begin{bmatrix} sR & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} X_1^{S_2} \\ Y_1^{S_2} \\ Z_1^{S_2} \\ 1 \end{bmatrix} \tag{1}$
其中, $\begin{bmatrix}X_1^{S_1} \\ Y_1^{S_1} \\ Z_1^{S_1} \end{bmatrix}$ 表示三维点集 $S_1$ 中的一个三维点坐标 $P_1^{S_1}$ , $\begin{bmatrix}X_1^{S_2} \\ Y_1^{S_2} \\ Z_1^{S_2} \end{bmatrix}$ 表示三维点集 $S_2$ 中的与 $P_1^{S_1}$ 相匹配的一个三维点坐标, 记为 $P_1^{S_2}$ . 相似矩阵中的 $s$ 表示缩放倍数, R为3x3的旋转矩阵, t为3x1的平移向量, $0^T$ 为1x3的零向量, 1为常数.

为了求解缩放倍数s, 我们再找一对3D-3D匹配点 $P_2^{S_1}$ 和 $P_2^{S_2}$ , 有
$\begin{bmatrix} X_2^{S_1} \\ Y_2^{S_1} \\ Z_2^{S_1} \\ 1 \end{bmatrix}= \begin{bmatrix} sR & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} X_2^{S_2} \\ Y_2^{S_2} \\ Z_2^{S_2} \\ 1 \end{bmatrix} \tag{2}$

等式(2) - 等式(1), 有
$\begin{bmatrix} X_2^{S_1} - X_1^{S_1} \\ Y_2^{S_1} - Y_1^{S_1} \\ Z_2^{S_1} - Z_1^{S_1} \\ 0 \end{bmatrix}= \begin{bmatrix} sR & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} X_2^{S_2} - X_1^{S_2} \\ Y_2^{S_2} - Y_1^{S_2} \\ Z_2^{S_2} - Z_1^{S_2} \\ 0 \end{bmatrix} \tag{3}$
即
$\begin{bmatrix} X_2^{S_1} - X_1^{S_1} \\ Y_2^{S_1} - Y_1^{S_1} \\ Z_2^{S_1} - Z_1^{S_1} \end{bmatrix}= sR \begin{bmatrix} X_2^{S_2} - X_1^{S_2} \\ Y_2^{S_2} - Y_1^{S_2} \\ Z_2^{S_2} - Z_1^{S_2} \end{bmatrix} \tag{4}$
等式(4)两边取模值, 因为旋转矩阵R不影响向量长度,所以有
$\sqrt{(X_2^{S_1} - X_1^{S_1})^2 + (Y_2^{S_1} - Y_1^{S_1})^2 + (Z_2^{S_1} - Z_1^{S_1})^2}\\ =s\sqrt{(X_2^{S_2} - X_1^{S_2})^2 + (Y_2^{S_2} - Y_1^{S_2} )^2 + (Z_2^{S_2} - Z_1^{S_2})^2} \tag{5}$
因此可求得缩放倍数s
$s=\frac{\sqrt{(X_2^{S_1} - X_1^{S_1})^2 + (Y_2^{S_1} - Y_1^{S_1})^2 + (Z_2^{S_1} - Z_1^{S_1})^2}}{\sqrt{(X_2^{S_2} - X_1^{S_2})^2 + (Y_2^{S_2} - Y_1^{S_2} )^2 + (Z_2^{S_2} - Z_1^{S_2})^2}} \tag{6}$
等式(6)的物理含义是: 两个点集的"形状"相同, 在两个点集中分别找到一条对应的线段, 线段长度的比值就是这两个点集的缩放倍数.
得到缩放倍数后, 我们就可以把问题简化为ICP能处理的形式, 并使用ICP求解.

ICP配准尺度不一致(带缩放)的点云

已知点云间的缩放倍数s后, 上面等式(1)可看作:
$\begin{bmatrix} X_1^{S_1} \\ Y_1^{S_1} \\ Z_1^{S_1} \\ 1 \end{bmatrix}= \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} sX_1^{S_2} \\ sY_1^{S_2} \\ sZ_1^{S_2} \\ 1 \end{bmatrix} \tag{7}$
其中, $\begin{bmatrix}R & t \\ 0^T & 1\end{bmatrix}$ 为刚体变换矩阵, 只包含旋转和平移, 缩放倍数s已知. 等式(7)表示把点集 $S_2$ 的坐标都乘以缩放倍数s后, 得到的新的点集与点集 $S_1$ 之间的变换关系变为刚体变换. 而刚体变换可通过ICP算法求解. ( ICP求解过程这里不再赘述, 可参考《视觉SLAM十四讲》第一版, 7.9章《3D-3D: ICP》)
最后, 将求解出的s和R, t组合，就可以得到配准点集 $S_1$ 和 $S_2$ 之间所需的相似变换矩阵 $\begin{bmatrix}sR & t \\ 0^T & 1\end{bmatrix}$ .

方法一的代码实现

下面是算法主要代码文件，如果你还是有困惑，可以在这里直接下载整个工程（基于cmake）
PointCloudRegistration.h:

#ifndef POINTCLOUDREGISTRATION_H
#define POINTCLOUDREGISTRATION_H

#include <iostream>
#include <vector>
#include <opencv2/core/core.hpp>

// 这里的[sR|t]是pts2到pts1的变换。
// pts1 = [sR|t] * pts2
void pose_estimation_3d3d (
    const std::vector<cv::Point3f>& pts2,
    const std::vector<cv::Point3f>& pts1,
    float &scale, cv::Mat& R, cv::Mat& t
);

#endif

PointCloudRegistration.cpp:

#include "PointCloudRegistration.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
// 这里的[sR|t]是pts2到pts1的变换。
// pts1 = [sR|t] * pts2
void pose_estimation_3d3d (
    const std::vector<cv::Point3f>& pts2,
    const std::vector<cv::Point3f>& pts1,
    float &scale, cv::Mat& R, cv::Mat& t
)
{
    // estimate scale
    // TODO: scale计算可以取均值, 或者作为优化问题
    cv::Point3f point1_sub = pts1[0] - pts1[1];
    cv::Point3f point2_sub = pts2[0] - pts2[1];
    float sum1 = point1_sub.x*point1_sub.x + point1_sub.y*point1_sub.y + point1_sub.z*point1_sub.z;
    float sum2 = point2_sub.x*point2_sub.x + point2_sub.y*point2_sub.y + point2_sub.z*point2_sub.z;
    scale = sqrt(sum1/sum2);
    // printf("scale_estimated:%f \n", scale);

    std::vector<cv::Point3f> pts2_scaled(pts2.size());
    for(int32_t i=0; i<pts2.size(); ++i){
        pts2_scaled[i] = pts2[i] * scale;
    }



    cv::Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2_scaled[i];
    }
    p1 = cv::Point3f( cv::Vec3f(p1) /  N);
    p2 = cv::Point3f( cv::Vec3f(p2) / N);
    std::vector<cv::Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2_scaled[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    // cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    if (U.determinant() * V.determinant() < 0)
	{
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
	}
    
    // cout<<"U="<<U<<endl;
    // cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( cv::Mat_<float> ( 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_<float> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

main.cpp:

#include <iostream>
#include <opencv2/core/core.hpp>
#include "PointCloudRegistration.h"
#include <Eigen/Geometry>

using namespace std;
using namespace cv;

int main(){


    // 生成数据
    std::vector<cv::Point3f> pts1, pts2;
    pts1.push_back({1, 0, 0});
    pts1.push_back({1, 1, 0});
    pts1.push_back({0, 1, 0});
    pts1.push_back({0, 1, 1});
    pts1.push_back({1, 1, 1});


    float scale = 2.0f;
    cv::Mat T = (cv::Mat_<float>(4,4) << 
                0.997207*scale,    0.0583427*scale,  -0.046639*scale,    0.137988,
               -0.0578775*scale,   0.99826*scale,     0.0112663*scale,  -0.065517,
                0.0472151*scale,  -0.00853546*scale,  0.998848*scale,   -0.0298169,
                0,           0,           0,           1);
    for(int32_t i=0; i<pts1.size(); ++i){
        cv::Mat point = (cv::Mat_<float>(4,1) << pts1[i].x, pts1[i].y, pts1[i].z, 1.0f);
        
        cv::Mat point2 = T * point;

        pts2.push_back({point2.ptr<float>(0)[0], point2.ptr<float>(1)[0], point2.ptr<float>(2)[0]});
        // add noise
        // pts2.push_back({point2.ptr<float>(0)[0] + (float)rand()/RAND_MAX*0.01f, 
        //             point2.ptr<float>(1)[0] + (float)rand()/RAND_MAX*0.01f, 
        //             point2.ptr<float>(2)[0] + (float)rand()/RAND_MAX*0.01f});
        // cout << "pts2:" << pts2 << endl;
    }

    // 估计s[R|t]

    float scale_estimated;
    Mat R;
    Mat t;
    Mat sRt; // [sR|t]
    // pts1 = [sR|t] * pts2
    pose_estimation_3d3d ( pts1, pts2, scale_estimated, R, t);
    std::cout<<"ICP via SVD results: "<<std::endl;
    std::cout<<"R = "<<R<<std::endl;
    std::cout<<"t = "<<t<<std::endl;
    // std::cout<<"R_inv = "<<R.t() <<std::endl;
    // std::cout<<"t_inv = "<<-R.t() *t<<std::endl;
    hconcat(scale_estimated*R, t, sRt);
    
    // 对比估计值与真值
    cout << "sRt:" << sRt << endl;
    cout << "T  :" << T << endl;

    // TODO: 优化sRt

    return 0;
}