c++ 刚性变换矩阵 Eigen

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#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; //位移量
  
    }
}