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
', [4,Inf]);
edges = fscanf(ef,'EDGE2 %d %d %f %f %f %f %f %f %f %f %f 
',[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;
    
    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