vSLAMNet（四）-最优化-Levenberg-Marquardt算法

1. 概述

• LM算法，全称为Levenberg-Marquard算法，它可用于解决非线性最小二乘问题，多用于曲线拟合等场合
• 主要思想
  • LM算法属于一种“信赖域法”——所谓的信赖域法，此处稍微解释一下：在最优化算法中，都是要求一个函数的极小值，每一步迭代中，都要求目标函数值是下降的，而信赖域法，顾名思义，就是从初始点开始，先假设一个可以信赖的最大位移s，然后在以当前点为中心，以s为半径的区域内，通过寻找目标函数的一个近似函数（二次的）的最优点，来求解得到真正的位移。在得到了位移之后，再计算目标函数值，如果其使目标函数值的下降满足了一定条件，那么就说明这个位移是可靠的，则继续按此规则迭代计算下去；如果其不能使目标函数值的下降满足一定的条件，则应减小信赖域的范围，再重新求解。

2. 算法原理

对于一个非线性最小二乘问题：

$x = \arg \min_x \frac{1}{2}||f(x)||^2$.

高斯牛顿的思想是把$f(x)$利用泰勒展开，取一阶线性项近似，即

$f(x + \Delta x) = f(x) + f’(x)\Delta x = f(x) + J(x)\Delta x$.

再将上述公式代入非线性最小二乘公式，得：

$\frac{1}{2}||f(x + \Delta x)||^2 = \frac{1}{2}[f(x)^Tf(x) + 2f(x)^TJ(x)\Delta x + \Delta x^TJ(x)^TJ(x)\Delta x]$.

阻尼法的的思想是再加入一个阻尼项$\frac{1}{2}\mu\Delta x^T\Delta x$，得到：

为了书写方便，将$\Delta x$记为h，

$h = \arg\min_hG(h) = \arg\min_h\frac{1}{2}f^Tf + h^TJ^Tf + \frac{1}{2}h^TJ^TJh + \frac{1}{2}\mu h^Th$.

对上式求偏导数，并令为0，得到：

$J^Tf + J^TJh + \mu h = 0$,

$(J^TJ + \mu I)h = -J^Tf$,

$(H + \mu I)h = -g$.

引入阻尼参数$\mu$由如下好处：

• 对于$\mu > 0$，$(H + \mu I)$正定，保证了h是梯度下降的方向
• 当$\mu$较大时，$h \approx -\frac{1}{\mu}g = -\frac{1}{\mu}F’(x)$，其实就是梯度、最速下降法，当离最终结果较远的时候，很好
• 当$\mu$较小时，方法接近与高斯牛顿，当离最终结果很近时，可以获得二次收敛速度，很好

$\mu$的设定策略如下：

• 初始时，取

$A_0 = J(x_0)^TJ(x_0)$,

$\mu_0 = \tau \cdot\max{a_{ii}^0}$.

• 迭代后，利用cost增益来确定

$\mu = \frac{F(x) - f(x + h)}{L(0) - L(h)}$.

迭代终止条件：

• 一阶导数为０：$F’(x) = g(x) = 0$，使用$||g(x)|| < \epsilon$，其中$\epsilon$是设定的终止条件
• x变化步距离足够小，$||h|| = ||x_{new} - x|| < \epsilon(||x|| + \epsilon)$
• 超过最大迭代次数

3. 例子

求解非线性方程$y = e^{ax^2 + bx + c}$，给定n组观测数据$(x, y)$，求解系数$X = [a, b, c]^T$。

解：由题可得：

令$f(X) = y - e^{ax^2 + bx + c}$，N组数据可以组成一个大的非线性方程组，通过构建最小二乘问题来进行系数求解，即

$x = \arg\min_x \frac{1}{2}||f(X)||^2$.

要求解这个问题，根据推导部分可知，需要求解雅克比，

实现代码如下：

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Cholesky>
#include <chrono>

/* 计时类 */
class Runtimer{
public:
    inline void start()
    {
        t_s_  = std::chrono::steady_clock::now();
    }
    
    inline void stop()
    {
        t_e_ = std::chrono::steady_clock::now();
    }
    
    inline double duration()
    {
        return std::chrono::duration_cast<std::chrono::duration<double>>(t_e_ - t_s_).count() * 1000.0;
    }
    
private:
    std::chrono::steady_clock::time_point t_s_; //start time ponit
    std::chrono::steady_clock::time_point t_e_; //stop time point
};

/*  优化方程 */
class LevenbergMarquardt{
public:
    LevenbergMarquardt(double* a, double* b, double* c):
    a_(a), b_(b), c_(c)
    {
        epsilon_1_ = 1e-6;
        epsilon_2_ = 1e-6;
        max_iter_ = 50;
        is_out_ = true;
    }
    
    void setParameters(double epsilon_1, double epsilon_2, int max_iter, bool is_out)
    {
        epsilon_1_ = epsilon_1;
        epsilon_2_ = epsilon_2;
        max_iter_ = max_iter;
        is_out_ = is_out;
    }
    
    void addObservation(const double& x, const double& y)
    {
        obs_.push_back(Eigen::Vector2d(x, y));
    }
    
    void calcJ_fx()
    {
        J_ .resize(obs_.size(), 3);
        fx_.resize(obs_.size(), 1);
        
        for ( size_t i = 0; i < obs_.size(); i ++)
        {
            const Eigen::Vector2d& ob = obs_.at(i);
            const double& x = ob(0);
            const double& y = ob(1);
            double j1 = -x*x*exp(*a_ * x*x + *b_*x + *c_);
            double j2 = -x*exp(*a_ * x*x + *b_*x + *c_);
            double j3 = -exp(*a_ * x*x + *b_*x + *c_);
            J_(i, 0 ) = j1;
            J_(i, 1) = j2;
            J_(i, 2) = j3;
            fx_(i, 0) = y - exp( *a_ *x*x + *b_*x +*c_);
        }
    }
    
    void calcH_g()
    {
        H_ = J_.transpose() * J_;
        g_ = -J_.transpose() * fx_;
    }
        
    double getCost()
    {
        Eigen::MatrixXd cost= fx_.transpose() * fx_;
        return cost(0,0);
    }
    
    double F(double a, double b, double c)
    {
        Eigen::MatrixXd fx;
        fx.resize(obs_.size(), 1);
        
        for ( size_t i = 0; i < obs_.size(); i ++)
        {
            const Eigen::Vector2d& ob = obs_.at(i);
            const double& x = ob(0);
            const double& y = ob(1);
            fx(i, 0) = y - exp( a *x*x + b*x +c);
        }
        Eigen::MatrixXd F = 0.5 * fx.transpose() * fx;
        return F(0,0);
    }
    
    double L0_L( Eigen::Vector3d& h)
    {
           Eigen::MatrixXd L = -h.transpose() * J_.transpose() * fx_ - 0.5 * h.transpose() * J_.transpose() * J_ * h;
           return L(0,0);
    }

    void solve()
    {
        int k = 0;
        double nu = 2.0;
        calcJ_fx();
        calcH_g();
        bool found = ( g_.lpNorm<Eigen::Infinity>() < epsilon_1_ );
        
        std::vector<double> A;
        A.push_back( H_(0, 0) );
        A.push_back( H_(1, 1) );
        A.push_back( H_(2,2) );
        auto max_p = std::max_element(A.begin(), A.end());
        double mu = *max_p;
        
        double sumt =0;

        while ( !found && k < max_iter_)
        {
            Runtimer t;
            t.start();
            
            k = k +1;
            Eigen::Matrix3d G = H_ + mu * Eigen::Matrix3d::Identity();
            Eigen::Vector3d h = G.ldlt().solve(g_);
            
            if( h.norm() <= epsilon_2_ * ( sqrt(*a_**a_ + *b_**b_ + *c_**c_ ) +epsilon_2_ ) )
                found = true;
            else
            {
                double na = *a_ + h(0);
                double nb = *b_ + h(1);
                double nc = *c_ + h(2);
                
                double rho =( F(*a_, *b_, *c_) - F(na, nb, nc) )  / L0_L(h);

                if( rho > 0)
                {
                    *a_ = na;
                    *b_ = nb;
                    *c_ = nc;
                    calcJ_fx();
                    calcH_g();
                                      
                    found = ( g_.lpNorm<Eigen::Infinity>() < epsilon_1_ );
                    mu = mu * std::max<double>(0.33, 1 - std::pow(2*rho -1, 3));
                    nu = 2.0;
                }
                else
                {
                    mu = mu * nu; 
                    nu = 2*nu;
                }// if rho > 0
            }// if step is too small
            
            t.stop();
            if( is_out_ )
            {
                std::cout << "Iter: " << std::left <<std::setw(3) << k << " Result: "<< std::left <<std::setw(10)  << *a_ << " " << std::left <<std::setw(10)  << *b_ << " " << std::left <<std::setw(10) << *c_ << 
                " step: " << std::left <<std::setw(14) << h.norm() << " cost: "<< std::left <<std::setw(14)  << getCost() << " time: " << std::left <<std::setw(14) << t.duration()  <<
                " total_time: "<< std::left <<std::setw(14) << (sumt += t.duration()) << std::endl;
            }   
        } // while
        
        if( found  == true)
            std::cout << "\nConverged\n\n";
        else
            std::cout << "\nDiverged\n\n";
        
    }//function 
    
    
    
    Eigen::MatrixXd fx_; 
    Eigen::MatrixXd J_; // 雅克比矩阵
    Eigen::Matrix3d H_; // H矩阵
    Eigen::Vector3d g_;
    
    std::vector< Eigen::Vector2d> obs_; // 观测
   
   /* 要求的三个参数 */
   double* a_, *b_, *c_;
    
    /* parameters */
    double epsilon_1_, epsilon_2_;
    int max_iter_;
    bool is_out_;
};//class LevenbergMarquardt
int main(int argc, char **argv) {
    const double aa = 0.1, bb = 0.5, cc = 2; // 实际方程的参数
    double a =0.0, b=0.0, c=0.0; // 初值
    
    /* 构造问题 */
    LevenbergMarquardt lm(&a, &b, &c);
    lm.setParameters(1e-10, 1e-10, 100, true);
    
    /* 制造数据 */
    const size_t N = 100; //数据个数
    cv::RNG rng(cv::getTickCount());
    for( size_t i = 0; i < N; i ++)
    {
        /* 生产带有高斯噪声的数据　*/
        double x = rng.uniform(0.0, 1.0) ;
        double y = exp(aa*x*x + bb*x + cc) + rng.gaussian(0.05);
        
        /* 添加到观测中　*/
        lm.addObservation(x, y);
    }
    /* 用LM法求解 */
    lm.solve();
    
    return 0;
}

（以上代码参考：https://zhuanlan.zhihu.com/p/42415718）

4. 参考

• Levenberg-Marquardt 最小二乘优化