手写VIO第三课-LM代码

深蓝学院vio课程的学习记录，个人备忘。

LM代码

在vio第三讲的代码中，主要是对于y=exp（axx+bx+c）这个函数进行迭代优化，这里面优化变量是abc。下面是代码部分：

int main()
{
    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);

在这里设置了优化变量的真实值，a=1.0, b=2.0, c=1.0，100个数据点等相关参数，定义随机数发生器generator，并定义了noise是符合（0，1）的高斯分布。

Problem problem(Problem::ProblemType::GENERIC_PROBLEM);

开始构建Problem，在这里初始化problem为GENERIC_PROBLEM，在Problem.h中可以看到以下代码：

enum class ProblemType {
        SLAM_PROBLEM,
        GENERIC_PROBLEM
    };

在这里如果是SLAM_PROBLEM，那么后面构建的H阵是稀疏的，pose和landmark是分开存储的，并且可以通过舒尔补来加速计算，如果是GENERIC_PROBLEM那么构建的H阵是稠密的。

shared_ptr< CurveFittingVertex > vertex(new CurveFittingVertex());//定义优化的顶点
 //vertex->SetParameters(Eigen::Vector3d (0.,0.,0.));
    Eigen::VectorXd x(3);
    x<<0.0,0.0,0.0;
    vertex->SetParameters(x);

构建顶点vertex，即优化变量是abc,此刻将他们设置为一个3x1 vector变量x并为其初始化x[0]=0.0,x[1]=0.0,x[2]=0.0,并将其设置到顶点中去。

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public Vertex
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingVertex(): Vertex(3) {}  // abc: 三个参数， Vertex 是 3 维的
    virtual std::string TypeInfo() const { return "abc"; }//debug时候用的，返回待优化变量
};

曲线模型的顶点是继承子Vertex这个类，CurveFittingVertex(): Vertex(3) {}表示待优化量的维数是3维的，进入Vertex.h中

explicit Vertex(int num_dimension, int local_dimension = -1);


——————————————————————————————————————
Vertex::Vertex(int num_dimension, int local_dimension) {
    parameters_.resize(num_dimension, 1);
    local_dimension_ = local_dimension > 0 ? local_dimension : num_dimension;
    id_ = global_vertex_id++;

//    std::cout << "Vertex construct num_dimension: " << num_dimension
//              << " local_dimension: " << local_dimension << " id_: " << id_ << std::endl;
}

对于Vertex中的参数，num_dimension表示的是待优化的参数维度，local_dimension表示的是参与优化的参数维度，当他=-1的时候表示与待优化参数维度是一样的（后面的位姿优化可能不一样）。parameters_被resize成一个3X1的vector。local_dimension_ = local_dimension > 0 ? local_dimension : num_dimension;表示本地参与s优化的维度是否大于0？如果小于0，那么local_dimension_ =num_dimension，如果大于0，local_dimension_就等他输入进来的维度。

// 将待估计的参数加入最小二乘问题
    problem.AddVertex(vertex);

为problrm添加顶点。

bool Problem::AddVertex(std::shared_ptr<Vertex> vertex) {
    if (verticies_.find(vertex->Id()) != verticies_.end()) {//查看verticies_中是否含有重复的vertex
        // LOG(WARNING) << "Vertex " << vertex->Id() << " has been added before";
        return false;
    } else {
        verticies_.insert(pair<unsigned long, shared_ptr<Vertex>>(vertex->Id(), vertex));
    }//如果这里面没有该顶点，则插入

    return true;
}

verticies_是一个map<unsigned long, std::shared_ptr<Vertex>>类型。

下面开始构造观测，在y上面加入了一个符合高斯分布的噪声。

 // 构造 N 次观测
    for (int i = 0; i < N; ++i) {

        double x = i/100.;
        double n = noise(generator);
        // 观测 y
        double y = std::exp( a*x*x + b*x + c ) + n;
//        double y = std::exp( a*x*x + b*x + c );

有了观测数据后，开始构建残差，即edge。

// 每个观测对应的残差函数
        shared_ptr< CurveFittingEdge > edge(new CurveFittingEdge(x,y));
        std::vector<std::shared_ptr<Vertex>> edge_vertex;
        edge_vertex.push_back(vertex);
        edge->SetVertex(edge_vertex);

CurveFittingEdge他是继承自Edge。

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public Edge
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x, double y ): Edge(1,1, std::vector<std::string>{"abc"}) {
        x_ = x;
        y_ = y;
    }
    // 计算曲线模型误差
    virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        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;
    }
    /// 返回边的类型信息
    virtual std::string TypeInfo() const override { return "CurveFittingEdge"; }
public:
    double x_,y_;  // x 值， y 值为 _measurement
};

在这个里面主要实现残差的构建，雅克比矩阵的构建。对Edge中的一些函数进行重写。

Edge(int residual_dimension, int num_verticies,
           const std::vector<std::string> &verticies_types) {
    residual_.resize(residual_dimension, 1);
//    verticies_.resize(num_verticies);      // TODO:: 这里可能会存在问题，比如这里resize了3个空,后续调用edge->addVertex. 使得vertex前面会存在空元素
    if (!verticies_types.empty())
        verticies_types_ = verticies_types;
    jacobians_.resize(num_verticies);
    id_ = global_edge_id++;

    Eigen::MatrixXd information(residual_dimension, residual_dimension);
    information.setIdentity();
    information_ = information;

//    cout<<"Edge construct residual_dimension="<<residual_dimension
//            << ", num_verticies="<<num_verticies<<", id_="<<id_<<endl;
}

这里residual_dimension与num_verticies表达的是残差的维度与顶点的数量。residual_resize为1x1。急啊jacobians_resize为1，information是信息阵。
接下来就开始重写ComputeResidual，也就是残差。

virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_;  // 构建残差
    }

在verticies_中提取待估计的参数，因为这里的顶点中待优化的变量只有一个，因此只有verticies_[0]，然后构建残差，也就是带噪声项-原项。
残差构建完成后，开始计算jacobian，也是通过重写Edge中ComputeJacobians来实现。

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;
    }

先提取出待优化变量，定义一个3x1的jaco_abc来存储函数的一阶导，并将3x1的jaco_abc存储到jacobians_[0]中。
接下来就是返回边的类型：

virtual std::string TypeInfo() const override { return "CurveFittingEdge"; }
public:
double x_,y_;  // x 值， y 值为 _measurement

接下来就是指定边对应的顶点;

std::vector<std::shared_ptr<Vertex>> edge_vertex;
edge_vertex.push_back(vertex);//这里待优化变量只有一维
edge->SetVertex(edge_vertex);//设置边所对应的顶点
problem.AddEdge(edge);//添加边

————————————————————————————————————————————————————————-
bool SetVertex(const std::vector<std::shared_ptr<Vertex>> &vertices) {
    verticies_ = vertices;
    return true;}

bool Problem::AddEdge(shared_ptr<Edge> edge) {
    if (edges_.find(edge->Id()) == edges_.end()) {//判断是否有重复的边
        edges_.insert(pair<ulong, std::shared_ptr<Edge>>(edge->Id(), edge));
    } else {
        // LOG(WARNING) << "Edge " << edge->Id() << " has been added before!";
        return false;
    }

    for (auto &vertex: edge->Verticies()) //遍历顶点
//for(int i=0;i<verticcies.size();i++)
{
        vertexToEdge_.insert(pair<ulong, shared_ptr<Edge>>(vertex->Id(), edge));
    }
    return true;
}

__________________________________
std::vector<std::shared_ptr<Vertex>> Verticies() const {
        return verticies_;//返回多有顶点

下面开始求解Problem问题（用LM算法）：

 problem.Solve(30);//迭代30次

进入Solve函数

bool Problem::Solve(int iterations) {


    if (edges_.size() == 0 || verticies_.size() == 0) {
        std::cerr << "\nCannot solve problem without edges or verticies" << std::endl;
        return false;
    }

首先判断边或者顶点是否为空，如果为空就报错。

TicToc t_solve;
    // 统计优化变量的维数，为构建 H 矩阵做准备
    SetOrdering();
    // 遍历edge, 构建 H = J^T * J 矩阵
    MakeHessian();
    // LM 初始化
    ComputeLambdaInitLM();


————————————————————————————————————————————————————————
void Problem::SetOrdering() {

    // 每次重新计数
    ordering_poses_ = 0;//位姿统计
    ordering_generic_ = 0;//一般问题
    ordering_landmarks_ = 0;//路标统计

    // Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
    // 统计带估计的所有变量的总维度
    for (auto vertex: verticies_) {
        ordering_generic_ += vertex.second->LocalDimension();  // 所有的优化变量总维数，这里是3
    }
}

————————————————————————————————————————————————————————————————————————
void Problem::MakeHessian() {
    TicToc t_h;
    // 直接构造大的 H 矩阵
    ulong size = ordering_generic_;//size的大小统计出来的大小，这里也就是3
    MatXX H(MatXX::Zero(size, size));//构建了一个3x3的H阵，H=J^T*J
    VecX b(VecX::Zero(size));//构建向量b  

    // TODO:: accelate, accelate, accelate
//#ifdef USE_OPENMP
//#pragma omp parallel for
//#endif

    // 遍历每个残差，并计算他们的雅克比，得到最后的 H = J^T * J
    for (auto &edge: edges_) {

        edge.second->ComputeResidual();//计算残差
        
        edge.second->ComputeJacobians();//计算雅克比

        auto jacobians = edge.second->Jacobians();//获取计算出的jacobian
        auto verticies = edge.second->Verticies();//获取边中顶点
        assert(jacobians.size() == verticies.size());
        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];//待优化变量jacobian
            ulong index_i = v_i->OrderingId();//获取id
            ulong dim_i = v_i->LocalDimension();//获取参与优化变量的维度

            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.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;
    t_hessian_cost_ += t_h.toc();

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

}

LM的求解形式是（J^T*J + λI）Δx=-J^T*f.对于λ的初值，在vio课中有这几种策略。
（1）将 λ的大小设置为与J^T*J对角线元素上最大值同一个数量级。即：λ0​=τ⋅max{(JT×J)ii​}，τ一般取 [10^-8～1]。依据具体情况调整。

stopThresholdLM_ = 1e-6 * currentChi_;          // 迭代条件为 误差下降 1e-6 倍

    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;//λ
}

λ的更新策略
ρ=F(x)−F(x+Δx)​/L(0)-L(Δx)在这里L(0)-L(Δx)始终是大于0的，如果F(x)−F(x+Δx)d大于0，那么就说明迭代是成功的，可以减少λ，使其更快下降，如果是大于0的话。那么说明迭代后的数比原来的还要大，那么就增大λ，减少步长，并放弃本次迭代结果。

λ:=λmax{31​,1−(2ρ−1)3};ν:=2;(ρ>0)
λ:=λ∗ν;ν:=2∗ν;​(ρ≤0)​

bool stop = false;
    int iter = 0;//迭代次数
    while (!stop && (iter < iterations)) {//判断停止迭代
        std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_
                  << std::endl;
        bool oneStepSuccess = false;
        int false_cnt = 0;
        while (!oneStepSuccess)  // 迭代失败，不断尝试 Lambda, 直到成功迭代一步
        {
            // setLambda
            AddLambdatoHessianLM();
            /*×××××××××××××××××××××××××××××××××××××××××
            AddLambdatoHessianLM()函数
           void Problem::AddLambdatoHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) += currentLambda_;//在H阵上面加上λI
    }
}
            
            ×××××××××××××××××××××××××××××××××××××××××*/
            // 第四步，解线性方程 H X = B
            SolveLinearSystem();
            /*×××××××××××××××××××××××××××××××××××××××
            void Problem::SolveLinearSystem() {

        delta_x_ = Hessian_.inverse() * b_;
//        delta_x_ = H.ldlt().solve(b_);

}
            ×××××××××××××××××××××××××××××××××××××××××*/
            //
            RemoveLambdaHessianLM();
            /*××××××××××××××××××××××××××××××××××××××××××××××××
            void Problem::RemoveLambdaHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    // TODO:: 这里不应该减去一个，数值的反复加减容易造成数值精度出问题？而应该保存叠加lambda前的值，在这里直接赋值
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) -= currentLambda_;
    }
}
            ×××××××××××××××××××××××××××××××××××××××××××××××××*/

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

            // 更新状态量 X = X+ delta_x
            UpdateStates();
            // 判断当前步是否可行以及 LM 的 lambda 怎么更新
            oneStepSuccess = IsGoodStepInLM();
            /******************************************************8
            bool Problem::IsGoodStepInLM() {
    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);//L(0)-L(Δx)
    scale += 1e-3;    // make sure it's non-zero :)

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;//F（x+Δx）
    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值再去【重新计算】当前步
    }
}
            *********************************************************/
            
            // 后续处理，
            if (oneStepSuccess) {
                // 在新线性化点 构建 hessian
                MakeHessian();
                // TODO:: 这个判断条件可以丢掉，条件 b_max <= 1e-12 很难达到，这里的阈值条件不应该用绝对值，而是相对值
//                double b_max = 0.0;
//                for (int i = 0; i < b_.size(); ++i) {
//                    b_max = max(fabs(b_(i)), b_max);
//                }
//                // 优化退出条件2： 如果残差 b_max 已经很小了，那就退出
//                stop = (b_max <= 1e-12);
                false_cnt = 0;
            } else {
                false_cnt++;
                RollbackStates();   // 误差没下降，回滚
            }
        }
        iter++;

        // 优化退出条件3： currentChi_ 跟第一次的chi2相比，下降了 1e6 倍则退出
        if (sqrt(currentChi_) <= stopThresholdLM_)
            stop = true;
    }
    std::cout << "problem solve cost: " << t_solve.toc() << " ms" << std::endl;
    std::cout << "   makeHessian cost: " << t_hessian_cost_ << " ms" << std::endl;
    return true;
}

参考文章：

https://blog.csdn.net/CSDN_XCS/article/details/95938482?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522161438909116780265465842%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=161438909116780265465842&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduend~default-1-95938482.pc_search_result_before_js&utm_term=verticies_