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

最新推荐文章于 2022-08-29 15:47:42 发布

changshen_xu

最新推荐文章于 2022-08-29 15:47:42 发布

阅读量6.1k

点赞数 4

分类专栏： # ----------VIO原理系列文章标签： VINS

本文链接：https://blog.csdn.net/CSDN_XCS/article/details/95938482

版权

----------VIO原理系列专栏收录该内容

14 篇文章 4 订阅

订阅专栏

【目标】：使用LM算法估计 $y=exp(at^2+bt+c)$ 参数a, b, c。
已知量： $t$ 及与之对应的含有噪声的量测值 $\hat{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\times 1$ 的向量 $x$ 中；
残差函数（Edge）： 以第k步迭代参数的估计值 $x_k$ （参数）和第i个自变量代入已知模型得到的预测值 $\hat{y}_k$ ，和第k步迭代的含有噪声的量测值 $y_k$ 的差值作为残差。记为 $f_i(x_k)$
损失函数（Problem）： $F(x_k)=\Sigma_{i=1}^N\|f_i(x_k)\|_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\Delta x=b$ 中的 $H$ 和 $b$ ，下一步求解其中的增量 $\Delta x$ 。
【补充：】子类中override的两个成员函数的推导和代码：（仅适用于该例题）
$y=e^{at^2+bt+c},\quad x=[a,b,c]^T$
$\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}$
$y(x+\Delta x)=y(x)+\frac{\partial y}{\partial x}\Delta 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^TJ+\lambda I)\Delta x = -J^Tf$ ，目前求得了其中的 $H=J^TJ$ ， $b=-J^Tf$ 。目前还差一个 $\lambda$ （它在每一步优化中动态调整）。
一个简单的 $\lambda$ 的初始化策略：
$\lambda_0=\tau\cdot max\{(J^TJ)_{ii}\}$
其中， $\tau$ 一般取 $10^{-8},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$ 的更新策略：
$\rho=\frac{F(x)-F(x+\Delta x)}{L(0)-L(\Delta x)}$
$\begin{aligned} \lambda := \lambda max\{\frac{1}{3},1-(2\rho-1)^3\}; \quad \nu:=2; \quad & (\rho>0) \\ \lambda := \lambda * \nu;\quad \nu:=2*\nu; \quad & (\rho \leq 0) \end{aligned}$

其中， $L(\Delta x)$ 为损失函数二阶泰勒展开的近似，即 $F(x+\Delta x)\approx L(\Delta x)=F(x)+J\Delta x+\frac{1}{2}\Delta x^TH\Delta x$ 。且有：
$L(0)-L(\Delta x)=\frac{1}{2}\Delta x^T(\lambda\Delta x+b)$
下面的代码对应的非常完美：
$F (x)$ 对应currentChi， $F(x+\Delta 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.求解 $\Delta x$

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

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

4.终止条件

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

记录 $\lambda$ 的变化

changshen_xu

关注

4
点赞
踩
35

收藏

觉得还不错? 一键收藏
2
评论
LM算法参数优化实例——残差函数、雅可比矩阵

【目标】：使用LM算法估计y=exp(at2+bt+c)y=exp(at^2+bt+c)y=exp(at2+bt+c)参数a, b, c。已知量：ttt及与之对应的含有噪声的量测值y^\hat{y}y^；同时，模型已知（仅未知参数）。仿真数据生成取参数为a=1.0, b=2.0, c=1.0 double a=1.0, b=2.0, c=1.0; // 真实参数值 ...
复制链接

扫一扫