转载请说明出处：http://blog.csdn.net/zhubaohua_bupt/article/details/74973347


在VSLAM优化部分，我们多次谈到，构建一个关于待优化位姿的误差函数
（直接法：灰度误差  ；特征点法：重投影误差），
当待优化的位姿使这个误差函数最小时(当SLAM运动不是太剧烈时，误差函数满足单峰性)，认为此时位姿最精确。


如果这个误差函数是线性，而且已知解析式的，很容易通过求导，令导数=0,求极值解决问题。
然而，误差函数是关于待优化位姿的一个非线性多元函数，怎么求使这个误差函数最小的位姿呢？


实际上，这是一个非线性无约束最优化问题，在目前主流的VSLAM(比如ORB，SVO，LSD)里，
采用的优化算法主要有两种：
一种是高斯牛顿(Gauss Newton)算法，另一种是列文伯格-马夸尔特(Levenberg-Marquardt)算法，简称LM算法。


下面我们详细探讨一下，高斯牛顿和LM算法的原理以及在VSLAM中的应用。
首先，最小二乘是要解决什么问题？
       1 最小二乘算法
                
     1.1 线性最小二乘问题


             
1.2 非线性最小二乘问题




               






迭代过程如下图所示：


1.2.1 高斯牛顿法




                        




               
1.2.2 LM算法




               




         


       



                    









        


2        高斯牛顿和LM算法在VSLAM中的应用









                                   http://blog.csdn.net/zhubaohua_bupt/article/details/74011005
















     http://blog.csdn.net/zhubaohua_bupt/article/details/74011005

分享一篇写的非常好的博客LM算法，简单明了地介绍了梯度下降法、牛顿法、高斯牛顿法和Levenberg-Marquardt这几种优化算法之间的联系和区别。

关于LM写的不错的文章：
https://blog.csdn.net/a6333230/article/details/83304098
三维重构PMP论文中使用的信赖域算法（LM）matlab版,修改在标定中可用
function [G, val] = rect_rot(G, A, d)
% Rectify rotations
%
% G: Rectifying transform
% A, d: Coefficients for orthonormality constraint
% val: Value of residual error

ik = size(G, 1);
ikk = numel(G);
k = ikk/ik;
Id = eye(ik);
IdG = eye(ikk);

% Parameters for trust-region optimization
delta = 1;
eta1 = 1/4;
eta2 = 3/4;
kappa = 2;
epsilon = 1e-3;

% Trust-region strategy
f = A*reshape(G*G', [], 1)-d;
c = norm(f)^2;
pc = inf;
while pc-c > 1e-10
    pG = G;
    pf = f;
    pc = c;

    J = A*kron(G, Id)+reshape(permute(reshape(A*kron(Id, G), [], k, ik), [1 3 2]), [], ikk);
    H = J'*J;
    g = J'*f;

    while true
        temp = H+IdG*delta;
        r = (temp+trace(temp)*1e-10*IdG)\g;
        G(:) = G(:)-r;

        f = A*reshape(G*G', [], 1)-d;
        c = norm(f)^2;

        rho = (pc-c)/(pc-norm(pf-J*r)^2);
        rho(isnan(rho)) = 0;
        if rho < eta1 || pc < c
            delta = delta*kappa;
        elseif rho > eta2
            delta = delta/kappa;
        end

        if rho < epsilon || pc < c
            G = pG;
            if delta > 1e10
                c = pc;
                break;
            end
        else
            break;
        end
    end
end

val = mse(f);

end

什么是最优化？
Levenberg-Marquardt算法是最优化算法中的一种。最优化是寻找使得函数值最小的参数向量。它的应用领域非常广泛，如：经济学、管理优化、网络分析 、最优设计、机械或电子设计等等。






根据求导数的方法，可分为2大类。第一类，若f具有解析函数形式，知道x后求导数速度快。第二类，使用数值差分来求导数。根据使用模型不同，分为非约束最优化、约束最优化、最小二乘最优化。




什么是Levenberg-Marquardt算法？
它是使用最广泛的非线性最小二乘算法，中文为列文伯格-马夸尔特法。它是利用梯度求最大（小）值的算法，形象的说，属于“爬山”法的一种。它同时具有梯度法和牛顿法的优点。当λ很小时，步长等于牛顿法步长，当λ很大时，步长约等于梯度下降法的步长。图1显示了算法从起点，根据函数梯度信息，不断爬升直到最高点（最大值）的迭代过程。共进行了12步。（备注：图1中绿色线条为迭代过程）。










图1中，算法从山脚开始不断迭代。可以看到，它的寻优速度是比较快的，在山腰部分直接利用梯度大幅度提升（参见后文例子程序中lamda较小时），快到山顶时经过几次尝试（lamda较大时），最后达到顶峰（最大值点），算法终止。





下面给出算法流程：







LM算法的实现并不算难，它的关键是用模型函数 f 对待估参数向量p在其领域内做线性近似，忽略掉二阶以上的导数项，从而转化为线性最小二乘问题，它具有收敛速度快等优点。LM算法属于一种“信赖域法”，所谓的信赖域法，即是：在最优化算法中，都是要求一个函数的极小值，每一步迭代中，都要求目标函数值是下降的，而信赖域法，顾名思义，就是从初始点开始，先假设一个可以信赖的最大位移s，然后在以当前点为中心，以s为半径的区域内，通过寻找目标函数的一个近似函数（二次的）的最优点，来求解得到真正的位移。在得到了位移之后，再计算目标函数值，如果其使目标函数值的下降满足了一定条件，那么就说明这个位移是可靠的，则继续按此规则迭代计算下去；如果其不能使目标函数值的下降满足一定的条件，则应减小信赖域的范围，再重新求解。




LM算法需要对每一个待估参数求偏导，所以，如果你的拟合函数 f 非常复杂，或者待估参数相当地多，那么可能不适合使用LM算法，而可以选择Powell算法（Powell算法不需要求导）。




伪代码：










相关源码可以在网址http://www.ics.forth.gr/%CB%9Clourakis/levmar上获得。




为了方便理解，这里给出相应的算法原理和实现：

链接：http://pan.baidu.com/s/1pJp0BMj 密码：1u7g


有什么问题可以咨询我。