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軌跡），藍色為希望靠近的軌跡（等值線最近點軌跡），綠色為進行剛性變換之后的匹配軌跡：