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

在机器视觉学习中经常遇到计算旋转矩阵的问题，在这里整理最近学习的两种计算旋转矩阵的方法。

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);
}