根据点对求解刚体变换矩阵

刚体变换 是指在三维空间中，把一个物体做旋转、平移，是一种保持物体大小和形状不变的仿射变换，刚体变换又称为 欧式变换、齐次变换。

问题描述¶

在点云配准的课题下，当我们做完点云匹配之后，将获得N个点对（下标一一对应）

\(A = \{p_1, p_2, p_3, ..., p_n\}\)
\(B=\{p'_1, p'_2, p'_3, ..., p'_n\}\)

此时，我们需要根据，这N个点对，解算出刚体变换矩阵中，旋转矩阵\(R\)、偏移量\(t\)，使得

\[ {\forall}_{i}, p_i = R*p'_i + t \]

SVD方法¶

流程简介¶

计算两个点云的质心¶

首先，分别计算两个点云的质心，记 \(c\)为\(A\)的质心，\(c'\)为B的质心

计算方法：质心 = 点云的坐标算数平均值

Point centroid; //A质心

for pnt in A:
    centroid += pnt

centroid /= A.size();

去质心坐标¶

将每个点的坐标转换为**去质心坐标**，计算方法

记\(q_i\)为\(A\)的去质心坐标：\(q_i = p_i - c\)
记\(q'_i\)为\(B\)的去质心坐标：\(q'_i = p'_i - c'\)

计算旋转矩阵¶

首先，计算 \(W\)，这里\(W\)显然是3x3的矩阵

\[ W = \sum_{i}q_i * (\sum_i q'_i)^{T} \]

对\(W\)进行SVD分解，得到

\[ W = U \sum V^T \]

当\(W\)满秩时，则\(R=UV^T\)

求平移量¶

\[ t = p - R*p' \]

原理可参考文献

K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of two 3-d point sets,” PatternAnalysisandMachineIntelligence, IEEETransactionson,no.5,pp.698–700,1987.
F. Pomerleau, F. Colas, and R. Siegwart, “A review of point cloud registration algorithms for mobile robotics,” Foundations and Trends in Robotics (FnTROB), vol. 4, no. 1, pp. 1– 104, 2015.

opencv代码¶

完整代码：opencv-examples/solve_transform_matrix.cpp

#include<opencv2/opencv.hpp>

cv::Mat solveTransformMatrix(const std::vector<cv::Point3d>& A, const std::vector<cv::Point3d>& B)
{
    int numOfPnts = A.size();

    //# 至少3个点对
    if(numOfPnts < 3 || numOfPnts != B.size())
    {
        return {};
    }

    //# 计算质心
    cv::Point3d centroidA, centroidB;
    {
        for (int i{ 0 }; i < numOfPnts; ++i)
        {
            centroidA += A[i];
            centroidB += B[i];
        }

        centroidA /= numOfPnts;
        centroidB /= numOfPnts;
    }

    //# 去质心坐标
    cv::Mat ASubCentroid(3, numOfPnts, CV_64FC1);
    cv::Mat BSubCentroid(3, numOfPnts, CV_64FC1);

    for (int i = 0; i < numOfPnts; ++i)//N组点
    {
        //三行
        ASubCentroid.at<double>(0, i) = A[i].x - centroidA.x;
        ASubCentroid.at<double>(1, i) = A[i].y - centroidA.y;
        ASubCentroid.at<double>(2, i) = A[i].z - centroidA.z;

        BSubCentroid.at<double>(0, i) = B[i].x - centroidB.x;
        BSubCentroid.at<double>(1, i) = B[i].y - centroidB.y;
        BSubCentroid.at<double>(2, i) = B[i].z - centroidB.z;
    }

    //# 计算旋转矩阵
    cv::Mat matS = ASubCentroid * BSubCentroid.t();

    cv::Mat matU, matW, matV;
    cv::SVDecomp(matS, matW, matU, matV);

    cv::Mat matTemp = matU * matV;
    double det = cv::determinant(matTemp);//行列式的值

    double datM[] = { 1, 0, 0, 0, 1, 0, 0, 0, det };
    cv::Mat matM(3, 3, CV_64FC1, datM);

    cv::Mat matR = matV.t() * matM * matU.t();

    //# 计算平移量
    double* datR = (double*)(matR.data);
    double delta_X = centroidB.x - (centroidA.x * datR[0] + centroidA.y * datR[1] + centroidA.z * datR[2]);
    double delta_Y = centroidB.y - (centroidA.x * datR[3] + centroidA.y * datR[4] + centroidA.z * datR[5]);
    double delta_Z = centroidB.z - (centroidA.x * datR[6] + centroidA.y * datR[7] + centroidA.z * datR[8]);

    //# 转成4x4矩阵
    cv::Mat R_T = (cv::Mat_<double>(4, 4) <<
        matR.at<double>(0, 0), matR.at<double>(0, 1), matR.at<double>(0, 2), delta_X,
        matR.at<double>(1, 0), matR.at<double>(1, 1), matR.at<double>(1, 2), delta_Y,
        matR.at<double>(2, 0), matR.at<double>(2, 1), matR.at<double>(2, 2), delta_Z,
        0, 0, 0, 1
        );

    return R_T;
}

Eigen代码¶

完整代码：cgal-examples/solve_transform_matrix.cpp

#include<CGAL/Simple_cartesian.h>
using Kernel = CGAL::Simple_cartesian<double>;
using Point3 = Kernel::Point_3;

#include<Eigen/Core>
#include<Eigen/Dense> //Matrix.determinant
#include<Eigen/SVD>

Eigen::Matrix4d solveTransformMatrix(const std::vector<Point3>& A, const std::vector<Point3>& B)
{
    int numOfPnts = A.size();

    //# 至少3个点对
    if (numOfPnts < 3 || numOfPnts != B.size())
    {
        return {};
    }

    //# 计算质心
    double centroidA[3] = {0}, centroidB[3] = {0};
    {
        for (int i{ 0 }; i < numOfPnts; ++i)
        {
            for (int j{ 0 }; j < 3; ++j)
            {
                centroidA[j] += A[i][j];
                centroidB[j] += A[i][j];
            }
        }

        for (int j{ 0 }; j < 3; ++j)
        {
            centroidA[j] /= numOfPnts;
            centroidB[j] /= numOfPnts;
        }
    }

    //# 去质心坐标
    Eigen::MatrixXd ASubCentroid(3, numOfPnts);
    Eigen::MatrixXd BSubCentroid(3, numOfPnts);

    for (int i = 0; i < numOfPnts; ++i)//N组点
    {
        //三行
        ASubCentroid(0, i) = A[i].x() - centroidA[0];
        ASubCentroid(1, i) = A[i].y() - centroidA[1];
        ASubCentroid(2, i) = A[i].z() - centroidA[2];

        BSubCentroid(0, i) = B[i].x() - centroidB[0];
        BSubCentroid(1, i) = B[i].y() - centroidB[1];
        BSubCentroid(2, i) = B[i].z() - centroidB[2];
    }

    //# 计算旋转矩阵
    auto matS = ASubCentroid * BSubCentroid.transpose();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(matS, Eigen::ComputeFullU | Eigen::ComputeFullV);
    auto matU = svd.matrixU();
    auto matV = svd.matrixV().transpose();

    auto matTemp = matU * matV;
    double det = matTemp.determinant();//行列式的值

    Eigen::Matrix3d matM;
    matM << 1, 0, 0, 0, 1, 0, 0, 0, det;
    auto matR = matV.transpose() * matM * matU.transpose();

    ////# 计算平移量
    double delta_X = centroidB[0] - (centroidA[0] * matR(0, 0) + centroidA[1] * matR(0, 1) + centroidA[2] * matR(0, 2));
    double delta_Y = centroidB[1] - (centroidA[0] * matR(1, 0) + centroidA[1] * matR(1, 1) + centroidA[2] * matR(1, 2));
    double delta_Z = centroidB[2] - (centroidA[0] * matR(2, 0) + centroidA[1] * matR(2, 1) + centroidA[2] * matR(2, 2));

    //# 转成4x4矩阵
    Eigen::Matrix4d RT;
    RT <<
        matR(0, 0), matR(0, 1), matR(0, 2), delta_X,
        matR(1, 0), matR(1, 1), matR(1, 2), delta_Y,
        matR(2, 0), matR(2, 1), matR(2, 2), delta_Z,
        0, 0, 0, 1;

    return RT;
}