matlab練習程序（圖優化）

無論是激光、視覺或者是慣導直接推出來的里程計通常會有回環誤差，通過圖優化的方式能夠將回環誤差最小化，從而提高建圖精度。

圖優化也是一種優化，所以能用常見的非線性優化方法來做，這里用到的高斯牛頓法，和之前ndt那一篇類似。

1.定義誤差函數：

我們定義Xi為i點位姿，Xj為j點位姿，Rij與Tij為回環模塊找到的一條i點到j點的位姿轉換。

誤差函數定義如下：

2.計算e對xi和xj的偏導，得到相應的雅克比矩陣：

 

 

3.對H矩陣和b向量進行迭代更新：

對H更新：

其中omiga是信息矩陣，我這里用的單位陣。

對b更新：

4.計算位姿變化增量deltax：

5.迭代到一定次數或者小於一定閾值退出即可。

網上能找的到基於matlab圖優化版本在下面這個鏈接，我的測試數據也來自該工程：

https://github.com/versatran01/graphslam

我按照原理公式又重寫了一遍，結合下面代碼和上面公式一起理解應該會比較清晰。

matlab代碼如下：

clear all;
close all;
clc;

efile = 'killian-e.dat';
vfile = 'killian-v.dat';

ef = fopen(efile);
vf = fopen(vfile);
vertices = fscanf(vf, 'VERTEX2 %d %f %f %f\n', [4,Inf]);
edges = fscanf(ef,'EDGE2 %d %d %f %f %f %f %f %f %f %f %f \n',[11,Inf]);

vmeans = vertices(2:4, vertices(1,:)+1)';
eids = (edges(1:2,:) + 1)';
emeans = edges(3:5,:)';

plot(vmeans(:,1),vmeans(:,2));
axis equal;

for k=1:5
    
    H = zeros(length(vmeans)*3);
    b = zeros(length(vmeans)*3,1);
    
    for i=1:length(eids)
        
        id_i = eids(i,1);
        id_j = eids(i,2);
        vi = vmeans(id_i,:);
        vj = vmeans(id_j,:);
        eij = emeans(i,:);
        
        ti = vi(1:2)';
        tj = vj(1:2)';
        tij = eij(1:2)';
        
        Ri = [cos(vi(3)) -sin(vi(3));sin(vi(3)) cos(vi(3))];
        Rj = [cos(vj(3)) -sin(vj(3));sin(vj(3)) cos(vj(3))];
        Rij = [cos(eij(3)) -sin(eij(3));sin(eij(3)) cos(eij(3))];
        
        err_ij = [Rij'*(Ri'*(tj-ti)-tij);tan(vj(3) - vi(3) - eij(3))];
        
        dRi = [-sin(vi(3)) -cos(vi(3));cos(vi(3)) -sin(vi(3))];
        A = [-Rij'*Ri' Rij'*dRi'*(tj-ti);zeros(1,2) -1];
        B = [Rij'*Ri' zeros(2,1);zeros(1,2) 1];
        
        H_ii = A'*A;
        H_ij = A'*B;
        H_ji = B'*A;
        H_jj = B'*B;
        
        bi = err_ij'*A;
        bj = err_ij'*B;
        
        H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) = H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) + H_ii;
        H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) = H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) + H_jj;
        H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) = H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) + H_ij;
        H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) = H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) + H_ji;
        b((id_i-1)*3+1:id_i*3,1) = b((id_i-1)*3+1:id_i*3,1) + bi';
        b((id_j-1)*3+1:id_j*3,1) = b((id_j-1)*3+1:id_j*3,1) + bj';  
    end
    
    H(1:3,1:3) = H(1:3,1:3) + eye(3);
    
    SH = sparse(H);
    deltax = -SH\b;
    
    newmeans = vmeans + reshape(deltax,3,length(vmeans))';    
    vmeans = newmeans;
    
end
figure;
plot(vmeans(:,1),vmeans(:,2))
axis equal;

結果如下：

優化前：

優化后：

測試數據下載地址：https://files.cnblogs.com/files/tiandsp/killian.zip