SVD求解两组点云(2D/3D)刚体变换-数学推导/C++ Eigen实现

本文通过C++非类型模板形参（Non-type template parameters），实现基于SVD方法的2D/3D空间中两组点云的刚体变换求解，并基于C++17实现了多线程版本。

1、原理推导

笔者在 二维ICP配准算法-C++ 直接法实现中实现了对目标函数直接求导，并使导数等于0的直接配准算法。本文基于上述问题重新使用SVD求解2D/3D的刚体变换。

1.1 目标函数

已知空间中的两组点云$P=[p_1,p_2,......,p_n]$和$Q=[q_1,q_2,......,q_n]$,则求解刚体变换的最小而成问题可以被定义为：式(1)
$$(R,T)=\mathrm{argmin}\sum _{i=1}^n||(R{p}_i+T)-q_i|{|}^2$$

1.2 计算平移

首先假设旋转矩阵R是固定的，求目标函数对T的偏导数，并使导数等于零：式(2)
$$0=\sum _{i=1}^{n}2(R{p}_i+T-q_i)$$
展开得：式(3)
$$0=2R\sum _{i=1}^n{p}_i+2\sum _{i=1}^{n}T-2\sum _{i=1}^n{q}_i$$
移项得：式(4)
$$\sum _{i=1}^{n}T=\sum _{i=1}^n{q}_i-R\sum _{i=1}^n{p}_i$$
化简得：式(5)
$$T=\frac{{\sum }_{i=1}^n{q}_i}{{\sum }_{i=1}^n}-R\frac{{\sum }_{i=1}^n{p}_i}{{\sum }_{i=1}^n}$$
若：
$$q_{center}=\frac{{\sum }_{i=1}^n{q}_i}{{\sum }_{i=1}^n},p_{center}=\frac{{\sum }_{i=1}^n{p}_i}{{\sum }_{i=1}^n}$$
则：
$$T=q_{center}-R{p}_{center}$$
此时T表示的是两组点云质心之间的位移，将上述的T带入到原目标函数中：式(6)
$$(R,T)=\mathrm{argmin}\sum _{i=1}^n||(R{p}_i+q_{center}-R{p}_{center})-q_i|{|}^2$$
整理得：式(7)
$$(R,T)=\mathrm{argmin}\sum _{i=1}^n||R(p_i-p_{center})-(q_i-q_{center})|{|}^2$$
根据上述公式可以得知，我们可以通过对点云进行去质心操作，来使位移为零，进而能够使我们单独求解旋转。令：
$$x_i=p_i-p_{center},y_i=q_i-q_{center}$$
则原目标函数可以被表示为：式(8)
$$R=\mathrm{argmin}\sum _{i=1}^n||R{x}_i-y_i|{|}^2$$

1.3 计算旋转

对式(8)进行简化得：式(9)
$$\begin{gathered} \mathrm{argmin}\sum _{i=1}^n||R{x}_i-y_i|{|}^2 \\ =\sum _{i=1}^n(R{x}_i-y_i{)}^T(R{x}_i-y_i) \\ =\sum _{i=1}^n(x_i^T{R}^T-y_i^T)(R{x}_i-y_i) \\ =\sum _{i=1}^n{x}_i^T{R}^{T}R{x}_i-y_i^{T}R{x}_i-x_i^T{R}^T{y}_i+y_i^T{y}_i \end{gathered}$$
由于R是正交矩阵满足$R{R}^T=I$,进一步可以化简为：式(10)
$$\begin{gathered} R=\mathrm{argmin}\sum _{i=1}^n{x}_i^T{x}_i-2y_i^{T}R{x}_i+y_i^T{y}_i \\ =\mathrm{argmin}\sum _{i=1}^n-2y_i^{T}R{x}_i \\ =\mathrm{argmax}\sum _{i=1}^n{y}_i^{T}R{x}_i \end{gathered}$$
考虑如下形式的矩阵乘法：
$$\begin{gathered} Y^{T}RX=\left[\begin{array}{c}{y}_1^T\\ y_2^T\\ .\\ .\\ y_n^T\end{array}\right]\left[\begin{array}{ccc}& & \\ & R& \\ & & \end{array}\right]\left[\begin{array}{ccccc}{x}_1& x_2& .& .& x_n\end{array}\right] \\ =\left[\begin{array}{c}{y}_1^T\\ y_2^T\\ .\\ .\\ y_n^T\end{array}\right]\left[\begin{array}{ccccc}R{x}_1& R{x}_2& .& .& R{x}_n\end{array}\right] \\ =\left[\begin{array}{ccccc}{y}_1^{T}R{x}_1& & & & \ast \\ & y_2^{T}R{x}_2& & & \\ & & .& & \\ & & & .& \\ \ast & & & & y_n^{T}R{x}_n\end{array}\right] \end{gathered}$$
如上所示，我们注意到：$$\sum _{i=1}^n{y}_i^{T}R{x}_i=tr(Y^{T}RX)$$
因此式(10)的优化问题可以转化成求解$M=Y^{T}RX$矩阵迹最大化的问题；矩阵的迹有如下性质：
$$tr(AB)=tr(BA)$$
因此有 式(11)：
$$tr(Y^{T}RX)=tr(Y^T(RX))=tr((RX)Y^T)$$
令$X{Y}^T=M$，则有M矩阵的SVD：
$$S=UD{V}^T$$
将S的SVD结果带入到式11得：式(12)
$$tr(RX{Y}^T)=tr((RU)(D{V}^T))=tr(D{V}^{T}RU)$$
由于U，R和V都是正交矩阵，则根据SO(3)得性质$M=V^{T}RU$也是正交矩阵，这意味着M得每一列都是正交向量，正交向量有如下性质：
$$m_j^T{m}_j=1\Rightarrow \sum _{i=1}^n{m}_{ij}^2=1\Rightarrow |m_{ij}|\le 1$$
根据上述性质，可以推出：
$$tr(DM)\le tr(D)$$
矩阵D(奇异值降序排列)是各个元素都大于0的对角矩阵；因此当M对角线上的各个元素等于1的时候tr(DM)有最大值，最大值为tr(D)。换句话说由于M是正交矩阵，且其对角线元素等于1的时候有最大值，那么这就意味着此时M是单位矩阵。因此可以推出：
$$I=M=V^{T}RU\Rightarrow R=V{U}^T$$

1.4 瑕旋转(旋转+反射(镜像))

上述的推导过程只能保证R是正交矩阵，而无法保证其是旋转矩阵（旋转矩阵一定是正交矩阵，但正交矩阵不一定是旋转矩阵，旋转矩阵的行列式为1，正交矩阵行列式为$\pm 1$）。因此可以通过$V{U}^T$的行列式的值，判断其是否包含反射。如下矩阵是反射变换（反射再乘一个反射是没反射）。
$$M_{reflect}=\left[\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & .& & \\ & & & .& \\ & & & & -1\end{array}\right]$$
因此完整的计算旋转的公式为：
$$R=V\left[\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & .& & \\ & & & .& \\ & & & & det(V{U}^T)\end{array}\right]U^T$$

1.4 算法流程

1. 计算两个点云的质心
   $$q_{center}=\frac{{\sum }_{i=1}^n{q}_i}{{\sum }_{i=1}^n},p_{center}=\frac{{\sum }_{i=1}^n{p}_i}{{\sum }_{i=1}^n}$$
2. 点云去质心
   $$x_i=p_i-p_{center},y_i=q_i-q_{center}$$
3. 求解相关性矩阵S
   $$S=X{Y}^T$$
4. 对矩阵S进行奇异值分解$S=UD{V}^T$，计算旋转
   $$R=V\left[\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & .& & \\ & & & .& \\ & & & & det(V{U}^T)\end{array}\right]U^T$$
5. 计算平移
   $$T=q_{center}-R{p}_{center}$$

2、C++ Eigen实现

// rigid_transform.hpp
#ifndef RIGID_TRANSFORM_H
#define RIGID_TRANSFORM_H
#include <Eigen/Dense>
#include <execution>

template <int dim>
class RigidTransform
{
public:
    using PointType = Eigen::Matrix<double, dim, 1>;
    using RigidTransformType = Eigen::Matrix<double, dim + 1, dim + 1>;

    RigidTransform() = default;
    ~RigidTransform() = default;
    RigidTransformType solve(const std::vector<PointType> &P,
                             const std::vector<PointType> &Q);
    RigidTransformType solve(const Eigen::Matrix<double, dim, dim> &M,
                             const Eigen::Matrix<double, dim, 1> &P_center,
                             const Eigen::Matrix<double, dim, 1> &Q_center);

private:
    PointType normalize_point_(std::vector<PointType> &points);
    PointType normalize_point_mt_(std::vector<PointType> &points);
    Eigen::Matrix<double, dim, dim> compute_M(const std::vector<PointType> &X,
                                              const std::vector<PointType> &Y);
    Eigen::Matrix<double, dim, dim> compute_M_mt(const std::vector<PointType> &X,
                                                 const std::vector<PointType> &Y);
};

template <int dim>
Eigen::Matrix<double, dim + 1, dim + 1> RigidTransform<dim>::solve(const std::vector<PointType> &P,
                                                                   const std::vector<PointType> &Q)
{
    if (P.size() != Q.size() || P.size() < dim)
    {
        throw std::runtime_error("point cloud size must be >= 3 and source size == target size!");
    }

    auto X = P;
    auto Y = Q;

#ifdef USE_CPP17_MT
    // Point cloud normalize multi thread
    PointType P_center = normalize_point_mt_(X);
    PointType Q_center = normalize_point_mt_(Y);
    // Compute M multi thread
    Eigen::Matrix<double, dim, dim> S = compute_M_mt(X, Y);
#else
    // Point cloud normalize single thread
    PointType P_center = normalize_point_(X);
    PointType Q_center = normalize_point_(Y);
    // Compute M single thread
    Eigen::Matrix<double, dim, dim> S = compute_M(X, Y);
#endif

    return solve(S, P_center, Q_center);
}

template <int dim>
Eigen::Matrix<double, dim + 1, dim + 1> RigidTransform<dim>::solve(const Eigen::Matrix<double, dim, dim> &S,
                                                                   const Eigen::Matrix<double, dim, 1> &P_center,
                                                                   const Eigen::Matrix<double, dim, 1> &Q_center)
{
    // Compute Rotation
    Eigen::JacobiSVD<Eigen::Matrix<double, dim, dim>> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix<double, dim, dim> U = svd.matrixU();
    Eigen::Matrix<double, dim, dim> V = svd.matrixV();

    Eigen::Matrix<double, dim, dim> D = Eigen::Matrix<double, dim, dim>::Identity();
    D(dim - 1, dim - 1) = (V * U.transpose()).determinant();
    Eigen::Matrix<double, dim, dim> R = V * D * U.transpose();

    // Compute Translation
    PointType T = Q_center - R * P_center;

    // Construct SE(3)
    RigidTransformType SE3 = RigidTransformType::Identity();
    SE3.block<dim, dim>(0, 0) = R;
    SE3.block<dim, 1>(0, dim) = T;

    return SE3;
}

template <int dim>
Eigen::Matrix<double, dim, 1> RigidTransform<dim>::normalize_point_(std::vector<PointType> &points)
{
    // Get point cloud center
    PointType center = PointType::Zero();
    for (size_t i = 0; i < points.size(); ++i)
    {
        center += points[i];
    }
    center = center / points.size();

    // Remove center
    for (size_t i = 0; i < points.size(); ++i)
    {
        points[i] = points[i] - center;
    }

    return center;
}

template <int dim>
Eigen::Matrix<double, dim, 1> RigidTransform<dim>::normalize_point_mt_(std::vector<PointType> &points)
{
    // Get point cloud center
    PointType center = std::reduce(points.begin(), points.end(), PointType::Zero().eval(),
                                   [](const PointType &sum, const PointType &item)
                                   { return sum + item; }) /
                       points.size();

    // Remove center
    std::transform(points.begin(), points.end(), points.begin(), [&center](const PointType &p)
                   { return p - center; });

    return center;
}

template <int dim>
Eigen::Matrix<double, dim, dim> RigidTransform<dim>::compute_M(const std::vector<PointType> &X,
                                                               const std::vector<PointType> &Y)
{
    Eigen::Matrix<double, dim, dim> M = Eigen::Matrix<double, dim, dim>::Zero();

    for (size_t i = 0; i < X.size(); ++i)
    {
        M += X[i] * Y[i].transpose();
    }

    return M;
}

template <int dim>
Eigen::Matrix<double, dim, dim> RigidTransform<dim>::compute_M_mt(const std::vector<PointType> &X,
                                                                  const std::vector<PointType> &Y)
{
    std::vector<size_t> index(X.size());
    std::for_each(index.begin(), index.end(), [idx = 0](size_t &i) mutable
                  { i = idx++; });
    std::vector<Eigen::Matrix<double, dim, dim>> M_vec(index.size());
    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const size_t &i)
                  { M_vec[i] = X[i] * Y[i].transpose(); });

    return std::reduce(M_vec.begin(), M_vec.end(), Eigen::Matrix<double, dim, dim>::Zero().eval(),
                           [](const Eigen::Matrix<double, dim, dim> &sum, const Eigen::Matrix<double, dim, dim> &item)
                           { return sum + item; });
}

#endif

3、测试

// test.cc
#define USE_CPP17_MT

#include "rigid_transform.hpp"

template <typename FuncT>
void evaluate_and_call(FuncT &&func, int run_num)
{
    double total_time = 0.f;
    for (int i = 0; i < run_num; ++i)
    {
        auto t1 = std::chrono::high_resolution_clock::now();
        func();
        auto t2 = std::chrono::high_resolution_clock::now();
        total_time +=  std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;;
    }

    std::cout << "run cost: " << total_time / 1000.f << std::endl;
}

void test_2Drigid_transform()
{
    std::vector<Eigen::Vector2d> P;
    P.push_back(Eigen::Vector2d(100, 0));
    P.push_back(Eigen::Vector2d(0, 100));

    Eigen::Matrix2d R;
    double theta = CV_PI / 6;
    R << std::cos(theta), -std::sin(theta),
        std::sin(theta), std::cos(theta);

    Eigen::Vector2d T(10, 10);

    std::vector<Eigen::Vector2d> Q;
    for (const auto &p : P)
    {
        Eigen::Vector2d transformed = R * p + T;
        Q.push_back(transformed);
    }

    RigidTransform<2> rigid_transform_2d;
    Eigen::Matrix3d SE2 = rigid_transform_2d.solve(P, Q);
    std::cout << "SE2: " << std::endl;
    std::cout << SE2 << std::endl;
}

void test_3Drigid_transform()
{
    std::vector<Eigen::Vector3d> P;
    P.push_back(Eigen::Vector3d(100, 0, 0));
    P.push_back(Eigen::Vector3d(0, 100, 0));
    P.push_back(Eigen::Vector3d(0, 0, 100));

    Eigen::AngleAxisd rotateX(CV_PI / 6, Eigen::Vector3d::UnitX());
    Eigen::Vector3d T(10, 10, 10);

    std::vector<Eigen::Vector3d> Q;
    for (const auto &p : P)
    {
        Eigen::Vector3d transformed = rotateX.matrix() * p + T;
        Q.push_back(transformed);
    }

    RigidTransform<3> rigid_transform_2d;
    Eigen::Matrix4d SE3 = rigid_transform_2d.solve(P, Q);
    std::cout << "SE3: " << std::endl;
    std::cout << SE3 << std::endl;
}

int main()
{
    std::cout << "================3D================" << std::endl;
    evaluate_and_call(test_3Drigid_transform, 1);
    std::cout << "================2D================" << std::endl;
    evaluate_and_call(test_2Drigid_transform, 1);

    return 0;
}

4、程序输出

================3D================
SE3:
           1            0            0           10
           0     0.866025         -0.5           10
           0          0.5     0.866025           10
           0            0            0            1
run cost: 0.0028997
================2D================
SE2:
0.866025     -0.5       10
     0.5 0.866025       10
       0        0        1
run cost: 0.0008708

4、总结

本文通过C++非类型模板形参（Non-type template parameters），实现了ICP算法中常用的刚体变换求解方法。只需要通过在声明类模板的时候设置2或3，就可以无缝切换2D和3D刚体变换求解（当然你也可以选择直接把3D点的最后一个维度置为0，直接使用3D算法求解2D问题，但这需要进行数据转换，本文通过C++模板避免了这个问题），并且你可以通过定义宏的方式来切换成多线程算法。