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

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