平面中两点集对应的旋转和平移

最新推荐文章于 2024-07-29 14:27:08 发布

大马哈鱼007

最新推荐文章于 2024-07-29 14:27:08 发布

阅读量1.2k

点赞数 1

转载：https://blog.csdn.net/qq_28467367/article/details/88647858

                                        <p>在两组对应的三维点数据之间寻找最佳的旋转和平移，使它们对齐/注册，是我遇到的一个常见问题。下面给出了3个对应点的最简单情况(需要解决的最小点)。<br>

在这里插入图片描述

对应点的颜色相同，R是旋转，t是平移。我们想要找到将数据集A中的点与数据集b对齐的最佳旋转和平移。这种变换有时被称为欧几里德变换或刚性变换，因为它保持了形状和大小。这与仿射变换形成对比，仿射变换包括缩放和剪切。

我将给出的解决方案来自于Besl和McKay在1992年发表的论文:
“A Method for Registration of 3-D Shapes’, by Besl and McKay, 1992.”

解决方案概述

我们正在求解方程中的R,t:
B = R*A + t，
其中R,t是应用于数据集A的变换，使其与数据集B尽可能对齐。求最优刚性变换矩阵可分解为以下步骤:
1.求两个数据集的质心
2.将两个数据集带到原点，然后求最优旋转(矩阵R)
3.求平移t

求质心

求质心这一点很简单，质心只是平均点，可以计算如下：
在这里插入图片描述

求最优旋转

有几种方法可以找到点之间的最佳旋转。我发现最简单的方法是使用奇异值分解(SVD)，

为了找到最佳的旋转，我们首先将两个数据集重新居中，使两个中心点都位于原点，如下图所示。
在这里插入图片描述
H是协方差矩阵。

特殊情况

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® < 0
multiply 3rd column of R by -1
end if

在这里插入图片描述

Matlab代码

% This function finds the optimal Rigid/Euclidean transform in 3D space
% It expects as input a Nx3 matrix of 3D points.
% It returns R, t

% You can verify the correctness of the function by copying and pasting these commands:
%{

R = orth(rand(3,3)); % random rotation matrix

if det® < 0
V(:,3) = -1;
R = VU’;
end

t = rand(3,1); % random translation

n = 10; % number of points
A = rand(n,3);
B = R*A’ + repmat(t, 1, n);
B = B’;

[ret_R, ret_t] = rigid_transform_3D(A, B);

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!”);

% expects row data
function [R,t] = rigid_transform_3D(A, B)
if nargin != 2
error(“Missing parameters”);
end

assert(size(A) == size(B))

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) &lt; 0
    printf("Reflection detected\n");
    V(:,3) = -1*V(:,3);
    R = V*U';
end

t = -R*centroid_A' + centroid_B'

end

在这里插入图片描述

 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® < 0
printf(‘Reflection detected\n’);
V(:,3) = -1V(:,3);
R = VU’;
end
%计算最终的旋转矩阵与平移向量
t = -R./(lam2^(0.5))*centroid_A’ + centroid_B’;
R = R./(lam2^(0.5));
end

参考自：
http://nghiaho.com/?page_id=671
https://blog.csdn.net/sinat_29886521/article/details/77506426?utm_source=blogxgwz0

                                </div>
            <link href="https://csdnimg.cn/release/phoenix/mdeditor/markdown_views-e44c3c0e64.css" rel="stylesheet">
                </div>