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

应用

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

方法主要来源

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

问题描述

尺度相同

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

H=∑Ni=1(PiA−centroidA)(PiB−centroidB)TH=\sum_{i=1}^{N}(P_{A}^{i}-centroid_{A})(P_{B}^{i}-centroid_{B})^{T}

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

R=VUT R=VU^{T}

T=−R∗centroidA+centroidB T=-R*centroid_{A}+centroid_{B}

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

尺度不同

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

λ=average∥(A−centroidA)∥∥(B−centroidB)∥ \lambda =average\frac{\|(A-centroid_{A})\|}{\|(B-centroid_{B})\|}

等量关系变为

(B−centroidB)=1λR(A−centroidA) (B-centroid_{B})=\frac{1}{\lambda }R(A-centroid_{A})

对以上等式进行化简得：

B=1λRA−1λR∗centroidA+centroidB B=\frac{1}{\lambda }RA-\frac{1}{\lambda }R*centroid_{A}+centroid_{B}

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

如何得到对应点坐标

以上方法的最重要的问题是如何得到对应点集 A,B A,B。一个具有 R,T R,T的相机，其相机中心在世界坐标系中的位置为 pos=−RTT′ 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) < 0printf('Reflection detected\n');V(:,3) = -1*V(:,3);R = V*U';endt = -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) < 0multiply 3rd column of V by -1recompute 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) < 0multiply 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) < 0printf('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 A和计算出的R,TR,T计算出 B1 B_{1}，求其与真实值 B B之间的差别。

P=[RT 01]P=\begin{bmatrix} R & T\ 0 & 1 \end{bmatrix}

[B1 1]=P[A1] \begin{bmatrix} B_{1}\ 1 \end{bmatrix}=P\begin{bmatrix} A\\ 1 \end{bmatrix}

err=1N∥B−B1∥ 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!');

一些思考

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