手写系列之手写LM(Levenberg–Marquardt)算法(基于eigen)

紧接上次的手写高斯牛顿,今天顺便将LM算法也进行了手写实现,并且自己基于eigen的高斯牛顿进行了对比,可以很明显的看到,LM的算法收敛更快,精度也略胜一筹,这次高博的书不够用了,参考网上伪代码进行实现.同学们有什么问题都可以讨论,理论部分的细节后面会补上,最近确实有点忙.

伪代码

程序源码

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include"opencv2/opencv.hpp"
#include"opencv4/opencv2/opencv.hpp"
#include<iostream>
#include<vector>
using namespace Eigen;
using namespace std;
using namespace cv;
//生成测试数据
void makeTheTestNum(vector<double> &xSet ,vector<double> &ySet);
//LM算法结算
void LM(const vector<double> &xSet ,const vector<double> &ySet ,double &a,double &b,double &c);

void makeTheTestNum(vector<double> &xSet ,vector<double> &ySet){

    RNG rng;   
    double noise = rng.gaussian(1);
    //设定值
    double a = 2;
    double b = 1;
    double c = 1;
    for(int i = 0;i <100;i++)
    {   
        double x = i/100.0;//注意这个.0,不然出来全是0
        double fx = exp(a*x*x + b*x + c)+noise;
        xSet.push_back(x);
        ySet.push_back(fx);
    }
    cout<<xSet.size()<<endl;
    if(xSet.size() != ySet.size())
        cout<<"data is bad!"<<endl;

}

void LM(const vector<double> &xSet ,const vector<double> &ySet ,double &a,double &b,double &c){
    bool flag = 0;
    double cost =0.0;
    double lastcost = 0.0;
    int maxtimes = 1000;
    double v = 2;
    double rho = 0;
    double tao = 1e-10;
    //获得初值
    Matrix3d H = Matrix3d::Zero();
    Vector3d g = Vector3d::Zero();
    Vector3d J;
    //装填数据
    for(int j = 0;j< xSet.size();j++){  
        double x = xSet[j];
        double y = ySet[j];
        // cout<<"x" <<x<<endl;
        // cout<<"y" <<y<<endl;
        double e = y - exp(a*x*x + b*x + c);
        J[0] =  -exp(a*x*x + b*x + c)*x*x;
        J[1] = -exp(a*x*x + b*x + c)*x;
        J[2] = -exp(a*x*x + b*x + c)*1;

        Matrix3d tempH = J*J.transpose(); 
        H +=tempH;
        g += -J*e;
        cost +=e*e;
    }
    //设置这个u的初值
    double u = tao*H.maxCoeff();
    cout<<"init u :"<<u<<endl;
    cout<<"H init"<<H.matrix()<<endl;
    cout<<"J init"<<J.matrix()<<endl;
    cout<<"g init"<<g.matrix()<<endl;
    Matrix3d I = MatrixXd::Identity(3, 3);
    for(int i =0; i< maxtimes;i++){
        
        //使用eigen解算线性方程组,此处和GN的方程多了一个u*I,这就是LM的关键
        Matrix3d A = H+u*I;
        Vector3d delta_abc = A.ldlt().solve(g);
        //cout<<"delta_abc"<<delta_abc<<endl;
        //Vector3d delta_abc = H.ldlt().solve(g);
        if(delta_abc.norm()<1e-12){   
            flag =1;
            break;
                }
        cout<<"delta_abc"<<delta_abc.transpose()<<endl;
        //判断是否发散
        if(isnan(delta_abc[0])||isnan(delta_abc[1])||isnan(delta_abc[2])){   
            flag =0;
            break;
                }
        a += delta_abc[0];
        b += delta_abc[1];
        c += delta_abc[2];
        double cost_new = 0;
        for(int j = 0;j< xSet.size();j++){  
            double x = xSet[j];
            double y = ySet[j];
            double e = y - exp(a*x*x + b*x + c);
            cost_new +=e*e;
        }
        rho = (cost - cost_new)/(delta_abc.transpose()*(u*delta_abc+g));

        //LM的工作
        if(rho >0 ){
        	cost= 0;
            //注意初始化两个H和g,如果不是0会有很多奇怪的错误
            H = Matrix3d::Zero();
            g = Vector3d::Zero();
            J = Vector3d::Zero();
            //装填数据
            for(int j = 0;j< xSet.size();j++){  
                double x = xSet[j];
                double y = ySet[j];
                // cout<<"x" <<x<<endl;
                // cout<<"y" <<y<<endl;
                double e = y - exp(a*x*x + b*x + c);
                J[0] =  -exp(a*x*x + b*x + c)*x*x;
                J[1] = -exp(a*x*x + b*x + c)*x;
                J[2] = -exp(a*x*x + b*x + c)*1;

                Matrix3d tempH = J*J.transpose(); 
                H +=tempH;
                g += -J*e;
                cost +=e*e;
            }
            if(delta_abc.norm()<1e-12||cost<1e-12){   
                flag =1;
                break;
                }
            //更新u和v,缩小范围,更接近高斯牛顿
            u = u*max(0.3333333,(1-(2*rho-1)*(2*rho-1)*(2*rho-1)));
            v=2;
            }
            else{
            //不满足,扩大范围,更接近最速下降
                u = u * v;
                v = 2 * v;
            }
            cout<<"第"<<i<<"次:"<<endl;
            cout<<"a : "<<a<<endl;
            cout<<"b : "<<b<<endl;
            cout<<"c : "<<c<<endl;
            lastcost = cost;
            cout<<"delta_abc"<<delta_abc.norm()<<endl;
        }

        if(flag)
        {
                cout<<"已收敛,结果为:"<<endl;
                cout<<"final a : "<<a<<endl;
                cout<<"final b : "<<b<<endl;
                cout<<"final c : "<<c<<endl;
        }
        else
        {
            cout<<"发散了QAQ,最后一次结果"<<endl;
                cout<<"final a : "<<a<<endl;
                cout<<"final b : "<<b<<endl;
                cout<<"final c : "<<c<<endl;       
        }



}

int main(){
    cout.precision(9);
    //设置初始值
    double a = -3;
    double b = -1;
    double c = 7;
    //观测值
    vector<double> xSet ;
    vector<double> ySet;
    makeTheTestNum(xSet ,ySet);
    LM(xSet, ySet, a, b, c);
    return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( guassNewton )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )



# Eigen
find_package(Eigen3 3.3 REQUIRED)
include_directories(${Eigen3_INCLUDE_DIRS})
# OpenCV
find_package(OpenCV 3.1 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})



add_executable( guassNewton  LM.cc )
target_link_libraries(guassNewton   ${OpenCV_LIBS} )

运行结果

第0次:
a : -2.42817009
b : -1.21346293
c : 6.01511826
delta_abc1.1586837
delta_abc  1.48927117 -0.567649404 -0.959180392
第1次:
a : -0.93889892
b : -1.78111233
c : 5.05593786
delta_abc1.86015631
delta_abc  3.20365945  -1.27539914 -0.902080513
第2次:
a : 2.26476053
b : -3.05651147
c : 4.15385735
delta_abc3.56424271
delta_abc  2.28598992 -0.697482592 -0.902219971
第3次:
a : 4.55075046
b : -3.75399407
c : 3.25163738
delta_abc2.55464925
delta_abc-0.445211318    1.4096799 -0.963758978
第4次:
a : 4.10553914
b : -2.34431417
c : 2.2878784
delta_abc1.76472148
delta_abc -1.31455441     2.111265 -0.826759985
第5次:
a : 2.79098473
b : -0.233049172
c : 1.46111842
delta_abc2.62088254
delta_abc-0.691169818   1.07445643 -0.400135006
第6次:
a : 2.09981491
b : 0.841407256
c : 1.06098341
delta_abc1.33876075
delta_abc-0.0980877612    0.15582017 -0.0599021543
第7次:
a : 2.00172715
b : 0.997227425
c : 1.00108126
delta_abc0.193621802
delta_abc-0.00172660324  0.00277169756 -0.00108091369
第8次:
a : 2.00000055
b : 0.999999123
c : 1.00000034
delta_abc0.00343974425
delta_abc-5.45923277e-07  8.78403636e-07 -3.43603167e-07
第9次:
a : 2
b : 1
c : 0.999999999
delta_abc1.08981113e-06
已收敛,结果为:
final a : 2
final b : 1
final c : 0.999999999