图优化理论


文章目录


1 资料

内容摘选自下面这几篇博客：

引用的是下面的英文课程：

另外还有下面的做法：

2 理论整理

SLAM问题的处理方法主要分为滤波和图优化两类。滤波的方法中常见的是扩展卡尔曼滤波、粒子滤波、信息滤波等，熟悉滤波思想的同学应该容易知道这类SLAM问题是递增的、实时的处理数据并矫正机器人位姿。比如基于粒子滤波的SLAM的处理思路是假设机器人知道当前时刻的位姿，利用编码器或者IMU之类的惯性导航又能够计算下一时刻的位姿，然而这类传感器有累计误差，所以再将每个粒子的激光传感器数据或者图像特征对比当前建立好的地图中的特征，挑选和地图特征匹配最好的粒子的位姿当做当前位姿，如此往复。当然在gmapping、hector_slam这类算法中，不会如此轻易的使用激光数据，激光测距这么准，当然不能只用来计算粒子权重，而是将激光数据与地图环境进行匹配（scan match）估计机器人位姿，比用编码器之流精度高出很多。

图优化SLAM问题能够分解成两个任务：

  1. 构建图，机器人位姿当做顶点，位姿间关系当做边，这一步常常被成为前端（front-end），往往是传感器信息的堆积。

  2. 优化图，调整机器人位姿顶点尽量满足边的约束，这一步称为后端（back-end）。

图优化过程如下图所示：先堆积数据，机器人位姿为构建的顶点。边是位姿之间的关系，可以是编码器数据计算的位姿，也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。构建的图和原始未经优化的地图如下：

20200512145935


l_{i j} \propto\left[\mathbf{z}_{i j}-\hat{\mathbf{z}}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right]^{T} \mathbf{\Omega}_{i j}\left[\mathbf{z}_{i j}-\hat{\mathbf{z}}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right]


lij​∝[zij​−z^ij​(xi​,xj​)]TΩij​[zij​−z^ij​(xi​,xj​)]
上式和前面的误差平方和函数很像，只不过这里显式的指明了误差函数的形式。所以我们发现，误差的权重矩阵（正式名称为信息矩阵）等于协防差矩阵的逆。由于图优化里每一条边代表一个测量值，如表示相邻位姿关系的编码器测量值或者图像（激光）匹配得到的位姿变换矩阵。所以图优化里每一条边的信息矩阵就是这些测量协防差矩阵的逆。如果协防差越小，表示这次测量越准越值得相信，信息权重就越大。

采用开源数据集，并运行下面代码。这里的工作是将开源的代码简化之后。

clear all;
close all;
% clc;

tic
%%=========================建立位姿图=======================================
load pgraph;

node.pose = pgraph.pose;
figure;plot(pgraph.pose(1,:),pgraph.pose(2,:))
[unused, node.num] = size(node.pose);
edge = pgraph.edge;
%%=========================================================================

% node = 0;
% edge = 0;

%%=========================优化位姿图=======================================

figure;
    plot(node.pose(1, :), node.pose(2, :));
for iterate_num = 1:5
    H = zeros(node.num*3);
    b = zeros(node.num*3, 1);
    %%建立H矩阵
    %------------------------测试代码--------------------------------------
    e_r = zeros(1, length(edge));
    er_index = 1;
    %---------------------------------------------------------------------
    for edge_index=1:length(edge)
        %%边的起点
        i = edge(edge_index).from;
        
        %%边的终点
        j = edge(edge_index).to;
%         disp([i,j])
        
        %%边起点的位姿
        ip = node.pose(:, i);
        %%边终点的位姿
        jp = node.pose(:, j);
        %%边的相对位姿估计信息
        zp = edge(edge_index).z;
        
        %%边的二次型误差矩阵
        omega = edge(edge_index).omega;
        
        %%旋转矩阵
        Ri = [cos(ip(3)) -sin(ip(3));sin(ip(3)) cos(ip(3))];
        Rj = [cos(jp(3)) -sin(jp(3));sin(jp(3)) cos(jp(3))];
        Rz = [cos(zp(3)) -sin(zp(3));sin(zp(3)) cos(zp(3))];
        
        %%求导
        dRi = [-sin(ip(3)) -cos(ip(3));cos(ip(3)) -sin(ip(3))]; 
        %%误差
        e = Rz'*Ri'*(jp(1:2)-ip(1:2))-Rz'*zp(1:2);
        
        e(3) = mod(jp(3)-ip(3)-zp(3), 2*pi);
        if e(3) > pi
            e(3) = e(3)-2*pi;
        end
        
        %------------------------测试代码--------------------------------------
        e_r(er_index) = e'*e;
        er_index = er_index+1;
        %---------------------------------------------------------------------
        %%Jacobian矩阵
        %对pose i的偏导
        A = [-Rz'*Ri' , Rz'*dRi'*(jp(1:2)-ip(1:2));
            0 0 -1];
        %对Pose j的偏导
        B = [Rz'*Ri' , [0;0]; 0 0 1];
        
        Hii = A'*omega*A;  %%omega 信息矩阵
        Hjj = B'*omega*B;
        Hij = A'*omega*B;
        
        bi = -A'*omega*e;
        bj = -B'*omega*e;
        
        ii = (i-1)*3+[1:3];
        jj = (j-1)*3+[1:3];
        
        H(ii, ii) = H(ii, ii)+Hii;
        H(ii, jj) = H(ii, jj)+Hij;
        H(jj, jj) = H(jj, jj)+Hjj;
        H(jj, ii) = H(jj, ii)+Hij';
        
        b(ii) = b(ii)+bi;
        b(jj) = b(jj)+bj;
    end
    
    %%解方程组
    %固定顶点1
    H(1:3, 1:3) = H(1:3, 1:3)  +  1000*eye(3);
    
%     H = H+1e-5*diag(diag(H));
    H_sparse = sparse(H);
    
    %%进行行列最小度排序，使cholesky分解后的三角矩阵稀疏。有开源代码。
    reorder_index = colamd(H_sparse);
    
    %要替换
    R = chol(H_sparse(reorder_index, reorder_index)).';
    
    tmpv = R \ b(reorder_index);
    
    dx = R' \ tmpv;
    
    dx(reorder_index) = dx;
%     dx = H_sparse\b;
    
    dx(1:3) = 0;
    dx = reshape(dx, 3, node.num);
    
    node.pose = node.pose + dx;
    
    figure;
    plot(e_r);
    pause(0.2);
    figure;
    plot(node.pose(1, :), node.pose(2, :));
    pause(0.5);
end
%%=========================================================================
toc

3 图优化项目

将扩展卡尔曼滤波的方法改成基于图优化的项目。