在之前写的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矩阵的计算会受到点排序影响。具体如何出现这种结果，我还没有想明白，希望大家能够和我一起讨论！
谢谢大家啦！