ICCP算法——刚性变换2

在之前写的ICCP算法随笔中，使用了线段组匹配的方法。讨论过程中发现这个方法相比于点集匹配复杂并且难理解多了，因此想按照原论文的想法，把点集匹配的方法也用MATLAB实现了一下。

重新写代码的过程中发现，S矩阵解算得到的最佳旋转角度theta的正负会影响点集的旋转方向，因此需要计算匹配点到等值线最近点yi的距离，选择能够使变换后的点集与等值线最近点集yi距离最小的旋转角度。

% 修改transformation中的线段组匹配法，改成点集匹配法
clc;
clear all;
% 测试数据

% xi=[0,-2.5; 2,-1.5; 4.5, 0];   % INS轨迹
% yi=[1,-0.5; 3,-1; 5,0];        % 等值线最近点

xi=[4.5, 0; 2,-1.5; 0,-2.5];   % INS轨迹
yi=[5,0; 3,-1; 1,-0.5];        % 等值线最近点

xi = xi';                      % 注意格式为列：横坐标，纵坐标；行：不同的点坐标
yi = yi';
% 寻找使目标函数最小的变换方式（有权重）
% 权重：全部为1，所有点视为同等重要
w=0;            % 权重
w_sum=0;        % 总权重
N=3;
for i=1:N       % 分配各点权重
    w(i) = 1;               % 假设权重为1
%     w(i) = N + 1 - i;           % 假设按时间顺序，排在前面的点更重要
    w_sum = w_sum + w(i);
end
% 计算带权重的质心
x_center=zeros(2,1);
y_center=zeros(2,1);
for k=1:N
    x_center(1) = x_center(1) + w(i) * xi(1, k);
    x_center(2) = x_center(2) + w(i) * xi(2, k);   
    y_center(1) = y_center(1) + w(i) * yi(1, k);                 %真实轨迹
    y_center(2) = y_center(2) + w(i) * yi(2, k);
end
x_center=x_center/w_sum;              %权重由标号定义
y_center=y_center/w_sum;
% 交叉协方差矩阵
S = 0;
for i = 1 : N
    S = S + w(i) * (yi(:, k) - y_center(:)) * ((xi(:, k) - x_center(:))');
end
% 求解旋转矩阵
% 最大特征值
lamda(1)=((S(1,1)+S(2,2))^2+(S(1,2)-S(2,1))^2)^(1/2);
lamda(2)=-lamda(1);
lamda(3)=((S(1,1)-S(2,2))^2+(S(1,2)+S(2,1))^2)^(1/2);
lamda(4)=-lamda(3);
lamda_max=max(lamda);

% 判断旋转方向
theta(1) = 2 * atan(-(S(1,1)+S(2,2)-lamda_max)/(S(1,2)-S(2,1)));
% 旋转矩阵R和平移向量t
R=[cos(theta(1)), -sin(theta(1));
    sin(theta(1)), cos(theta(1))];      %关于坐标系原点旋转
t=y_center-R*x_center;
% 变换结果
for i=1:N
    xis_1(:,i) = t + R*xi(:,i);
end
% 计算点之间的距离
sum_norm_1 = 0;
for i = 1 : N
    sum_norm_1 = w(i) * norm(yi(:,i)-xis_1(:,i),2);
end
theta(2) = -2 * atan(-(S(1,1)+S(2,2)-lamda_max)/(S(1,2)-S(2,1)));    % 旋转方向如何确定
% 旋转矩阵R和平移向量t
R=[cos(theta(2)), -sin(theta(2));
    sin(theta(2)), cos(theta(2))];      %关于坐标系原点旋转
t=y_center-R*x_center;
% 变换结果
for i=1:N
    xis_2(:,i) = t + R*xi(:,i);
end

% 计算点之间的距离
sum_norm_2 = 0;
for i = 1 : N
    sum_norm_2 = w(i) * norm(yi(:,i)-xis_2(:,i),2);
end
if sum_norm_1 > sum_norm_2
    xis = xis_2;
else
    xis = xis_1;
end

plot(xi(1,:),xi(2,:),'o-k');
hold on;
plot(yi(1,:),yi(2,:),'*-b');
hold on;
plot(xis(1,:),xis(2,:),'*-g');
hold on;

变换后的结果如下图所示，黑色为初始点集（INS轨迹）xi，蓝色为等值线最近点集yi，绿色为匹配后的结果。

当我仅仅调换测试数据点集xi和yi中的点排列顺序，发现会得到另一个看起来对称的匹配结果，如下图两条绿色曲线所示：

为什么输入点的顺序不同，得到的最佳匹配结果也不同，产生两个看似对称的结果？这可能是因为S矩阵的计算会受到点排序影响。具体如何出现这种结果，我还没有想明白，希望大家能够和我一起讨论！

谢谢大家啦！