视觉SLAM笔记（54） Ceres 操作后端优化

程序员文章站 2023-12-25 18:43:33

...

视觉SLAM笔记（54） Ceres 操作后端优化

1. Ceres 求解 BA
2. 求解

1. Ceres 求解 BA

g2o 用 Edges 来保存每一个代价函数，但 Ceres 却是用 Problem 类型来构建最终的目标函数
使用 AddResidualBlock 来添加代价函数，最后组成整个目标函数

为了简化整个过程，首先定义代价函数类型
并且定义 Create 成员来使用 Ceres 当中的 AutoDiff 特性

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include <iostream>
#include "ceres/ceres.h"

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

class SnavelyReprojectionError
{
public:
    SnavelyReprojectionError(double observation_x, double observation_y) :observed_x(observation_x), observed_y(observation_y) {}

    template<typename T>
    bool operator()(const T* const camera,
        const T* const point,
        T* residuals)const {
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    static ceres::CostFunction* Create(const double observed_x, const double observed_y) {
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
            new SnavelyReprojectionError(observed_x, observed_y)));
    }


private:
    double observed_x;
    double observed_y;
};

#endif // SnavelyReprojection.h

定义好之后，就可以使用 SnavelyReprojectionError::Create 函数来轻松地构建这个目标函数

void BuildProblem(BALProblem* bal_problem, Problem* problem, const BundleParams& params)
{
    const int point_block_size = bal_problem->point_block_size();
    const int camera_block_size = bal_problem->camera_block_size();

    // 构建points、cameras参数类型的优化变量
    double* points = bal_problem->mutable_points(); 
    double* cameras = bal_problem->mutable_cameras();

    // Observations is 2 * num_observations long array observations
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x 
    // and y position of the observation. 
    const double* observations = bal_problem->observations();

    // 有观测数量有多少就有多少误差项
    for(int i = 0; i < bal_problem->num_observations(); ++i){
        CostFunction* cost_function;

        // Each Residual block takes a point and a camera as input 
        // and outputs a 2 dimensional Residual
        // 在这里构建代价函数costfunction每个误差块是以一个points和一个cameras为输入的
        cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]);

        // 设置是否开启核函数
        LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL;

        // Each observatoin corresponds to a pair of a camera and a point 
        // which are identified by camera_index()[i] and point_index()[i]
        // respectively.
        double* camera = cameras + camera_block_size * bal_problem->camera_index()[i];
        double* point = points + point_block_size * bal_problem->point_index()[i];

     
        problem->AddResidualBlock(cost_function, loss_function, camera, point);
    }

}

为了使用 Schur 消元， Ceres 采取的策略和 g2o 有很大的不同
Ceres采用额外的类型 ParameterBlockOrdering 来管理 schur 消元顺序
并且使用 AddElementToGroup 来对变量进行编号从而定义消元顺序

例如下面设置点云变量为 0，相机变量为 1 就可以让点云变量先进行消元（优先消元编号最小的变量）：

void SetOrdering(BALProblem* bal_problem, ceres::Solver::Options* options, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int point_block_size = bal_problem->point_block_size();
    double* points = bal_problem->mutable_points();

    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    double* cameras = bal_problem->mutable_cameras();


    if (params.ordering == "automatic")
        return;

    ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering;

    // The points come before the cameras
    for (int i = 0; i < num_points; ++i)
        ordering->AddElementToGroup(points + point_block_size * i, 0);


    for (int i = 0; i < num_cameras; ++i)
        ordering->AddElementToGroup(cameras + camera_block_size * i, 1);

    options->linear_solver_ordering.reset(ordering);

}

2. 求解

Ceres 除了能够在原始数组上操作以外，另一大优势在于它的优化设置非常便捷
g2o的设置全靠变量类型来选择不同的下降策略，以及选择稠密或者稀疏的线性方程组解法
然而 ceres 全靠给 Solver::Options 的类型成员变量进行赋值来调整
这种方式比 g2o 快捷便利很多

Options 类型几乎集成了 Ceres 的所有求解方法设置和管理
包括上面提到的 Schur 消元顺序

void SetSolverOptionsFromFlags(BALProblem* bal_problem,
    const BundleParams& params, Solver::Options* options) {

    CHECK(ceres::StringToLinearSolverType(params.linear_solver, &options->linear_solver_type));
    CHECK(ceres::StringToSparseLinearAlgebraLibraryType(params.sparse_linear_algebra_library, &options->sparse_linear_algebra_library_type));
    CHECK(ceres::StringToDenseLinearAlgebraLibraryType(params.dense_linear_algebra_library, &options->dense_linear_algebra_library_type));
    // 设置线性滤波器
    options->max_num_iterations = params.num_iterations;
    options->minimizer_progress_to_stdout = true;
    options->num_threads = params.num_threads; // 用于计算的线程数目，可以加速 Jacobian 矩阵的计算
    // 下降策略的选取
    CHECK(StringToTrustRegionStrategyType(params.trust_region_strategy,
        &options->trust_region_strategy_type));
    // 设置变量排序
    SetOrdering(bal_problem, options, params);
}

Ceres 的求解也很简单，和视觉SLAM笔记（28） Ceres 的例子一样
只需要简单地设置一下关于梯度和相邻两次迭代之间目标函数之差的相关阈值
就可以 Solve 函数负责Ceres 的求解功能
只需要传给它对应的选项，目标函数即可
Summary 类型用来负责函数求解每一次迭代的统计结果

options.gradient_tolerance = 1e-16;
options.function_tolerance = 1e-16;
Solver::Summary summary;
Solve(options, &problem, &summary);

和视觉SLAM笔记（53） g2o 操作后端优化示例一样
该程序在运行后也可以在执行文件目录下找到 final.ply 文件和 initial.ply文件
视觉SLAM笔记（54） Ceres 操作后端优化
由于该程序和 g2o 程序中采用一致的数据， initial.ply 具有相同的图案
使用 MeshLab 软件打开 final.ply 可以看到下图所示的结果

通过对比也可以发现优化结果和g2o 完全一样

其中，高翔博士的源码，没有输出 final.ply 文件和 initial.ply文件
是因为缺少名称参数
需要对common/BundleParams.h文件进行修改

// arg.param("initial_ply", initial_ply,"","Export the BAL file data as a PLY file.");
// arg.param("final_ply", final_ply, "", "Export the refined BAL file data as a PLY");
arg.param("initial_ply", initial_ply,"initial.ply","Export the BAL file data as a PLY file.");
arg.param("final_ply", final_ply, "final.ply", "Export the refined BAL file data as a PLY");

Ceres 库公开的 API 说明详细，同时源代码可读性也
相比 g2o 这样公开文档太少的库，也更加推荐使用 Ceres 这样的库来做 SLAM

参考：

《视觉SLAM十四讲》