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

程序员文章站 2023-12-25 19:09:33

...

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

1. BA 数据集
2. g2o 求解 BA
3. 求解

1. BA 数据集

目录下的 common 文件夹是实验的数据集部分
它的布局如图所示
视觉SLAM笔记（53） g2o 操作后端优化
其中， 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 时，求解的设置无非在于这几种：

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

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

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

视觉SLAM笔记（53） g2o 操作后端优化
接下来，安装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

优化前后的效果如下图：
视觉SLAM笔记（53） g2o 操作后端优化

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

参考：

《视觉SLAM十四讲》

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