点云刚体配准，并计算误差

#include <vector>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

Eigen::MatrixXd repmat(Eigen::MatrixXd matrix, int row, int col)
{
    Eigen::MatrixXd matrixExtend;
    if (row < 1 || col < 1)
    {
        return matrixExtend;
    }

    int extend_rows = matrix.rows();
    int extend_cols = matrix.cols();

    matrixExtend = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>();//动态矩阵初始化

    matrixExtend.resize(matrix.rows(), matrix.cols());
    matrixExtend = matrix;
    for (int colExtend = 1; colExtend < col; colExtend++)
    {
        extend_cols = extend_cols + matrix.cols();
        Eigen::MatrixXd matrixTemp;
        matrixTemp = matrixExtend;
        matrixExtend.resize(extend_rows, extend_cols);//改变要扩展矩阵的维度
        matrixExtend << matrixTemp, matrix;
    }

    Eigen::MatrixXd matrixRow;
    matrixRow = matrixExtend;
    for (int rowExtend = 1; rowExtend < row; rowExtend++)
    {
        extend_rows = extend_rows + matrix.rows();
        Eigen::MatrixXd matrixTemp;
        matrixTemp = matrixExtend;
        matrixExtend.resize(extend_rows, extend_cols);//改变要扩展矩阵的维度
        matrixExtend << matrixTemp, matrixRow;
    }

    return matrixExtend;
}



Eigen::Matrix4d Regist(std::vector<double> srcdata, std::vector<double> dstdata, double& err )
{
    //深度拷贝原始数据 原因是 Eigen 去质心 会修改原始数据
    int col = 3;
    int row = srcdata.size()/3;
    int N = srcdata.size()/3;
    // SVD计算点集配准,输入两个对应点云，输出旋转矩阵、平移向量
    Eigen::MatrixXd src = Eigen::Map<Eigen::MatrixXd>(srcdata.data(), col, row).transpose();
    Eigen::MatrixXd dst = Eigen::Map<Eigen::MatrixXd>(dstdata.data(), col, row).transpose();

    // 1、计算质心
    Eigen::RowVector3d srcMean = src.colwise().mean();  // 每列求均值
    Eigen::RowVector3d tarMean = dst.colwise().mean();
    // 2、去质心
    Eigen::MatrixXd A = src.rowwise() - srcMean;
    Eigen::MatrixXd B = dst.rowwise() - tarMean;
    // 3、构建协方差矩阵
    Eigen::Matrix3d covMat = (A.transpose() * B) / A.rows();
    // 4、SVD分解求旋转矩阵
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(covMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXd V = svd.matrixV();
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd R =  V * U.transpose() ;

    //通过检查R的行列式（来自上面的SVD）并查看它是否为负（-1）可以解决这个问题。如果是，则 V 的第 3 列乘以 -1。
    double det  = R.determinant() < 0? -1: 1;
    Eigen::MatrixXd matM = Eigen::Matrix3d::Identity();
    matM<<1, 0, 0,
          0, 1, 0,
          0, 0, det;
    R =  V * matM * U.transpose() ;

    // 5、求平移向量
    Eigen::MatrixXd T = tarMean.transpose() - R* srcMean.transpose();

    Eigen::MatrixXd matrix = Eigen::Matrix4d::Identity();
    matrix.block<3,3>(0,0) = R;
    matrix.block<3,1>(0,3) = T;

    Eigen::MatrixXd inpB = (R * src.transpose() + repmat(T, 1, N)).transpose();

//    std::cout<<"Regist Matrix\n"<<matrix<<std::endl<<std::endl;
//    std::cout<<"Regist inpB: \n"<<inpB<<std::endl<<std::endl;
    err = 0.0;
    for (size_t i = 0; i < N; ++i)
    {
        auto distance =  (dst.row(i) - inpB.row(i)).norm();
        err += distance * distance;
    }
    err = sqrt(err/N);
//    std::cout<<"err: "<<err<<std::endl<<std::endl;
    return matrix;
}