c++ 刚性变换矩阵 Eigen

网上只找到了python求刚性变换矩阵的代码，只能参考该代码自己写一个了，本人c++萌新，烦请大家帮忙指正错误，万分感谢！

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>  
void rigid_transform_3D(vector<vector<double>> A, vector<vector<double>> B)
{

    double centroid_A;
    double centroid_B;
    double sumAX = 0;
    double sumAY = 0;
    double sumAZ = 0;
    double sumBX = 0;
    double sumBY = 0;
    double sumBZ = 0;
    double mean_AX = 0;
    double mean_AY = 0;
    double mean_AZ = 0;
    double mean_BX = 0;
    double mean_BY = 0;
    double mean_BZ = 0;
    double AAX = 0;
    double AAY = 0;
    double AAZ = 0;
    double BBX = 0;
    double BBY = 0;
    double BBZ = 0;
    Eigen::Matrix< double, Eigen::Dynamic, 3> matrixA ;
    Eigen::Matrix< double, Eigen::Dynamic, 3> matrixB ;
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic> matrixAtrans ;
    Eigen::Matrix< double, 3, 3> matrixTrans;

    if(A.size() == B.size())
    {
        int size = A.size();
        Eigen::Vector3d centroid_A;
        Eigen::Vector3d centroid_B;
        for(int i = 0; i < size; i++)
        {
            sumAX += A[i][0];
            sumAY += A[i][1];
            sumAZ += A[i][2];
            sumBX += B[i][0];
            sumBY += B[i][1];
            sumBZ += B[i][2];
        }
        mean_AX = sumAX / size;
        mean_AY = sumAY / size;
        mean_AZ = sumAZ / size;
        mean_BX = sumBX / size;
        mean_BY = sumBY / size;
        mean_BZ = sumBZ / size;

        centroid_A << mean_AX, mean_AY, mean_AZ;
        centroid_B << mean_BX, mean_BY, mean_BZ;

        //矩阵A 的每列元素 减 该列的平均数
        for(int j = 0; j < size; j++)
        {
            AAX = A[j][0] - mean_AX;
            AAY = A[j][1] - mean_AY;
            AAZ = A[j][2] - mean_AZ;
            BBX = B[j][0] - mean_BX;
            BBY = B[j][1] - mean_BY;
            BBZ = B[j][2] - mean_BZ;
            matrixA.resize(size, 3);
            matrixA(j,0) = AAX;
            matrixA(j,1) = AAY;
            matrixA(j,2) = AAZ;
            matrixB.resize(size, 3);
            matrixB(j,0) = BBX;
            matrixB(j,1) = BBY;
            matrixB(j,2) = BBZ;
        }

        matrixAtrans = matrixA.transpose();
        matrixTrans = matrixAtrans * matrixB;   


        Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrixTrans, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::Matrix3d V = svd.matrixV(), U = svd.matrixU();
        Eigen::Matrix3d S = U.inverse() * matrixTrans * V.transpose().inverse();
        Eigen::Matrix3d Vt = V.transpose();
        Eigen::Matrix3d R = V * U.transpose(); //刚性变换矩阵
        Eigen::Vector3d t;
        double RDet = R.determinant();
        if (RDet < 0)
        {
            cout << "Reflection detected" << endl;
            Vt(2,0) = Vt(2,0)*(-1);
            Vt(2,1) = Vt(2,1)*(-1);
            Vt(2,2) = Vt(2,2)*(-1);
            R = Vt.transpose() * U.transpose();  //刚性变换矩阵          
        }
        t = -(R * centroid_A) + centroid_B; //位移量
  
    }
}

参考python代码链接： https://blog.csdn.net/qq_35565669/article/details/102748505

这里在svd上遇到了一点问题，numpy的svd结果和EigenSVD的结果不一样，具体问题可以参考这个链接 https://blog.csdn.net/weixin_38258767/article/details/107187633

矩阵忘得差不多了 不确定 if (RDet < 0)之后的操作是否正确，希望大佬们给点意见。

最后

以上就是风趣野狼为你收集整理的c++ 刚性变换矩阵 Eigen的全部内容，希望文章能够帮你解决c++ 刚性变换矩阵 Eigen所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。