ICCP算法——刚性变换

在之前的一篇随笔中，通过MATLAB代码实现了ICCP算法中提取等值线和寻找等值线最近点的功能。详情见链接：https://www.cnblogs.com/huangliu1111/p/13089188.html

1、线段集合距离定义

根据1999年的文章《Vehicle localization on gravity maps》，接下来，需要实现ICCP中最重要的步骤，即对等值线最近点构成的线段集合进行刚性变换。刚性变换包括旋转R和平移两个步骤。希望通过刚性变换，使INS指示轨迹线段集合与等值线最近点线段集合的距离最小。

对于线段集合的最近距离描述，该文献直接引用了另一篇1997年的文献《Matching Sets of 3D Line Segments with Application to Polygonal Arc Matching》中的定义。

利用一个线段的中点坐标a、方向向量b和长度l来描述该线段。线段上的所有点均可用这三个参数来表达。两个线段上相对于中点位置相同的点视为一组相关点，所有相关点间的欧式距离之和用于衡量两个线段之间的距离，相当于“相似程度”。

2、旋转矩阵和平移向量

利用旋转矩阵R与线段端点坐标的乘积运算，实现对线段的旋转变换。能够使线段集合距离最小的旋转角度直接通过交叉协方差矩阵S解得（见参考文献《Matching Sets of 3D Line Segments with Application to Polygonal Arc Matching》）。旋转矩阵利用四元数表示。

在线段的计算和变换过程中，主要考虑线段的中点和线段集合的质心。

MATLAB代码如下：

clc;
clear all;

xi=[0,-2.5; 2,-1.5; 4.5, 0];
yi=[1,-0.5; 3,-1; 5,0];
% 寻找使目标函数最小的变换方式（有权重）

% 权重：线段长度决定
w=0;
w_sum=0;
N=3;
for i=1:N-1
    w(i)=((xi(i+1,1)-xi(i,1))^2+(xi(i+1,2)-xi(i,2))^2)^(1/2);     %线段长度
    x(i,1)=(xi(i+1,1)+xi(i,1))/2;
    x(i,2)=(xi(i+1,2)+xi(i,2))/2;
    y(i,1)=(yi(i+1,1)+yi(i,1))/2;
    y(i,2)=(yi(i+1,2)+yi(i,2))/2;
    w_sum=w_sum+w(i); %总长度
end
% 质心
x_center=zeros(1,2);
y_center=zeros(1,2);
for i=1:N-1
    x_center(1)=x_center(1)+w(i)*x(i,1);
    x_center(2)=x_center(2)+w(i)*x(i,2);
    y_center(1)=y_center(1)+w(i)*y(i,1);
    y_center(2)=y_center(2)+w(i)*y(i,2);
end
x_center=x_center/w_sum;
y_center=y_center/w_sum;
% 交叉协方差矩阵
S=0;
for i=1:N-1
    S=S+w(i)*((y(i,:)-y_center(1,:))'*(x(i,:)-x_center(1,:)));
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=-2*atan(-(S(1,1)+S(2,2)-lamda_max)/(S(1,2)-S(2,1)));
% 旋转矩阵R和平移向量t
R=[cos(theta), -sin(theta);
    sin(theta), cos(theta)];      %关于坐标系原点旋转
t=y_center'-R*x_center';

% 变换结果
for i=1:N
    xis(i,:) = (R*(xi(i,1:2)') +t)';
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轨迹），蓝色为希望靠近的轨迹（等值线最近点轨迹），绿色为进行刚性变换之后的匹配轨迹：