非线性优化（二）：徒手实现LM算法

我应该还没有这么勤快地更新过专栏文章...但上周刚刚写了关于常见的最小二乘算法的简介。其实公式推导在我的文章第六讲：非线性优化之中已经有了，而非线性优化（一）：优化方法中，关于各个算法的推导和伪代码介绍也很通俗。但无论怎样，talking is easy, show me the code. 而且，也为了让我自己对最小二乘算法更加的熟悉，这篇文章里我们就来徒手实现LM算法。

因为我自己常用的进行算法验证的语言是MATLAB，所以这里也用MATLAB来做了。但所有代码只是借用了MATLAB的矩阵操作，如果换成Python，或者利用Eigen来用C++实现也是很简单的。

废话不多说，直接开始吧。

如果你看过我的文章卡尔曼滤波：从入门到精通，应该记得里面我们是从一个一维的例子开始，慢慢推导到高维。这里也一样，而且也借用里面的一维小车的例子来进行数据模拟。

假设我们的待优化变量，也就是图优化中的节点，是一维小车运动是的坐标x；而测量是一个测距仪，每次记录的是小车前后时刻之间的位移。为了验证代码，让我们先用一个非常非常简单的模拟数据。

vertex = [0; 1.1; 0.2];
edges = [2, 1, 1;
         3, 2, -1;
         1, 3, 0];

这里，（假设就是IMU吧），我们得到了小车的3个位置：0，1.1和0.2。

而对于边edges而言，重要的是所连接的节点和测量值。因此，一行数据中，第一列是k+1时刻，第二列是k时刻，而第三列是k到k+1时刻测量得到的小车位移。对应地，error = edges(i, 3) - (vertex(edges(i, 1) - vertex(edges(i, 2)).

因此可以看到，3个时刻中，小车的实际位置应该是0, 1和0。这是为了模拟一个闭环。

现在我们就徒手实现实现一个很粗糙的LM算法，来对这个数据进行平差。

clc
clear

vertex = [0; 1.1; 0.2];
edges = [2, 1, 1;
         3, 2, -1;
         1, 3, 0];
     
idx1 = 1; idx2 = 2; measure = 3;

r = 0;
for i = 1 : length(vertex)
    e_sqrt = edges(i, measure) - (vertex(edges(i, idx1)) - vertex(edges(i, idx2)));
    r = r + e_sqrt' * e_sqrt;
end
iter_num = 100;

residuals = zeros(iter_num + 1, 1);
residuals(1) = r;
vertices = zeros(iter_num + 1, length(vertex));
vertices(1, :) = vertex;

u = 0.2;
iter = 0;
while iter < iter_num
    iter = iter + 1;
    H = zeros(length(vertex));
    b = zeros(length(vertex), 1);
    for i = 1 : length(vertex)
        % residuals
        e_sqrt = edges(i, measure) - (vertex(edges(i, idx1)) - vertex(edges(i, idx2)));
        % jacobian
        J_1 = -1;
        J_2 = 1;
        % hession
        H(edges(i, idx1), edges(i, idx1)) = H(edges(i, idx1), edges(i, idx1)) + J_1' * J_1;
        H(edges(i, idx2), edges(i, idx2)) = H(edges(i, idx2), edges(i, idx2)) + J_2' * J_2;
        H(edges(i, idx1), edges(i, idx2)) = H(edges(i, idx1), edges(i, idx2)) + J_1' * J_2;
        H(edges(i, idx2), edges(i, idx1)) = H(edges(i, idx2), edges(i, idx1)) + J_2' * J_1;
        % b
        b(edges(i ,idx1)) = b(edges(i ,idx1)) - J_1 * e_sqrt;
        b(edges(i ,idx2)) = b(edges(i ,idx2)) - J_2 * e_sqrt;
    end

    H = H + u * eye(size(H));
    
    delta = H \ b;

    vertex = vertex + delta;
    r = 0;
    for i = 1 : length(vertex)
        e_sqrt = edges(i, measure) - (vertex(edges(i, idx1)) - vertex(edges(i, idx2)));
        r = r + e_sqrt' * e_sqrt;
    end
    
    vertices(iter + 1, :) = vertex;
    residuals(iter + 1) = r;
    iter = iter + 1;
end

vertex'

虽然先说了是个粗糙的LM算法，但我知道你肯定在吐槽：这也太粗糙了。其实得益于我们的例子是个很简单的一维线性case，所以一些算法的实现部分还是很简单的。而且对于真正的工程实现而言，一般也都是先用这个很low的例子来验证算法的正确性和有效性，再进行优化。

言归正传，看完这段代码，相信你很容易发现其中还有很多东西没有完成。

这个算法傻乎乎地迭代了100次，其实从第8次开始，残差的数值就在 $10^{-11}$ 左右了。上一篇文章讲的各种停止迭代的方式我们一个都没用上。
LM算法的一部分就是阻尼因子u的更新，我们也没有实现。
迭代的最终结果是0.1000 1.1000 0.1000。精确倒是挺精确，但是其实一般我们都会要求第1项位置保持不变。

好了，为了解决以上问题，我们再对算法进行一下更新。

clc
clear

vertex = [0; 1.1; 0.2];
edges = [2, 1, 1;
         3, 2, -1;
         1, 3, 0];
     
idx1 = 1; idx2 = 2; measure = 3;

r = 0;
for i = 1 : length(vertex)
    e_sqrt = edges(i, measure) - (vertex(edges(i, idx1)) - vertex(edges(i, idx2)));
    r = r + e_sqrt' * e_sqrt;
end
iter_num = 100;

residuals = zeros(iter_num + 1, 1);
residuals(1) = r;
vertices = zeros(iter_num + 1, length(vertex));
vertices(1, :) = vertex;

sigma_1 = 1e-8;
sigma_2 = 1e-6;
u = 0.2;
v = 2;
iter = 0;
ii = 1;
found = false;
while false == found && iter < iter_num
    iter = iter + 1;
    H = zeros(length(vertex));
    b = zeros(length(vertex), 1);
    for i = 1 : length(vertex)
        % residuals
        e_sqrt = edges(i, measure) - (vertex(edges(i, idx1)) - vertex(edges(i, idx2)));
        % jacobian
        J_1 = -1;
        J_2 = 1;
        % hession
        H(edges(i, idx1), edges(i, idx1)) = H(edges(i, idx1), edges(i, idx1)) + J_1' * J_1;
        H(edges(i, idx2), edges(i, idx2)) = H(edges(i, idx2), edges(i, idx2)) + J_2' * J_2;
        H(edges(i, idx1), edges(i, idx2)) = H(edges(i, idx1), edges(i, idx2)) + J_1' * J_2;
        H(edges(i, idx2), edges(i, idx1)) = H(edges(i, idx2), edges(i, idx1)) + J_2' * J_1;
        % b
        b(edges(i ,idx1)) = b(edges(i ,idx1)) - J_1 * e_sqrt;
        b(edges(i ,idx2)) = b(edges(i ,idx2)) - J_2 * e_sqrt;
    end
    H(1, 1) = H(1, 1) + 1e4;
    H = H + u * eye(size(H));
    
    delta = H \ b;
    
    if norm(delta) < sigma_2 * (norm(vertex) + sigma_2)
        found = true;
    else
        vertex_new = vertex + delta;
        r = 0;
        for i = 1 : length(vertex_new)
            e_sqrt = edges(i, measure) - (vertex_new(edges(i, idx1)) - vertex_new(edges(i, idx2)));
            r = r + e_sqrt' * e_sqrt;
        end
        
        dL = delta' * (u * delta + b);
        dF = (residuals(ii) - r)' * (residuals(ii) + r);
        if dL > 0 && dF > 0
            vertex = vertex_new;
            v = 2;
            u = u * max(1/3, 1 - (2 * dF / dL - 1)^3);
            
            vertices(ii + 1, :) = vertex;
            residuals(ii + 1) = r;
            ii = ii + 1;
            
            if abs(max(b)) <= sigma_1
                found = true;
            end
        else
            u = u * v;
            v = 2 * v;
        end
    end
end

vertex'

OK，如此，我们就实现了一个很简单的LM算法。虽然这肯定是不够的，但是我们已经有了一个可用的算法雏形了。

现在，让我们来实现一个二维的例子。（而且还是线性的，因为我实在是太懒了....因为不想弄处理流形，这里的核心是优化算法，关于求导等可以参见我的李群和李代数系列文章）

如果读者知道g2o（稍微了解就可了）的话，就知道它的代码是很抽象和模块化的。

对于节点而言，重要的是3个东西:

节点的编号（我们的数据量很小，这里就用数组下标代替了）；
初值（我们也通过数组直接给了）；
和更新方式（g2o中的oplusImpl，需要用户自己定义）；

当然，还有节点的类型和维度，这两个是通过初始化类的模板参数得到的，我们并不涉及。

而对于边，重要的也是3个东西：

所连接的顶点和测量值；
残差计算（g2o中的computeError，需要用户实现，毕竟你的误差模型是啥只有你自己知道）；
雅可比矩阵的计算（g2o中的linearOplusImpl，用户可以不实现，g2o会通过算法帮你求导，怎么做的我们可以后面有时间再来介绍）。

所以，我们按照这个来对代码进行模块化（当然不是实现类，只是会抽象出函数出来而已）。已经上面的实现可能不太友好，比如我们的雅可比和海塞矩阵的计算，因为正常来说都是让用户实现一个雅可比矩阵计算的函数，而不是这样直接算海塞矩阵。

现在，我们有了个能在二维平面上运动的小车，对应的节点和边是这样的：

vertex = [0, 0; 1.2, 0; 2.3, 0; 3.2, 0; 3.2, 0.6; 3.2, 1.3; 3.2, 1.6; 3.1, 1.6; 1.8, 1.6; 1.1, 1.6; 0.1, 1.6; 0.1, 1.2; 0.1, 0.3];
edges = [2, 1, 1.3, 0;
         3, 2, 0.9, 0;
         4, 3, 0.8, 0;
         5, 4, 0, 0.8;
         6, 5, 0, 0.6;
         7, 6, 0, 0.1;
         8, 7, -0.2, 0;
         9, 8, -1.1, 0;
         10, 9, -0.9, 0;
         11, 10, -0.8, 0;
         12, 11, 0, -0.6;
         13, 12, 0, -0.75;
         1, 13, 0, 0];

这里，edges每一行的前两维和之前是一样的；其中，第三维是两个时间间隔之间x坐标的差异，第四维是两个时间间隔之间y坐标的差异。把它们画出来的话就是

同样，小车走了个闭环，但我们得到的imu测量值却相去甚远。对应的优化代码为:

写在前面：虽然这个算法很简单，但在实现这所有算法的时候我也踩了一些坑，也出现了算法运行地不对的时候。所以知道了理论还是不够，实践出真知。看别人写代码总觉得很容易，自己开始写了才发现不是这样的。所以看完了前半部分一维情况的代码，希望你可以自己动手实现下这个二维的case. 毕竟前面的代码已经把很多事情都做好了。

clc
clear

vertex_true = [0, 0; 1.3, 0; 2.2, 0; 3, 0; 3, 0.8; 3, 1.4; 3, 1.5; 2.8, 1.5; 1.7, 1.5; 0.8, 1.5; 0, 1.5; 0, 0.9; 0, 0.15];

vertex = [0, 0; 1.2, 0; 2.3, 0; 3.2, 0; 3.2, 0.6; 3.2, 1.3; 3.2, 1.6; 3.1, 1.6; 1.8, 1.6; 1.1, 1.6; 0.1, 1.6; 0.1, 1.2; 0.1, 0.3];

edges = [2, 1, 1.3, 0;
         3, 2, 0.9, 0;
         4, 3, 0.8, 0;
         5, 4, 0, 0.8;
         6, 5, 20, 0.6;
         7, 6, 0, 0.1;
         8, 7, -0.2, 0;
         9, 8, -1.1, 0;
         10, 9, -0.9, 0;
         11, 10, -0.8, 0;
         12, 11, 0, -0.6;
         13, 12, 0, -0.75;
         1, 13, 0, 0];
     
global x y idx1 idx2 m_x m_y dim_mea

x = 1; y = 2;
idx1 = 1; idx2 = 2; m_x = 3; m_y = 4;
% 误差和测量的维度是一样的，都是2
% 边连接的两个节点的维度也是一样的，都是2；但在VSLAM中，位姿节点一般为6维，路标点一般为3维
dim_vertex = 2; dim_mea = 2;


r = 0;
for i = 1 : length(vertex)
    e_i = computeError(edges, vertex, i);
    r = r + e_i' * e_i;
end

iter_num = 100;

residual = zeros(iter_num + 1, 1);
residual(1) = r;
vertices = zeros((iter_num + 1) * dim_mea,  length(vertex));
vertices(1:2, :) = vertex';

sigma_1 = 1e-8;
sigma_2 = 1e-6;
u = 0.02;
v = 2;
iter = 0;
ii = 1;
found = false;
while false == found && iter < iter_num
    iter = iter + 1;
    H = zeros(length(vertex) * dim_vertex, length(vertex) * dim_vertex);
    b = zeros(length(vertex) * dim_vertex, 1);
    for i = 1 : length(vertex)
        % residual
        e_i = computeError(edges, vertex, i);
        % jacobian
        [jacobianOplusXi, jacobianOplusXj] = linearizeOplus();
        % 由于两个节点的维度是一样的，所以这里计算各自的雅可比矩阵的存放位置的时候就比较简单
        % 对于VSLAM而言，为了方便后续做舒尔补，一般要先统计位姿的数量，将路标点的雅可比矩阵放在所有位姿的雅可比矩阵后面
        % 然后位姿和路标点分别维持一个下标，安装各自顺序存放
        J_i = zeros(dim_mea, length(vertex) * dim_vertex);
        %从(1 (edges(1, idx1) - 1) * dim_vertex + 1)开始的2-by-2矩阵块
        J_i(1:dim_mea, (edges(i, idx1) - 1) * dim_vertex + 1 : edges(i, idx1) * dim_vertex) = jacobianOplusXi;
        J_i(1:dim_mea, (edges(i, idx2) - 1) * dim_vertex + 1 : edges(i, idx2) * dim_vertex) = jacobianOplusXj;
        % hessian
        H = H + J_i' * J_i;
        % b
        b = b - J_i' * e_i;
    end
    H(1, 1) = H(1, 1) + 1e6;
    H(2, 2) = H(2, 2) + 1e6;
    H = H + u * eye(size(H));
    
    delta = H \ b;
    
    if norm(delta) < sigma_2 * (norm(vertex) + sigma_2)
        found = true;
    else
        vertex_new = oplusImpl(vertex, delta);
        
        r = 0;
        for i = 1 : length(vertex)
            e_i = computeError(edges, vertex_new, i);
            r = r + e_i' * e_i;
        end
        dL = delta' * (u * delta + b);
        dF = (residual(ii) - r)' * (residual(ii) + r);
        if dL > 0 && dF > 0
            vertex = vertex_new;
            v = 2;
            u = u * max(1/3, 1 - (2 * dF / dL - 1)^3);
            
            ii = ii + 1;
            vertices((ii - 1) * 2 + 1 : ii * 2, :) = vertex';
            residual(ii) = r;
            
            if abs(max(b)) <= sigma_1
                found = true;
            end
        else
            u = u * v;
            v = 2 * v;
        end
    end
end

vertex_true'
vertex'

function e = computeError(edges, vertex,  i)
global x y idx1 idx2 m_x m_y dim_mea

e = zeros(dim_mea, 1); % 2-by-1 vector
e(x) = edges(i, m_x) - (vertex(edges(i, idx1), x) - vertex(edges(i, idx2), x));
e(y) = edges(i, m_y) - (vertex(edges(i, idx1), y) - vertex(edges(i, idx2), y));
end

function [jacobianOplusXi, jacobianOplusXj] = linearizeOplus()
jacobianOplusXi = [-1, 0; 0, -1]; % 2-by-2 matrix
jacobianOplusXj = [1, 0; 0, 1];
end

function vertex_new = oplusImpl(vertex, delta)
% 存储节点是，格式是[x, y; x, y; ...]
% 但更新量是[x, y, x, y, ...]
vertex_new = vertex;
vertex_new(:, 1) = vertex_new(:, 1) + delta(1 : 2 : end);
vertex_new(:, 2) = vertex_new(:, 2) + delta(2 : 2 : end);
end

整个代码不复杂（其实我们的整体流程和g2o里。但有几点需要说明：

对于g2o这样比较成熟的开源库而言，图优化的框架和具体的优化算法是分开的。一个是HyperGraph，一个是OptimizationAlgorithm。前者实现节点、边等等图优化的元素和框架；后者对应具体的优化算法，如LM算法、狗腿法等。（其实更重要的是线性系统Hx=b的求解器Solver。这里就不展开了，后面再说吧。）但我们的例子很简单，所以就合在一起了。
一些代码抽出来做了函数。当你使用g2o时，一般也是重载这3个函数：computeError(), linearizeOplus()和oplusImpl().
计算完雅可比，对整个问题进行求解时，最重要的是某个节点的雅可比矩阵在所有节点的大雅可比矩阵中的位置。我们的问题比较小，还可以通过打印等方式直接地看，但对于真正的SLAM问题，面对成百上千的相机位姿和路标点，一定要放对。这也是为什么我前面说节点的编号很重要的原因。

如果你看得比较仔细，或者已经运行过上面的代码，就会发现一个问题：是的，其中有一个测量值是错误的，但我们没做任何处理，所以优化出来的结果是这样的：

为了得到尽量正确的结果，你可以把测量值改回去，就有

但是实际情况里，你也不知道哪个测量是错的。如果你对BA比较熟悉，就知道这里鲁棒核函数就应该闪亮登场了。这里，我们实现Huber核函数：

$L_\delta(a) = \begin{cases} \frac{1}{2} a^2 & |a| \le \delta \\ \delta ( |a| - \frac{1}{2} \delta & \text{otherwise} \end{cases}$

使用核函数，我们的等式就变了，因为在对残差求导的时候，也要对核函数进行求导。对应地，雅可比矩阵的计算变成了：

$\mathbf{b}_i = \rho'_i \mathbf{J}_i^T \Omega_i \mathbf{r}_i$

而海塞矩阵为：

$\mathbf{H}_i = \mathbf{J}^T_i (\rho'_i \Omega_i + 2 \rho''_i (\Omega_i \mathbf{r}_i)(\Omega_i \mathbf{r}_i)^T) \mathbf{J}_i$

这里， $\Omega$ 是测量值的协方差矩阵。

公式推导完了，现在就可以来进行代码实现了：

clc
clear

vertex_true = [0, 0; 1.3, 0; 2.2, 0; 3, 0; 3, 0.8; 3, 1.4; 3, 1.5; 2.8, 1.5; 1.7, 1.5; 0.8, 1.5; 0, 1.5; 0, 0.9; 0, 0.15];

vertex = [0, 0; 1.2, 0; 2.3, 0; 3.2, 0; 3.2, 0.6; 3.2, 1.3; 3.2, 1.6; 3.1, 1.6; 1.8, 1.6; 1.1, 1.6; 0.1, 1.6; 0.1, 1.2; 0.1, 0.3];

edges = [2, 1, 1.3, 0;
         3, 2, 0.9, 0;
         4, 3, 0.8, 0;
         5, 4, 0, 0.8;
         6, 5, 20, 0.6;
         7, 6, 0, 0.1;
         8, 7, -0.2, 0;
         9, 8, -1.1, 0;
         10, 9, -0.9, 0;
         11, 10, -0.8, 0;
         12, 11, 0, -0.6;
         13, 12, 0, -0.75;
         1, 13, 0, 0];
     
global x y idx1 idx2 m_x m_y dim_mea

x = 1; y = 2;
idx1 = 1; idx2 = 2; m_x = 3; m_y = 4;
% 误差和测量的维度是一样的，都是2
% 边连接的两个节点的维度也是一样的，都是2；但在VSLAM中，位姿节点一般为6维，路标点一般为3维
dim_vertex = 2; dim_mea = 2;

robustify = true; thres = 0.3;

r = 0;
for i = 1 : length(vertex)
    e_i = computeError(edges, vertex, i);
    if false == robustify
        r = r + e_i' * e_i;
    else
        rho_i = HuberKernel(e_i' * e_i, thres);
        r = r + rho_i(1);
    end
end

iter_num = 100;

residual = zeros(iter_num + 1, 1);
residual(1) = r;
vertices = zeros((iter_num + 1) * dim_mea,  length(vertex));
vertices(1:2, :) = vertex';

sigma_1 = 1e-8;
sigma_2 = 1e-6;
u = 0.02;
v = 2;
iter = 0;
ii = 1;
found = false;
while false == found && iter < iter_num
    iter = iter + 1;
    H = zeros(length(vertex) * dim_vertex, length(vertex) * dim_vertex);
    b = zeros(length(vertex) * dim_vertex, 1);
    for i = 1 : length(vertex)
        % residual
        e_i = computeError(edges, vertex, i);
        % jacobian
        [jacobianOplusXi, jacobianOplusXj] = linearizeOplus();
        % 由于两个节点的维度是一样的，所以这里计算各自的雅可比矩阵的存放位置的时候就比较简单
        % 对于VSLAM而言，为了方便后续做舒尔补，一般要先统计位姿的数量，将路标点的雅可比矩阵放在所有位姿的雅可比矩阵后面
        % 然后位姿和路标点分别维持一个下标，安装各自顺序存放
        J_i = zeros(dim_mea, length(vertex) * dim_vertex);
        %从(1 (edges(1, idx1) - 1) * dim_vertex + 1)开始的2-by-2矩阵块
        J_i(1:dim_mea, (edges(i, idx1) - 1) * dim_vertex + 1 : edges(i, idx1) * dim_vertex) = jacobianOplusXi;
        J_i(1:dim_mea, (edges(i, idx2) - 1) * dim_vertex + 1 : edges(i, idx2) * dim_vertex) = jacobianOplusXj;
        if false == robustify
            % hessian
            H = H + J_i' * J_i;
            % b
            b = b - J_i' * e_i;
        else
            rho_i = HuberKernel(e_i' * e_i, thres);
            Omega_i = robustInformation(rho_i, e_i, eye(dim_mea, dim_mea));
            
            H = H + J_i' * Omega_i * J_i;
            b = b - rho_i(2) * J_i' * e_i;
        end
    end
    H(1, 1) = H(1, 1) + 1e6;
    H(2, 2) = H(2, 2) + 1e6;
    H = H + u * eye(size(H));
    
    delta = H \ b;
    
    if norm(delta) < sigma_2 * (norm(vertex) + sigma_2)
        found = true;
    else
        vertex_new = oplusImpl(vertex, delta);
        
        r = 0;
        for i = 1 : length(vertex)
            e_i = computeError(edges, vertex_new, i);
            if false == robustify
                r = r + e_i' * e_i;
            else
                rho_i = HuberKernel(e_i' * e_i, thres);
                r = r + rho_i(1);
            end
        end
        dL = delta' * (u * delta + b);
        dF = (residual(ii) - r)' * (residual(ii) + r);
        if dL > 0 && dF > 0
            vertex = vertex_new;
            v = 2;
            u = u * max(1/3, 1 - (2 * dF / dL - 1)^3);
            
            ii = ii + 1;
            vertices((ii - 1) * 2 + 1 : ii * 2, :) = vertex';
            residual(ii) = r;
            
            if abs(max(b)) <= sigma_1
                found = true;
            end
        else
            u = u * v;
            v = 2 * v;
        end
    end
end

vertex_true'
vertex'

function e = computeError(edges, vertex,  i)
global x y idx1 idx2 m_x m_y dim_mea

e = zeros(dim_mea, 1); % 2-by-1 vector
e(x) = edges(i, m_x) - (vertex(edges(i, idx1), x) - vertex(edges(i, idx2), x));
e(y) = edges(i, m_y) - (vertex(edges(i, idx1), y) - vertex(edges(i, idx2), y));
end

function [jacobianOplusXi, jacobianOplusXj] = linearizeOplus()
jacobianOplusXi = [-1, 0; 0, -1]; % 2-by-2 matrix
jacobianOplusXj = [1, 0; 0, 1];
end

function vertex_new = oplusImpl(vertex, delta)
% 存储节点是，格式是[x, y; x, y; ...]
% 但更新量是[x, y, x, y, ...]
vertex_new = vertex;
vertex_new(:, 1) = vertex_new(:, 1) + delta(1 : 2 : end);
vertex_new(:, 2) = vertex_new(:, 2) + delta(2 : 2 : end);
end

function rho = HuberKernel(e_sqr, thres)
thres_sqr = thres * thres;
rho = zeros(3, 1);
if e_sqr <= thres_sqr
    rho(1) = e_sqr;
    rho(2) = 1;
    rho(3) = 0;
else
    e_sqrt = sqrt(e_sqr);
    rho(1) = 2 * e_sqrt * thres - thres_sqr;
    rho(2) = thres / e_sqrt;
    rho(3) = -0.5 * rho(2) / e_sqr;
end
end

function Omega = robustInformation(rho, e, information)
Omega = rho(2) * information;
weighted_error = information * e;
Omega = Omega + 2 * rho(3) * (weighted_error * weighted_error');
end

如此，我们就完成了一个完整的LM算法。虽然它相当轻量，但是对于理解最小二乘算法还是很有帮助的。希望你看完了也对LM算法有了更深的理解。

令：其实你可以发现，加了鲁棒核函数虽然极大减轻了outliers的影响，但整个小车的轨迹还是不对。这也是BA的一个缺点：对于outliers敏感。所以在很多问题里，常常先进行RANSAC来去除外点，然后再来进行BA。或者在ORB-SLAM里，图优化每迭代一次，作者就会调用g2o中的每条边的computeError和chi函数来获得这条边的误差，及时把误差大的outliers排除出去。

如果觉得有用，还请点个赞: )

非线性优化（二）：徒手实现LM算法

David LEE

为你推荐

捷联惯导基础知识解析之五（低成本姿态航向参考系统）

应用于三维重建的TSDF(三)voxblox代码解析

单足机器人技术分析——平面单足跳跃机器人

【Coppeliasim｜轨迹规划（一）】设置机械臂的关节角时保证机械臂不散架

精选SLAM第六讲实践： gaussNewton ，ceres库，g2o库, g2o_viewer可视化优化超详细安装与使用

ROS联合Webots扩展(二)通过语音控制机器人

关于作者

David LEE

18

0

1

1

非线性优化（三）：g2o源代码

第四讲：李群和李代数

结构光综述

相关推荐

鲁棒控制理论（三）H∞控制的特点

Abaqus模拟空气弹簧

【联合仿真】Adams六关节机械臂与Matlab/Simulink的联合仿真(上)

从零开始的分析力学基础 3.刚体动力学(2)

基于车辆运动学横纵向协同的模型预测控制MPC

matlab非线性等式约束优化求解机械臂最优坐标方向

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

非线性优化（二）：徒手实现LM算法

David LEE

为你推荐

捷联惯导基础知识解析之五（低成本姿态航向参考系统）

应用于三维重建的TSDF(三)voxblox代码解析

单足机器人技术分析——平面单足跳跃机器人

【Coppeliasim｜轨迹规划（一）】设置机械臂的关节角时保证机械臂不散架

精选SLAM第六讲实践： gaussNewton ，ceres库，g2o库, g2o_viewer可视化优化超详细安装与使用

ROS联合Webots扩展(二)通过语音控制机器人

评论（0）

关于作者

David LEE

18

0

1

1

非线性优化（三）：g2o源代码

第四讲：李群和李代数

结构光综述

相关推荐

鲁棒控制理论（三）H∞控制的特点

Abaqus模拟空气弹簧

【联合仿真】Adams六关节机械臂与Matlab/Simulink的联合仿真(上)

从零开始的分析力学基础 3.刚体动力学(2)

基于车辆运动学横纵向协同的模型预测控制MPC

matlab非线性等式约束优化求解机械臂最优坐标方向

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板