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