机器视觉——旋转矩阵的计算（二）

参考文献该算法是opencv采用的四元数解法，参见 Using Quaternions to Calculate RMSD
xk和yk是两组三维点，计算R和T使得yk=R∗xk+T.
1.计算点中心x¯=∑xk/n,y¯=∑yk/n
2.区中心xk~=xk−y¯,yk~=yk−y¯
3.计算协方差矩阵R
4.计算矩阵Q

5.对矩阵Q其最大特征值对应的特征向量即为最后的旋转矩阵的四元数表示，将其转换为旋转矩阵，T=y-R*x
以机器视觉中的P3P问题为例，部分代码如下：

bool p3p::align(double M_end[3][3],                // 两组向量集求旋转矩阵，参见Kabsch algorithm（最小化方均根误差）    double X0, double Y0, double Z0,               // opencv采用等价的四元数解法，参见 Using Quaternions to Calculate RMSD    double X1, double Y1, double Z1,    double X2, double Y2, double Z2,    double R[3][3], double T[3]){    // Centroids:                                  // 世界坐标系到相机坐标系的对齐    double C_start[3], C_end[3];    for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;    C_start[0] = (X0 + X1 + X2) / 3;    C_start[1] = (Y0 + Y1 + Y2) / 3;    C_start[2] = (Z0 + Z1 + Z2) / 3;    // Covariance matrix s:    double s[3 * 3];    for(int j = 0; j < 3; j++) {        s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];        s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];        s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];    }    double Qs[16], evs[4], U[16];    Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];    Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];    Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];    Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];    Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];    Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];    Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];    Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];    Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];    Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];    jacobi_4x4(Qs, evs, U);    // Looking for the largest eigen value:    int i_ev = 0;    double ev_max = evs[i_ev];    for(int i = 1; i < 4; i++)        if (evs[i] > ev_max)            ev_max = evs[i_ev = i];    // Quaternion:    double q[4];    for(int i = 0; i < 4; i++)        q[i] = U[i * 4 + i_ev];    double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3];    double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];    double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];    double q2_3 = q[2] * q[3];    R[0][0] = q02 + q12 - q22 - q32;    R[0][1] = 2. * (q1_2 - q0_3);    R[0][2] = 2. * (q1_3 + q0_2);    R[1][0] = 2. * (q1_2 + q0_3);    R[1][1] = q02 + q22 - q12 - q32;    R[1][2] = 2. * (q2_3 - q0_1);    R[2][0] = 2. * (q1_3 - q0_2);    R[2][1] = 2. * (q2_3 + q0_1);    R[2][2] = q02 + q32 - q12 - q22;    for(int i = 0; i < 3; i++)        T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]);    return true;}