视觉SLAM笔记（53） g2o 操作后端优化

---

1. BA 数据集

目录下的 common 文件夹是实验的数据集部分
它的布局如图所示

其中， flags 文件夹下的两个文件定义了 CommandArgs 这个类
该类是用来解析用户输入的参数，同时也对程序需要的参数提供默认值以及文档说明

BundleParams 这个类定义了 Bundle Adjustment 使用的所有参数，也调用了 CommandArgs 类型的变量
由于 CommandArgs 这个类的存在，可以直接对程序后面使用 -help 来查看程序
所有的参数含义，使用的方式可以参考该程序中 BundleParams 类型的写法

tools 是一些数学工具函数
相机和路标参数和 视觉SLAM笔记（52） BA 与图优化 提到的代价函数，即保持一致
不过在实验中要把相机内参也当作优化变量考虑进去

用 f, k₁, k₂ 来表示焦距和畸变，三个参数表示平移，三个参数表示旋转（于是相机一共 9 个未知量），路标也使用三维坐标来表示

projection.h 是 投影计算流程图 的代码实现

由于 g2o 和 后续的ceres 都会用到这个投影模型，这个文件也将被两个工程所共享

---

2. g2o 求解 BA

和以前一样， g2o 使用图模型来描述问题的结构
所以要用节点来表示相机和路标，然后用边来表示它们之间的观测

仍然使用自定义的点和边，只需覆盖一些关键函数即可
针对相机和路标，可以定义如下继承，使用 override 关键字来表示对基类虚函数的覆盖：

#include <Eigen/Core>
#include "ceres/autodiff.h"

#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"

#include "tools/rotation.h"
#include "common/projection.h"

class VertexCameraBAL : public g2o::BaseVertex<9, Eigen::VectorXd>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexCameraBAL() {}

    virtual bool read(std::istream& /*is*/)
    {
        return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
        return false;
    }

    virtual void setToOriginImpl() {}

    virtual void oplusImpl(const double* update) override
    {
        Eigen::VectorXd::ConstMapType v(update, VertexCameraBAL::Dimension);
        _estimate += v;
    }

};


class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL() {}

    virtual bool read(std::istream& /*is*/)
    {
        return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
        return false;
    }

    virtual void setToOriginImpl() {}

    virtual void oplusImpl(const double* update) override
    {
        Eigen::Vector3d::ConstMapType v(update);
        _estimate += v;
    }
};

定义了节点以后，也还需要定义边来表示节点之间的联系
每一条边都对应了一个代价函数
同时为了避免复杂的求导运算，借助 ceres 库当中的 Autodiff(即自动求导) 功能
该功能需要类型将需要求导的公式实现在括号运算符 operator() 里
所以代码写成如下的样子：

class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexCameraBAL, VertexPointBAL>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL() {}

    virtual bool read(std::istream& /*is*/)
    {
        return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
        return false;
    }

    virtual void computeError() override   // 覆盖基类函数，使用operator()计算代价函数
    {
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> (vertex(0));
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> (vertex(1));

        (*this) (cam->estimate().data(), point->estimate().data(), _error.data());

    }

    // 为了使用 Ceres 求导功能而定义的函数，让本类成为拟函数类
    template<typename T>
    bool operator() (const T* camera, const T* point, T* residuals) const
    {
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        residuals[0] = predictions[0] - T(measurement() (0));
        residuals[1] = predictions[1] - T(measurement() (1));

        return true;
    }


    virtual void linearizeOplus() override
    {
        // 使用数字雅可比矩阵
        // BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
        // return;

        // using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians

        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> (vertex(0));
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> (vertex(1));
        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;

        Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
        Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
        double *parameters[] = { const_cast<double*> (cam->estimate().data()), const_cast<double*> (point->estimate().data()) };
        double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
        double value[Dimension];
        // Ceres 中的自动求导函数用法，需要提供 operator() 函数成员
        bool diffState = BalAutoDiff::Differentiate(*this, parameters, Dimension, value, jacobians);

        // copy over the Jacobians (convert row-major -> column-major)
        if (diffState)
        {
            _jacobianOplusXi = dError_dCamera;
            _jacobianOplusXj = dError_dPoint;
        }
        else
        {
            assert(0 && "Error while differentiating");
            _jacobianOplusXi.setZero();
            _jacobianOplusXi.setZero();
        }
    }
};

以上定义了本问题中使用的节点和边
下面就需要根据 BALProblem 类当中的实际数据来生成一些节点和边，交给 g2o 进行优化
值得注意的是，为了充分利用 BA 中的稀疏性，需要在这里将路标中的 setMarginalized 属性设置为 true
下面是代码的主要片段：

void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    // 添加相机和对应的初始值
    const double* raw_cameras = bal_problem->cameras();
    for (int i = 0; i < num_cameras; ++i)
    {
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i, camera_block_size);
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);   // 初始值
        pCamera->setId(i);                    // 为每个相机顶点设置id
        optimizer->addVertex(pCamera);        // 添加顶点优化

    }

    // 在数据集中设置具有初始值的顶点
    const double* raw_points = bal_problem->points();
    // const int point_block_size = bal_problem->point_block_size();
    for (int j = 0; j < num_points; ++j)
    {
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);   // 点云的初始值
        pPoint->setId(j + num_cameras);     // 为了不和相机冲突，加上相机的 id

        // 设置该点在解方程时进行 Schur 消元
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    // 为图添加边
    const int  num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();   // 第一次观察的指针

    for (int i = 0; i < num_observations; ++i)
    {
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        const int camera_id = bal_problem->camera_index()[i]; // 使用前面的 id 获取相机
        const int point_id = bal_problem->point_index()[i] + num_cameras; // 使用前面的 id 获得点
        // Huber loss 函数
        if (params.robustify)
        {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }
        // 设置边所对应的两个节点
        bal_edge->setVertex(0, dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1, dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
        // 设置其协方差矩阵(在此处为单位矩阵)
        bal_edge->setInformation(Eigen::Matrix2d::Identity());
        // 设置边所对应的观测值
        bal_edge->setMeasurement(Eigen::Vector2d(observations[2 * i + 0], observations[2 * i + 1]));

        optimizer->addEdge(bal_edge);
    }

}

---

3. 求解

在使用 g2o 时，求解的设置无非在于这几种：

1. 使用何种方法（LM, DogLeg）等来定义非线性优化的下降策略
2. 使用哪类线性求解器

注意到这里需要使用到稀疏性，所以必须选用稀疏的求解器

void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
    std::unique_ptr<BalBlockSolver> solver_ptr;
    std::unique_ptr<BalBlockSolver::LinearSolverType> linearSolver;

    // 使用稠密计算方法
    if (params.linear_solver == "dense_schur") {
        linearSolver = g2o::make_unique<BalDenseSolver>();
    }
    // 使用稀疏计算方法
    else if (params.linear_solver == "sparse_schur") {
        auto cholesky = g2o::make_unique<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>>();
        cholesky->setBlockOrdering(true); // 让 solver 对矩阵进行排序保持稀疏性
        linearSolver = std::move(cholesky);
    }

    // solver_ptr = new BalBlockSolver(linearSolver);
    solver_ptr = g2o::make_unique<BalBlockSolver>(std::move(linearSolver));

    g2o::OptimizationAlgorithmWithHessian* solver;
    if (params.trust_region_strategy == "levenberg_marquardt") {
        // solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr)); // 使用 LM 下降法
    }
    else if (params.trust_region_strategy == "dogleg") {
        // solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);  
        solver = new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr)); // 使用 DogLeg 下降法
    }
    else
    {
        std::cout << "Please check your trust_region_strategy parameter again.." << std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
}

现在问题基本上搭建好了
通过 BuildProblem 函数完成了对目标函数的构造，也通过 SetSolverOptionsFromFlags 函数通过用户的输入参数来设置优化求解
剩下需要做的就是思考该如何求解这个问题了
有了 g2o 这样的优化库，求解基本上可以一步完成，这点跟前面介绍的 g2o 用法几乎是一样的：

optimizer.initializeOptimization();
optimizer.setVerbose(true);
optimizer.optimize(params.num_iterations);

执行优化：

$ ./g2o_customBundle -input ../data/problem-16-22106-pre.txt 

接下来，安装meshlab并用它把这两个文件打开：

$ sudo add-apt-repository ppa:zarquon42/meshlab  
$ sudo apt-get update  
$ sudo apt-get install meshlab  

使用 Meshlab 分别打开 g2o 执行文件夹下的 initial.ply 和 final.ply 两个文件

$ meshlab initial.ply
$ meshlab final.ply

优化前后的效果如下图：

g2o 在做 Bundle Adjustment 的优化时必须要将其所有点云全部 Schur 掉，否则会出错！
其原因在于使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType> 这个类型来指定 linearsolver
其中模板参数当中的 PoseMatrixType 在程序中为相机姿态参数的维度
那么 Bundle Adjustment 当中 Schur 消元后解的线性方程组必须时只含有相机姿态变量

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（52） BA 与图优化
视觉SLAM笔记（51） 非线性系统和 EKF
视觉SLAM笔记（50） 线性系统和 KF
视觉SLAM笔记（49） 后端状态估计
视觉SLAM笔记（48） 局部地图

---

谢谢！