在机器视觉学习中经常遇到计算旋转矩阵的问题，在这里整理最近学习的两种计算旋转矩阵的方法。
1.参考点击打开链接中文献，该方法也是opencv中epnp算法计算旋转矩阵的方法
假设Pc和Pw是一组3d点在不同坐标系下的坐标，计算旋转矩阵R和平移向量T使得Pc=Pw*R+T
/**R存储旋转矩阵 ***t存储平移量 ***pc**pw存储点坐标 ***num点的个数 */ void estimate_R_and_t(double R[3][3], double t[3], double *pcs, double *pws, int number_of_correspondences) // 计算R和t （Horn，...） { double pc0[3], pw0[3]; pc0[0] = pc0[1] = pc0[2] = 0.0; pw0[0] = pw0[1] = pw0[2] = 0.0; for (int i = 0; i < number_of_correspondences; i++) { const double * pc = &pcs[3 * i]; const double * pw = &pws[3 * i]; for (int j = 0; j < 3; j++) { pc0[j] += pc[j]; pw0[j] += pw[j]; } } for (int j = 0; j < 3; j++) { // 求出pcs和pws的几何中心 pc0[j] /= number_of_correspondences; pw0[j] /= number_of_correspondences; } double abt[3 * 3], abt_u[3 * 3], abt_v[3 * 3],abt_vt[9]; for (int i = 0; i < 9; i++) { abt[i] = 0; } for (int i = 0; i < number_of_correspondences; i++) { double * pc = &pcs[3 * i]; double * pw = &pws[3 * i]; for (int j = 0; j < 3; j++) { abt[3 * j] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]); // 对 各个pcs和pws点减去各自的几何中心所形成的矩阵 进行svd分解 abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]); abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]); } } dluav(abt, 3, 3, abt_u, abt_vt, 0.00001, 4);//对abt进行svd分解 ZJ_Share_Trans(3, 3, abt_vt, abt_v); for (int i = 0; i < 3; i++) // 计算旋转矩阵 for (int j = 0; j < 3; j++) R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j); const double det = R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] - R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1]; if (det < 0) { R[2][0] = -R[2][0]; R[2][1] = -R[2][1]; R[2][2] = -R[2][2]; } t[0] = pc0[0] - dot(R[0], pw0); // 计算平移矩阵 t[1] = pc0[1] - dot(R[1], pw0); t[2] = pc0[2] - dot(R[2], pw0); }