在之前的一篇隨筆中，通過MATLAB代碼實現(xiàn)了ICCP算法中提取等值線和尋找等值線最近點的功能。詳情見鏈接：https://www.cnblogs.com/huangliu1111/p/13089188.html

1、線段集合距離定義

根據(jù)1999年的文章《Vehicle localization on gravity maps》，接下來，需要實現(xiàn)ICCP中最重要的步驟，即對等值線最近點構(gòu)成的線段集合進行剛性變換。剛性變換包括旋轉(zhuǎn)R和平移兩個步驟。希望通過剛性變換，使INS指示軌跡線段集合與等值線最近點線段集合的距離最小。

對于線段集合的最近距離描述，該文獻直接引用了另一篇1997年的文獻《Matching Sets of 3D Line Segments with Application to Polygonal Arc Matching》中的定義。

利用一個線段的中點坐標a、方向向量b和長度l來描述該線段。線段上的所有點均可用這三個參數(shù)來表達。兩個線段上相對于中點位置相同的點視為一組相關(guān)點，所有相關(guān)點間的歐式距離之和用于衡量兩個線段之間的距離，相當于“相似程度”。

2、旋轉(zhuǎn)矩陣和平移向量

利用旋轉(zhuǎn)矩陣R與線段端點坐標的乘積運算，實現(xiàn)對線段的旋轉(zhuǎn)變換。能夠使線段集合距離最小的旋轉(zhuǎn)角度直接通過交叉協(xié)方差矩陣S解得（見參考文獻《Matching Sets of 3D Line Segments with Application to Polygonal Arc Matching》）。旋轉(zhuǎn)矩陣利用四元數(shù)表示。

在線段的計算和變換過程中，主要考慮線段的中點和線段集合的質(zhì)心。

MATLAB代碼如下：

clc;
clear all;
xi=[0,-2.5; 2,-1.5; 4.5, 0];
yi=[1,-0.5; 3,-1; 5,0];
% 尋找使目標函數(shù)最小的變換方式（有權(quán)重）
% 權(quán)重：線段長度決定
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
% 質(zhì)心
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;
% 交叉協(xié)方差矩陣
S=0;
for i=1:N-1
S=S+w(i)*((y(i,:)-y_center(1,:))'*(x(i,:)-x_center(1,:)));
end
% 求解旋轉(zhuǎn)矩陣
% 最大特征值
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)));
% 旋轉(zhuǎn)矩陣R和平移向量t
R=[cos(theta), -sin(theta);
sin(theta), cos(theta)]; %關(guān)于坐標系原點旋轉(zhuǎn)
t=y_center'-R*x_center';
% 變換結(jié)果
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;

變換結(jié)果如圖所示，黑色為原始軌跡（INS軌跡），藍色為希望靠近的軌跡（等值線最近點軌跡），綠色為進行剛性變換之后的匹配軌跡：

總結(jié)

以上是生活随笔為你收集整理的ICCP算法——刚性变换的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。