LM算法参数优化实例——残差函数、雅可比矩阵

【目标】:使用LM算法估计 y = e x p ( a t 2 + b t + c ) y=exp(at^2+bt+c) y=exp(at2+bt+c)参数a, b, c
已知量: t t t及与之对应的含有噪声的量测值 y ^ \hat{y} y^;同时,模型已知(仅未知参数)。

仿真数据生成

取参数为a=1.0, b=2.0, c=1.0

	double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N = 100;                        // 数据点数目
    double w_sigma= 1.;                 // 噪声Sigma值(方差)

    std::default_random_engine generator;
    std::normal_distribution<double> noise(0.,w_sigma);

	// 构造 N 次观测
    for (int i = 0; i < N; ++i) {
        double t = i/100.;             // t的取值在0.0~1.0之间
        double n = noise(generator);
        double y = std::exp( a*t*t + b*t + c ) + n; //  // 含噪声的观测 y
    }

确定待优化变量、残差函数、及损失函数

待优化变量(Vertex): 参数a, b, c,此时放在一个 3 × 1 3\times 1 3×1的向量 x x x中;
残差函数(Edge): 以第k步迭代参数的估计值 x k x_k xk(参数)和第i个自变量代入已知模型得到的预测值 y ^ k \hat{y}_k y^k,和第k步迭代的含有噪声的量测值 y k y_k yk的差值作为残差。记为 f i ( x k ) f_i(x_k) fi(xk)
损失函数(Problem): F ( x k ) = Σ i = 1 N ∥ f i ( x k ) ∥ 2 F(x_k)=\Sigma_{i=1}^N\|f_i(x_k)\|_2 F(xk)=Σi=1Nfi(xk)2

【代码1-待优化变量】:

    shared_ptr< CurveFittingVertex > vertex(new CurveFittingVertex());
    vertex->SetParameters(Eigen::Vector3d (0.,0.,0.));

其中,CurveFittingVertex继承自Vertex,且在构造函数中指明了一个3维向量:

// 曲线模型的顶点,模板参数:优化变量维度
class CurveFittingVertex: public Vertex{
public:
    CurveFittingVertex(): Vertex(3) {}  // abc: 三个参数, Vertex 是 3 维的
};

Vertex在构造函数中,其成员变量VecX Parameters_;resize为一个3维向量:

explicit Vertex(int num_dimension, int local_dimension = -1);//类内的声明
Vertex::Vertex(int num_dimension, int local_dimension) {    // 类外的定义
    parameters_.resize(num_dimension, 1);
    id_ = global_vertex_id++; // 初始值有:unsigned long global_vertex_id = 0;
}

注:VecX实际上是一个别名typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VecX;

【代码2-残差项】
对于每一个观测量,构造其对应的残差(此时还并未真正去计算):

// 每个观测对应的残差函数
shared_ptr< CurveFittingEdge > edge(new CurveFittingEdge(t,y));

// CurveFittingEdge 构造函数为:
class CurveFittingEdge: public Edge{
public:
    CurveFittingEdge( double x, double y ): Edge(1,1, std::vector<std::string>{"abc"}) {
        x_ = x;
        y_ = y;
    }
    ...
}

父类Edge的构造函数为:

Edge::Edge(int residual_dimension, int num_verticies,
           const std::vector<std::string> &verticies_types) {
    residual_.resize(residual_dimension, 1); // 残差向量
    jacobians_.resize(num_verticies);        // 雅可比矩阵
    // 信息矩阵
    Eigen::MatrixXd information(residual_dimension, residual_dimension);
    information.setIdentity();
    information_ = information;
}
...
补充其中用到的几个类型
    VecX residual_;                 // 残差
    std::vector<MatXX> jacobians_;  // 雅可比,每个雅可比维度是 residual x vertex[i]
    MatXX information_;             // 信息矩阵
    VecX observation_;              // 观测信息

同时,对于该观测量及其对应的残差,指定它们与哪些待优化的变量(参数)有关!

std::vector<std::shared_ptr<Vertex>> edge_vertex;
edge_vertex.push_back(vertex);  //本次曲线拟合例题中仅一个顶点(其他或许多个)
edge->SetVertex(edge_vertex);  // 指定该edge对应的顶点

// 该成员函数的定义:
bool SetVertex(const std::vector<std::shared_ptr<Vertex>> &vertices) {
    verticies_ = vertices;
    return true;
}
// 其中,成员变量的定义:
std::vector<std::shared_ptr<Vertex>> verticies_; // 该边对应的顶点

【代码3-损失函数与优化】
问题的最终构建和求解

Problem problem(Problem::ProblemType::GENERIC_PROBLEM);
// 其构造函数
Problem::Problem(ProblemType problemType) :
        problemType_(problemType) {
    verticies_marg_.clear();
}
// 其成员变量类型
HashVertex verticies_marg_;
typedef std::map<unsigned long, std::shared_ptr<Vertex>> HashVertex;

添加Vertex

problem.AddVertex(vertex);

// 该成员函数的定义:
bool Problem::AddVertex(std::shared_ptr<Vertex> vertex) {
    if (verticies_.find(vertex->Id()) != verticies_.end()) {
        return false;  // 重复的顶点不会再添加,根据ID来判定是否重复。
    } else {
        verticies_.insert(pair<unsigned long, shared_ptr<Vertex>>(vertex->Id(), vertex));
    }
    return true;
}

// 其成员变量:
HashVertex verticies_;    /// all vertices

(注):每个vertex都有自己的ID。在本例题中,只有一个顶点,因此在使用和优化该顶点的参数时,经常看到Vec3 abc = verticies_[0]->Parameters();中的0,本质是本例题中仅有的一个顶点的ID。

添加Edge

problem.AddEdge(edge);

// 该成员函数定义:
bool Problem::AddEdge(shared_ptr<Edge> edge) {
    edges_.insert(pair<ulong, std::shared_ptr<Edge>>(edge->Id(), edge));
    for (auto &vertex: edge->Verticies()) {
        vertexToEdge_.insert(pair<ulong, shared_ptr<Edge>>(vertex->Id(), edge));
    }
    return true;
}

// 使用的成员变量
HashEdge edges_;    /// all edges
HashVertexIdToEdge vertexToEdge_;     /// 由vertex id查询edge (一对多)
typedef std::unordered_map<unsigned long, std::shared_ptr<Edge>> HashEdge;
typedef std::unordered_multimap<unsigned long, std::shared_ptr<Edge>> HashVertexIdToEdge;

求解

problem.Solve(30);

// 该成员函数的定义:
bool Problem::Solve(int iterations) {    
    SetOrdering();      // 统计待估计的所有变量的总维度,为构建 H 矩阵做准备
    MakeHessian();      // 遍历edge, 构建 H = J^T * J 矩阵
    ComputeLambdaInitLM();  // LM 初始化
    
    // LM 算法迭代求解
    bool stop = false;
    int iter = 0;
    while (!stop && (iter < iterations)) {
        bool oneStepSuccess = false;
        int false_cnt = 0;
        while (!oneStepSuccess)  { // 不断尝试 Lambda, 直到成功迭代一步
            AddLambdatoHessianLM();   // setLambda
            
            // 第四步,解线性方程 H X = B
            SolveLinearSystem();
            //
            RemoveLambdaHessianLM();
            // 优化退出条件1: delta_x_ 很小则退出
            if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
                stop = true;
                break;
            }
            // 更新状态量 X = X+ delta_x
            UpdateStates();
            // 判断当前步是否可行以及 LM 的 lambda 怎么更新
            oneStepSuccess = IsGoodStepInLM();
            // 后续处理,
            if (oneStepSuccess) {
                // 在新线性化点 构建 hessian
                MakeHessian();             
                false_cnt = 0;
            } else {
                false_cnt++;
                RollbackStates();   // 误差没下降,回滚
            }
        }
        iter++;

        // 优化退出条件3: currentChi_ 跟第一次的chi2相比,下降了 1e6 倍则退出
        if (sqrt(currentChi_) <= stopThresholdLM_)
            stop = true;
    }
    return true;
}

依次简要看一下成员函数:
1.统计维数

成员函数Problem::SetOrdering()的定义:
// Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
// 统计待估计的所有变量的总维度
for (auto vertex: verticies_) {
    ordering_generic_ += vertex.second->LocalDimension();  // 所有的优化变量总维数
}

(注):对于本例题是3维!!

2.求解Hessian

void Problem::MakeHessian() {
    // 直接构造大的 H 矩阵
    ulong size = ordering_generic_;  // 维数
    MatXX H(MatXX::Zero(size, size));
    VecX b(VecX::Zero(size));
    
    // 遍历每个残差,并计算他们的雅克比,得到最后的 H = J^T * J
    for (auto &edge: edges_) {   
        // 其中的一个edge,计算残差和雅可比
        edge.second->ComputeResidual(); // 这两个成员函数在子类中进行了override
        edge.second->ComputeJacobians(); // 关于它俩的推导,见补充

        std::vector<MatXX> jacobians = edge.second->Jacobians();
        VecX verticies = edge.second->Verticies();
        assert(jacobians.size() == verticies.size()); // 两个都是vector,本例题中为1

        // 顶点中的每一个参数
        for (size_t i = 0; i < verticies.size(); ++i) {
            auto v_i = verticies[i];
            if (v_i->IsFixed()) continue;    // Hessian 里不需要添加它的信息,也就是它的雅克比为 0

            auto jacobian_i = jacobians[i]; // 该参数对应的雅可比
            ulong index_i = v_i->OrderingId();
            ulong dim_i = v_i->LocalDimension();
            // 此处PDF上直接为 H = J^T * J ?
            MatXX JtW = jacobian_i.transpose() * edge.second->Information();
            // 上三角
            for (size_t j = i; j < verticies.size(); ++j) {
                auto v_j = verticies[j];
                if (v_j->IsFixed()) continue;

                auto jacobian_j = jacobians[j];
                ulong index_j = v_j->OrderingId();
                ulong dim_j = v_j->LocalDimension();

                assert(v_j->OrderingId() != -1);
                MatXX hessian = JtW * jacobian_j;
                // 所有的信息矩阵叠加起来(即填充了一个H中的一块区域)
                H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
                if (j != i) {  // 对称的下三角
                    H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();
                }
            }
            b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
        }

    }
    Hessian_ = H;
    b_ = b;

    delta_x_ = VecX::Zero(size);  // initial delta_x = 0_n;
}

代码中出现了Eigen中的一个noalias()语句,其解释见博客链接:https://www.cnblogs.com/houkai/p/6349990.html
总而言之,该部分实现了正则方程(normal equation)中系数的确定,即 H Δ x = b H\Delta x=b HΔx=b中的 H H H b b b,下一步求解其中的增量 Δ x \Delta x Δx
【补充:】子类中override的两个成员函数的推导和代码:(仅适用于该例题)
y = e a t 2 + b t + c , x = [ a , b , c ] T y=e^{at^2+bt+c},\quad x=[a,b,c]^T y=eat2+bt+c,x=[a,b,c]T
∂ y ∂ x = [ ∂ y ∂ a ∂ y ∂ b ∂ y ∂ c ] = [ ∂ y ∂ a ∂ y ∂ b ∂ y ∂ c ] = [ t 2 ⋅ y t ⋅ y y ] \frac{\partial y}{\partial x}=\begin{bmatrix} \frac{\partial y}{\partial a}\\ \frac{\partial y}{\partial b}\\ \frac{\partial y}{\partial c} \end{bmatrix}=\begin{bmatrix} \frac{\partial y}{\partial a}\\ \frac{\partial y}{\partial b}\\ \frac{\partial y}{\partial c} \end{bmatrix}=\begin{bmatrix} t^2\cdot y\\ t\cdot y \\ y \end{bmatrix} xy=aybycy=aybycy=t2ytyy
y ( x + Δ x ) = y ( x ) + ∂ y ∂ x Δ x y(x+\Delta x)=y(x)+\frac{\partial y}{\partial x}\Delta x y(x+Δx)=y(x)+xyΔx

// 计算曲线模型误差
virtual void ComputeResidual() override{
    Vec3 abc = verticies_[0]->Parameters();  // 估计的参数 // 顶点有自己的ID
    residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_;  // 构建残差
}

// 计算残差对变量的雅克比
virtual void ComputeJacobians() override{
    Vec3 abc = verticies_[0]->Parameters();
    double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) );

    Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维,状态量 3 个,所以是 1x3 的雅克比矩阵
    jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y;
    jacobians_[0] = jaco_abc;
}

2.2 LM的H矩阵
LM算法求解的是方程: ( J T J + λ I ) Δ x = − J T f (J^TJ+\lambda I)\Delta x = -J^Tf (JTJ+λI)Δx=JTf,目前求得了其中的 H = J T J H=J^TJ H=JTJ b = − J T f b=-J^Tf b=JTf。目前还差一个 λ \lambda λ(它在每一步优化中动态调整)。
一个简单的 λ \lambda λ的初始化策略:
λ 0 = τ ⋅ m a x { ( J T J ) i i } \lambda_0=\tau\cdot max\{(J^TJ)_{ii}\} λ0=τmax{(JTJ)ii}
其中, τ \tau τ一般取 [ 1 0 − 8 , 1 ] [10^{-8},1] [108,1]

void Problem::ComputeLambdaInitLM() {
    double maxDiagonal = 0;
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    for (ulong i = 0; i < size; ++i) {
        maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal);
    }
    double tau = 1e-5;
    currentLambda_ = tau * maxDiagonal;
}

一种 λ \lambda λ的更新策略:
ρ = F ( x ) − F ( x + Δ x ) L ( 0 ) − L ( Δ x ) \rho=\frac{F(x)-F(x+\Delta x)}{L(0)-L(\Delta x)} ρ=L(0)L(Δx)F(x)F(x+Δx)
λ : = λ m a x { 1 3 , 1 − ( 2 ρ − 1 ) 3 } ; ν : = 2 ; ( ρ &gt; 0 ) λ : = λ ∗ ν ; ν : = 2 ∗ ν ; ( ρ ≤ 0 ) \begin{aligned} \lambda := \lambda max\{\frac{1}{3},1-(2\rho-1)^3\}; \quad \nu:=2; \quad &amp; (\rho&gt;0) \\ \lambda := \lambda * \nu;\quad \nu:=2*\nu; \quad &amp; (\rho \leq 0) \end{aligned} λ:=λmax{31,1(2ρ1)3};ν:=2;λ:=λν;ν:=2ν;(ρ>0)(ρ0)

其中, L ( Δ x ) L(\Delta x) L(Δx)为损失函数二阶泰勒展开的近似,即 F ( x + Δ x ) ≈ L ( Δ x ) = F ( x ) + J Δ x + 1 2 Δ x T H Δ x F(x+\Delta x)\approx L(\Delta x)=F(x)+J\Delta x+\frac{1}{2}\Delta x^TH\Delta x F(x+Δx)L(Δx)=F(x)+JΔx+21ΔxTHΔx。且有:
L ( 0 ) − L ( Δ x ) = 1 2 Δ x T ( λ Δ x + b ) L(0)-L(\Delta x)=\frac{1}{2}\Delta x^T(\lambda\Delta x+b) L(0)L(Δx)=21ΔxT(λΔx+b)
下面的代码对应的非常完美:
F ( x ) F(x) F(x)对应currentChi F ( x + Δ x ) F(x+\Delta x) F(x+Δx)对应tempChi

bool Problem::IsGoodStepInLM() {
    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_); // 似乎少了1/2
    scale += 1e-3;    // make sure it's non-zero :)

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    double rho = (currentChi_ - tempChi) / scale;
    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        double alpha = 1. - pow((2 * rho - 1), 3);
        alpha = std::min(alpha, 2. / 3.);  // 此处比公式中还多了一步
        double scaleFactor = (std::max)(1. / 3., alpha);
        currentLambda_ *= scaleFactor;
        ni_ = 2;
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ *= ni_;
        ni_ *= 2;
        return false; // 用新的lambda值再去【重新计算】当前步
    }
}

3.求解 Δ x \Delta x Δx

// Solve Hx = b, we can use PCG iterative method or use sparse Cholesky
void Problem::SolveLinearSystem() {
        delta_x_ = Hessian_.inverse() * b_; // 很直接的方法
}

求得 Δ x \Delta x Δx之后,首先要根据 ρ \rho ρ判断它是否是一次有效的更新(确实使得损失函数下降);在是有效更新的前提下,考虑是否满足终止条件。

4.终止条件

优化退出条件1: delta_x_ 很小则退出
if (delta_x_.squaredNorm() <= 1e-6) {
    stop = true;
    break;
}

记录 λ \lambda λ的变化

  • 4
    点赞
  • 35
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值