01 LM 算法及 Cpp 实现

文章目录

    • 01 LM 算法及 Cpp 实现
      • 1.1 应用
      • 1.2 阻尼法推导
      • 1.3 Cpp 算法实现

01 LM 算法及 Cpp 实现

1.1 应用

LM 算法用于解决非线性最小二乘问题

min ⁡ x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 (1) \min _x F(x)=\frac{1}{2}\|f(\boldsymbol{x})\|_2^2 \tag{1} xminF(x)=21f(x)22(1)

其中, f ( x ) f(\boldsymbol{x}) f(x) 是指各项的残差。

LM 算法有两种推导方式,即 阻尼法置信域 法(见《十四讲》),这里用阻尼法进行推导。

1.2 阻尼法推导

(1)一阶泰勒展开近似

f ( x ) f(\boldsymbol{x}) f(x) x n \boldsymbol{x_n} xn 处一阶泰勒展开(把 Δ x \Delta \boldsymbol{x} Δx 看做未知数),

f ( x n + Δ x ) ≈ f ( x n ) + J ( x n ) Δ x (2) f(\boldsymbol{x_n}+\Delta \boldsymbol{x}) \approx f(\boldsymbol{x_n})+\boldsymbol{J}\left(\boldsymbol{x}_n\right) \Delta \boldsymbol{x} \tag{2} f(xn+Δx)f(xn)+J(xn)Δx(2)

那么问题转化为,对于每次迭代,求解最优的 Δ x \Delta \boldsymbol{x} Δx

Δ x ∗ = arg ⁡ min ⁡ Δ x 1 2 ∥ f ( x n ) + J n ( x n ) Δ x ∥ 2 (3) \Delta \boldsymbol{x}^*=\arg \min _{\Delta \boldsymbol{x}} \frac{1}{2}\left\|f\left(\boldsymbol{x}_{\boldsymbol{n}}\right)+\boldsymbol{J}_{\boldsymbol{n}}\left(\boldsymbol{x}_{\boldsymbol{n}}\right) \Delta \boldsymbol{x}\right\|^2 \tag{3} Δx=argΔxmin21f(xn)+Jn(xn)Δx2(3)

(2)加入阻尼项

Δ x ∗ = arg ⁡ min ⁡ Δ x M ( Δ x ) = 1 2 ∥ f ( x n ) + J n ( x n ) Δ x ∥ 2 + 1 2 μ Δ x T Δ x (4) \Delta \boldsymbol{x}^*=\arg \min _{\Delta \boldsymbol{x}} M(\Delta \boldsymbol{x})=\frac{1}{2}\left\|f\left(\boldsymbol{x}_{\boldsymbol{n}}\right)+\boldsymbol{J}_{\boldsymbol{n}}\left(\boldsymbol{x}_{\boldsymbol{n}}\right) \Delta \boldsymbol{x}\right\|^2+\frac{1}{2} \mu \Delta \boldsymbol{x}^T \Delta \boldsymbol{x} \tag{4} Δx=argΔxminM(Δx)=21f(xn)+Jn(xn)Δx2+21μΔxTΔx(4)

其中, μ \mu μ 为阻尼系数,阻尼项 1 2 μ Δ x T Δ x \frac{1}{2} \mu \Delta \boldsymbol{x}^T \Delta \boldsymbol{x} 21μΔxTΔx 可以看做是对于过大的 Δ x \Delta \boldsymbol{x} Δx 的惩罚。

(3)求极值

Δ x \Delta \boldsymbol{x} Δx 求导,并令其等于零,

J ( x n ) T f ( x n ) + J ( x n ) T J ( x n ) Δ x + μ Δ x = 0 (5) \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)^T f\left(\boldsymbol{x}_{\boldsymbol{n}}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)^T \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right) \Delta \boldsymbol{x}+\mu \Delta \boldsymbol{x}=\mathbf{0} \tag{5} J(xn)Tf(xn)+J(xn)TJ(xn)Δx+μΔx=0(5)

Δ x ∗ = − ( J ( x n ) T J ( x n ) + μ I ) − 1 J ( x n ) T f ( x n ) (6) \Delta \boldsymbol{x}^*=-\left(\boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)^T \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)+\mu \boldsymbol{I}\right)^{-1} \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)^T f\left(\boldsymbol{x}_{\boldsymbol{n}}\right) \tag{6} Δx=(J(xn)TJ(xn)+μI)1J(xn)Tf(xn)(6)

J ( x n ) T J ( x n ) = H J ( x n ) T f ( x n ) = g \begin{aligned} \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)^T \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right) &=\boldsymbol{H} \\ \boldsymbol{J}\left(\boldsymbol{x}_{\boldsymbol{n}}\right)^T f\left(\boldsymbol{x}_{\boldsymbol{n}}\right) &=\boldsymbol{g} \end{aligned} J(xn)TJ(xn)J(xn)Tf(xn)=H=g

则式(6)可写为

Δ x ∗ = − ( H + μ I ) − 1 g (7) \Delta \boldsymbol{x}^*=-(\boldsymbol{H}+\mu \boldsymbol{I})^{-1} \boldsymbol{g} \tag{7} Δx=(H+μI)1g(7)

(4)阻尼系数 μ \mu μ 的调节

定义增益系数 ρ \rho ρ

ρ = f ( x + Δ x ) − f ( x ) J ( x ) T Δ x (8) \rho=\frac{f(\boldsymbol{x}+\boldsymbol{\Delta x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x})^T\Delta \boldsymbol{x}} \tag{8} ρ=J(x)TΔxf(x+Δx)f(x)(8)

其中,分子是实际下降的值,分母是近似下降的值。若 ρ \rho ρ 接近于 1 ,则近似效果好;若 ρ \rho ρ 太小,则说明实际减小的值远小于近似减小的值,即近似效果较差,需缩小近似范围,即增大阻尼系数 μ \mu μ; 若 ρ \rho ρ 太大,则说明实际减小的值大于近似减小的值,则需放大近似范围,即减小 μ \mu μ

两种调节方法:

 if  ρ < 0.25 μ : = μ ∗ 2  else if  ρ > 0.75 μ : = μ / 3 \begin{aligned} \text { if } & \rho<0.25 \\ & \mu:=\mu * 2 \\ \text { else if } & \rho>0.75 \\ & \mu:=\mu / 3 \end{aligned}  if  else if ρ<0.25μ:=μ2ρ>0.75μ:=μ/3

也就是, ρ < 0.25 \rho<0.25 ρ<0.25 时增大阻尼系数; ρ > 0.75 \rho>0.75 ρ>0.75 时,减小阻尼系数。

或者:

i f ρ > 0 μ : = μ ∗ max ⁡ { 1 3 , 1 − ( 2 ρ − 1 ) 3 } ; ν : = 2 e l s e μ : = μ ∗ ν ; ν : = 2 ∗ ν \begin{aligned} if \rho>0 \\ &\mu:=\mu * \max \left\{\frac{1}{3}, 1-(2 \rho-1)^3\right\} ; \quad \nu:=2\\ else\\ &\mu:=\mu * \nu ; \quad \nu:=2 * \nu \end{aligned} ifρ>0elseμ:=μmax{31,1(2ρ1)3};ν:=2μ:=μν;ν:=2ν

1.3 Cpp 算法实现

两种方法差别在于 ρ \rho ρ 的分母的计算(我的更快?)。

优化目标是待定系数,把他们看做 未知数,需要计算的 step 就是这些待定系数的 step。

(1)我的方法

/***********************************************************                                          *
* Time: 2022/11/3
* Author: xiaocong
* Function: LM 算法
 *
 * @ 解决的是最小二乘问题,也就是找到最优的系数使得残差最小
 * @ obj = A * sin(Bx) + C * cos(D*x) - F
 *      A * sin(Bx) + C * cos(D*x) 是目标函数
 *      F 是实际值
 *      目标是找到使 obj 最小的系数 A B C D
***********************************************************/

#include       // 稠密矩阵
#include       // 稀疏矩阵
#include 
#include 

using namespace std;
using namespace Eigen;

const double DERIV_STEP = 1e-5;
const int MAX_INTER = 100;

#define max(a, b) (((a)>(b))?(a):(b))

// 定义目标函数
double func(const VectorXd input, const VectorXd &output, const VectorXd ¶ms, int objIndex)
{
    // obj = A * sin(Bx) + C * cos(D*x) - F
    double x1 = params(0);   // params 中存储的是系数
    double x2 = params(1);
    double x3 = params(2);
    double x4 = params(3);

    double t = input(objIndex);    // input 中存储的是 x
    double f = output(objIndex);   // output 中存储的是实际值

    return x1 * sin(x2 * t) + x3 * cos(x4 * t) - f;    // 返回 objIndex 项的残差
}

// 计算残差矩阵
VectorXd objF(const VectorXd &input, const VectorXd &output, const VectorXd ¶ms)
{
    VectorXd obj(input.rows());    // 存储所有残差,也就是残差矩阵
    for (int i = 0; i < input.rows(); i++)
        obj(i) = func(input, output, params, i);
    return obj;   // 返回残差矩阵
}


// 残差平方和
double Func(const VectorXd &obj)
{
    return obj.squaredNorm() / 2.0;
}

// 求某个系数在某点的导数
double Deriv(const VectorXd &input, const VectorXd &output, int objIndex, const VectorXd ¶ms, int paraIndex)
{
    VectorXd para1 = params;
    VectorXd para2 = params;

    para1(paraIndex) -= DERIV_STEP;
    para2(paraIndex) += DERIV_STEP;

    double obj1 = func(input, output, para1, objIndex);
    double obj2 = func(input, output, para2, objIndex);

    return (obj2 - obj1) / (2 * DERIV_STEP);     // 该点处的导数,为求雅克比矩阵做准备
}


// 计算雅克比矩阵
/****************************
 * 我们优化的是系数 params,把他们看做未知数,分别求导,得到雅克比矩阵
 * 维度:(input.rows() x params.rows())
 * [[df/dA df/dB df/dC df/dD]    <--- x1
 *  [df/dA df/dB df/dC df/dD]    <--- x2
 *  [.......................]
 *  [df/dA df/dB df/dC df/dD]]    <--- xn
 ****************************/
MatrixXd Jacobian(const VectorXd &input, const VectorXd &output, const VectorXd ¶ms)
{
    int rowNum = input.rows();
    int colNum = params.rows();

    MatrixXd Jac(rowNum, colNum);

    for (int i = 0; i < rowNum; i++)
        for (int j = 0; j < colNum; j++)
            Jac(i, j) = Deriv(input, output, i, params, j);

    return Jac;
}


//求 Hessian 矩阵对角线最大值
// Hessian 矩阵:二阶导数
double maxMatrixDiagonale(const MatrixXd &Hessian)
{
    int max = 0;
    for (int i = 0; i < Hessian.rows(); i++)
    {
        if(Hessian(i, i) > max)
            max = Hessian(i, i);
    }
    return max;
}


// 近似下降值
double linerDeltaL(const VectorXd &step, const MatrixXd &Jac)
{
    VectorXd L = Jac * step;
    return L.norm();
}


void levenMar(const VectorXd &input, const VectorXd &output, VectorXd ¶ms)
{
    double epsilon = 1e-12;
    int iterCnt = 0;         // 迭代计数
    double tao = 1e-3;
    long long v = 2;

    // 求初始的 u
    MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵
    MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵,4x4
    double u = tao * maxMatrixDiagonale(H);     // Hessian 矩阵对角线最大值乘 tao


    while (iterCnt < MAX_INTER)     // double 类型不能直接作比较
    {
        VectorXd obj = objF(input, output, params);    // 误差矩阵
        MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵
        MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵,4x4
        VectorXd g = Jac.transpose() * obj;     // 也就是 g,4x1

        VectorXd step = (H + u * MatrixXd::Identity(H.rows(), H.cols())).inverse() * g;
        if(step.norm() < epsilon)
            break;

        cout << "H:" << endl << H << endl;
        cout << "step: " << endl << step << endl;

        VectorXd paramsNew(params.rows());
        paramsNew = params - step;            // 更新 params

        // 计算 params 误差
        obj = objF(input, output, params);

        // 计算 paramsNew 误差
        VectorXd  obj_new = objF(input, output, paramsNew);

        double deltaF = Func(obj) - Func(obj_new);     // 求差
        double deltaL = linerDeltaL(step, Jac);

        // 计算增益系数 rho
        double rho = deltaF / deltaL;          // 实际下降值 / 近似下降值
        cout << "rho is; " << rho < 0)
        {
            params = paramsNew;
            u *= max(1.0 / 3.0, 1 - pow(2 * rho - 1, 3));
            v = 2;
        } else
        {
            u = u * v;
            v = v * 2;
        }
        cout << "u= " << u << "\tv= " << v << endl;

        iterCnt ++;
        cout << "Iteration " << iterCnt << " times, result is :" << endl<< params << endl;

    }
}


int main()
{
    int params_num = 4;
    int total_data = 100;

    VectorXd input(total_data);
    VectorXd output(total_data);

    double A = 5, B = 1, C = 10, D = 2;    // 初始化

    // 生成数据
    for (int i = 0; i < total_data; i++)
    {
        double x = 20.0 * ((rand() % 1000) / 1000.0) - 10.0;    // [-10, 10]
        double deltaY = 2.0 * (rand() % 1000) / 1000.0;         // 随机噪声,[0, 2]
        double y = A * sin(B*x) + C * cos(D*x) + deltaY;
        input(i) = x;
        output(i) = y;
    }


    VectorXd params_levenMar(params_num);
    params_levenMar << 3.6, 1.3, 7.2, 1.7;

    levenMar(input, output, params_levenMar);
    cout << "**********************************************" << endl;
    cout << "Levenberg-Marquardt parameter: " << endl << params_levenMar << endl;

}

输出

Levenberg-Marquardt parameter:
 4.85628
0.997904
 10.0524
   2.003

(2)网络方法

/***********************************************************                                          *
* Time: 2022/11/2
* Author: xiaocong
* Function: LM 算法
 *
 * @ 解决的是最小二乘问题,也就是找到最优的系数使得残差最小
 * @ obj = A * sin(Bx) + C * cos(D*x) - F
 *      A * sin(Bx) + C * cos(D*x) 是目标函数
 *      F 是实际值
 *      目标是找到使 obj 最小的系数 A B C D
***********************************************************/

#include       // 稠密矩阵
#include       // 稀疏矩阵
#include 
#include     // 控制输入输出格式等
#include 

using namespace std;
using namespace Eigen;

const double DERIV_STEP = 1e-5;
const int MAX_INTER = 100;

#define max(a, b) (((a)>(b))?(a):(b))

// 定义目标函数
double func(const VectorXd input, const VectorXd &output, const VectorXd ¶ms, int objIndex)
{
    // obj = A * sin(Bx) + C * cos(D*x) - F
    double x1 = params(0);   // params 中存储的是系数
    double x2 = params(1);
    double x3 = params(2);
    double x4 = params(3);

    double t = input(objIndex);    // input 中存储的是 x
    double f = output(objIndex);   // output 中存储的是实际值

    return x1 * sin(x2 * t) + x3 * cos(x4 * t) - f;    // 返回 objIndex 项的残差
}

// 计算残差矩阵
VectorXd objF(const VectorXd &input, const VectorXd &output, const VectorXd ¶ms)
{
    VectorXd obj(input.rows());    // 存储所有残差,也就是残差矩阵
    for (int i = 0; i < input.rows(); i++)
        obj(i) = func(input, output, params, i);
    return obj;   // 返回残差矩阵
}


// 残差平方和
double Func(const VectorXd &obj)
{
    return obj.squaredNorm() / 2.0;
}

// 求某个系数在某点的导数
double Deriv(const VectorXd &input, const VectorXd &output, int objIndex, const VectorXd ¶ms, int paraIndex)
{
    VectorXd para1 = params;
    VectorXd para2 = params;

    para1(paraIndex) -= DERIV_STEP;
    para2(paraIndex) += DERIV_STEP;

    double obj1 = func(input, output, para1, objIndex);
    double obj2 = func(input, output, para2, objIndex);

    return (obj2 - obj1) / (2 * DERIV_STEP);     // 该点处的导数,为求雅克比矩阵做准备
}


// 计算雅克比矩阵
/****************************
 * 我们优化的是系数 params,把他们看做未知数,分别求导,得到雅克比矩阵
 * 维度:(input.rows() x output.rows())
 * [[df/dA df/dB df/dC df/dD]    <--- x1
 *  [df/dA df/dB df/dC df/dD]    <--- x2
 *  [.......................]
 *  [df/dA df/dB df/dC df/dD]]    <--- xn
 ****************************/
MatrixXd Jacobian(const VectorXd &input, const VectorXd &output, const VectorXd ¶ms)
{
    int rowNum = input.rows();
    int colNum = params.rows();

    MatrixXd Jac(rowNum, colNum);

    for (int i = 0; i < rowNum; i++)
        for (int j = 0; j < colNum; j++)
            Jac(i, j) = Deriv(input, output, i, params, j);

    return Jac;
}


//求 Hessian 矩阵对角线最大值
// Hessian 矩阵:二阶导数
double maxMatrixDiagonale(const MatrixXd &Hessian)
{
    int max = 0;
    for (int i = 0; i < Hessian.rows(); i++)
    {
        if(Hessian(i, i) > max)
            max = Hessian(i, i);
    }
    return max;
}


double linerDeltaL(const VectorXd &step, const VectorXd &gradient, const double u)
{
    double L = step.transpose() * (u * step - gradient);
    return L;
}


void levenMar(const VectorXd &input, const VectorXd &output, VectorXd ¶ms)
{
    int errNum = input.rows();
    int paraNum = params.rows();

    // initial parameters
    VectorXd obj = objF(input, output, params);    // 得到误差矩阵
    MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵
    MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵,4x4
    VectorXd gradient = Jac.transpose() * obj;     // 也就是 g,4x1

    double tao = 1e-3;
    long  long v = 2;
    double epsilon1 = 1e-12, epsilon2 = 1e-12;
    double u = tao * maxMatrixDiagonale(H);     // Hessian 矩阵对角线最大值乘 tao
    bool found = gradient.norm() <= epsilon1;
    if (found) return;        // 直接退出程序,不再执行后面的程序

    double last_sum = 0;
    int iterCnt = 0;         // 迭代计数

    while (iterCnt < MAX_INTER)
    {
        VectorXd obj = objF(input, output, params);    // 误差矩阵

        MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵
        MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵,4x4
        VectorXd gradient = Jac.transpose() * obj;     // 也就是 g,4x1

        if(gradient.norm() < epsilon1)
        {
            cout << "stop g(x) = 0 for a local minimizer optimizer." << endl;
            break;
        }

        cout << "H:" << endl << H << endl;

        VectorXd step = (H + u * MatrixXd::Identity(paraNum, paraNum)).inverse() * gradient;
        // 求 Delta x = (H + uI)^{-1}g      注意:step 维度(4x1)

        cout << "step: " << endl << step << endl;

        if(step.norm() <= epsilon2 * (params.norm()) + epsilon2)
        {
            cout << "stop because change in x is small" << endl;
            break;
        }

        VectorXd paramsNew(params.rows());
        paramsNew = params - step;            // 更新 params

        // 计算 params 误差
        obj = objF(input, output, params);

        // 计算 paramsNew 误差
        VectorXd  obj_new = objF(input, output, paramsNew);

        double deltaF = Func(obj) - Func(obj_new);     // 求差
        double deltaL = linerDeltaL(-1 * step, gradient, u);

        // 计算增益系数 rho
        double rho = deltaF / deltaL;          // 实际下降值 / 近似下降值
        cout << "rho is; " << rho < 0)
        {
            params = paramsNew;
            u *= max(1.0 / 3.0, 1 - pow(2 * rho - 1, 3));
            v = 2;
        } else
        {
            u = u * v;
            v = v * 2;
        }
        cout << "u= " << u << "\tv= " << v << endl;

        iterCnt ++;
        cout << "Iteration " << iterCnt << " times, result is :" << endl<< params << endl;


    }
}


int main()
{
    int params_num = 4;
    int total_data = 100;

    VectorXd input(total_data);
    VectorXd output(total_data);

    double A = 5, B = 1, C = 10, D = 2;    // 初始化

    // 生成数据
    for (int i = 0; i < total_data; i++)
    {
        double x = 20.0 * ((rand() % 1000) / 1000.0) - 10.0;    // [-10, 10]
        double deltaY = 2.0 * (rand() % 1000) / 1000.0;         // 随机噪声,[0, 2]
        double y = A * sin(B*x) + C * cos(D*x) + deltaY;
        input(i) = x;
        output(i) = y;
    }


    VectorXd params_levenMar(params_num);
    params_levenMar << 3.6, 1.3, 7.2, 1.7;

    levenMar(input, output, params_levenMar);
    cout << "Levenberg-Marquardt parameter: " << endl << params_levenMar << endl << endl << endl;
    cout << "**********************************************" << endl;

}

你可能感兴趣的:(视觉SLAM十四讲,算法,SLAM)