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

## 方法主要来源

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

## 问题描述

### 尺度相同

H=Ni=1(PiAcentroidA)(PiBcentroidB)T$H=\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=RcentroidA+centroidB$T=-R*centroid_{A}+centroid_{B}$

### 尺度不同

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

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

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

## 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)

### 结果验证与误差计算

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=1NBB1$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$R$，之后再使用一对对应点计算T$T$，此时使用平均值点可以提高精度。

05-14 1082

11-10 1666

01-16 5887

05-05 1万+

06-09 2万+

07-09 852

03-18 9313

07-10 4771

04-19 192

#### 物体坐标转换 ，同一个物体不同坐标系表达，求坐标系关系

©️2020 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客

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