通过两个坐标系对应点计算转换关系

通过两个坐标系对应点计算转换关系

应用

三维重建方法通常会自己估计相机的$R,T$矩阵，这些矩阵定义了一个世界坐标系，在使用客观的评估方法如Middlebury来评估精度时，需要使用评估方法提供的相机的$R,T$矩阵，这些矩阵定义了另外一个世界坐标系，两者通常会有尺度、旋转、平移的差别，这就需要在坐标系之间进行转换。

方法主要来源

1. Nghia Ho博客
2. 维基百科：Transformation_matrix
3. 代码所在文件夹名称：rigid_transform_3D

问题描述

尺度相同

两个相同尺度的世界坐标系可以通过$R,T$进行转换，计算转换关系需要知道双方$N$个对应点的坐标，设为$A,B$，则求解$B = R*A + T$即可。由于$N$可能比较大，因此此方程通常为超定方程，可使用奇异值分解(Singular Value Decomposition (SVD))进行计算，其内部原理是最小二乘法。

$H=\sum_{i=1}^{N}(P_{A}^{i}-centroid_{A})(P_{B}^{i}-centroid_{B})^{T}$

$[U,S,V]=SVD(H)$

$R=VU^{T}$

$T=-R*centroid_{A}+centroid_{B}$

其中$centroid_{A}$和$centroid_{B}$是$A,B$的平均中心。

尺度不同

当两个坐标系尺度不同时，$R$的计算同上，设两者的尺度倍数为$\lambda$，则

$\lambda =average\frac{\|(A-centroid_{A})\|}{\|(B-centroid_{B})\|}$

等量关系变为

$(B-centroid_{B})=\frac{1}{\lambda }R(A-centroid_{A})$

对以上等式进行化简得：

$B=\frac{1}{\lambda }RA-\frac{1}{\lambda }R*centroid_{A}+centroid_{B}$

因此最终要求的旋转矩阵和转移矩阵分别为$(\frac{1}{\lambda }RA)$和$(-\frac{1}{\lambda }R*centroid_{A}+centroid_{B})$

如何得到对应点坐标

以上方法的最重要的问题是如何得到对应点集$A,B$。一个具有$R,T$的相机，其相机中心在世界坐标系中的位置为$pos=-R^{T}T'$，分别计算出相机在两个世界坐标系下的位置，就可以得到一组对应点。

matlab核心代码

尺度相同的代码

Nghia Ho博客里的方法未考虑尺度，其核心代码为：

  %计算平均中心点
  centroid_A = mean(A);
  centroid_B = mean(B);

  N = size(A,1);

  H = (A - repmat(centroid_A, N, 1))' * (B - repmat(centroid_B, N, 1));

  [U,S,V] = svd(H);

  R = V*U';
  if det(R) < 0
  printf('Reflection detected\n');
  V(:,3) = -1*V(:,3);
  R = V*U';
  end

  t = -R*centroid_A' + centroid_B';
  detr=det(R)

作者的解释是：
There’s a special case when finding the rotation matrix that you have to take care of. Sometimes the SVD will return a ‘reflection’ matrix, which is numerically correct but is actually nonsense in real life. This is addressed by checking the determinant of R (from SVD above) and seeing if it’s negative (-1). If it is then the 3rd column of V is multiplied by -1.

if determinant(R) < 0
  multiply 3rd column of V by -1
  recompute R
end if

An alternative check that is possibly more robust was suggested by Nick Lambert, where R is the rotation matrix.

if determinant(R) < 0
  multiply 3rd column of R by -1
end if

尺度不同的代码

  centroid_A = mean(A);
  centroid_B = mean(B);

  N = size(A,1);

  H = (A - repmat(centroid_A, N, 1))' * (B - repmat(centroid_B, N, 1));

  A_move=A - repmat(centroid_A, N, 1);
  B_move=B - repmat(centroid_B, N, 1);

  A_norm=sum(A_move.*A_move,2);
  B_norm=sum(B_move.*B_move,2);

  %计算尺度平均值
  lam2=A_norm./B_norm;
  lam2=mean(lam2);

  [U,S,V] = svd(H);

  R = V*U';

  if det(R) < 0
  printf('Reflection detected\n');
  V(:,3) = -1*V(:,3);
  R = V*U';
  end
  %计算最终的旋转矩阵与平移向量
  t = -R./(lam2^(0.5))*centroid_A' + centroid_B';
  R = R./(lam2^(0.5));
  detr=det(R)

结果验证与误差计算

使用$A$和计算出的$R,T$计算出$B_{1}$，求其与真实值$B$之间的差别。

$P=\begin{bmatrix} R & T\ 0 & 1 \end{bmatrix}$

$\begin{bmatrix} B_{1}\ 1 \end{bmatrix}=P\begin{bmatrix} A\\ 1 \end{bmatrix}$

$err=\frac{1}{N}\|B-B_{1}\|$

matlab核心代码是：

A2 = (ret_R*A') + repmat(ret_t, 1, n);
A2 = A2';

% Find the error
err = A2 - B;
err = err .* err;
err = sum(err(:));
rmse = sqrt(err/n);

disp(sprintf('RMSE: %f', rmse));
disp('If RMSE is near zero, the function is correct!');

一些思考

1. 在思考尺度的影响时，直接用脑子想不太容易，而列出等量关系式之后进行化简，关系就清晰了；
2. 搜索解决方法时，用中文几乎搜不到，最后使用英文关键字corresponding points才搜到方法；
3. 将点集减去平均值点，其实就是将两个点集的一个对应点的坐标设置在了一起，即都为[0,0,0]，这样只要做相应的旋转就可以是两个坐标系重合，用来计算$R$，之后再使用一对对应点计算$T$，此时使用平均值点可以提高精度。