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

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

应用

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

方法主要来源

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

问题描述

尺度相同

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

H=Ni=1(PiAcentroidA)(PiBcentroidB)T

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

R=VUT

T=RcentroidA+centroidB

其中centroidAcentroidBA,B平均中心

尺度不同

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

λ=average(AcentroidA)(BcentroidB)

等量关系变为

(BcentroidB)=1λR(AcentroidA)

对以上等式进行化简得:

B=1λRA1λRcentroidA+centroidB

因此最终要求的旋转矩阵和转移矩阵分别为(1λRA)(1λRcentroidA+centroidB)

如何得到对应点坐标

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

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计算出B1,求其与真实值B之间的差别。

P=[RT 01]

[B1 1]=P[A1]

err=1NBB1

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,此时使用平均值点可以提高精度。
©️2020 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客 返回首页
实付0元
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值