Ceres 库：基础使用，以手写高斯-牛顿法为例

最新推荐文章于 2024-04-25 16:59:43 发布

Jason.Li_0012

最新推荐文章于 2024-04-25 16:59:43 发布

阅读量2.3k

点赞数 3

分类专栏：《视觉SLAM十四讲》笔记 C++\CMake 文章标签： c++

本文链接：https://blog.csdn.net/weixin_45929038/article/details/122990329

版权

《视觉SLAM十四讲》笔记同时被 2 个专栏收录

21 篇文章 19 订阅

订阅专栏

C++\CMake

13 篇文章 4 订阅

订阅专栏

Ceres 库

简介

Ceres库为Google开发的开源C++非线性优化库，被广泛使用于求解最小二乘问题。

Ceres库的Github主页如下：

安装

首先，下载Cere的源码：

git clone https://ceres-solver.googlesource.com/ceres-solver

其次，需要安装依赖项：

sudo apt install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev

随后，进入源码目录下进行编译安装：

cd ceres-solver
mkdir build && cd build
cmake ..
make -j12
sudo make install

默认安装文件位置：\usr\local\include\ceres和\usr\local\lib

工程配置

导入库文件，工程根目录下CMakeLists.txt填写如下：

# Ceres-solver
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

src下CMakeLists.txt文件填写如下内容：

# link ceres-solver
target_link_libraries(Study ${CERES_LIBRARIES})

include下头文件main.h填写如下内容：

//  Ceres-solver
#include <ceres/ceres.h>

问题描述

Cere库中，将最小二乘问题建模为如下形式（带边界的核函数最小二乘）：
$\min_x\frac{1}{2}\sum_i\rho_i\Bigl(\begin{Vmatrix}f_i(x_{i_1},x_{i_2},\cdots,x_{i_k})\end{Vmatrix}^2\Bigr)\\ s.t.\quad l_j\leq x_j \leq u_j$
在上述表示的问题中， $x_1,\cdots,x_n$ 被称为优化变量，也叫参数块（Parameter blocks）； $f_i(\cdot)$ 称为代价函数（Cost function）； $\rho_i(\cdot)$ 称为损失函数（Loss function），也称为核函数； $\rho_i\Bigl(\begin{Vmatrix}f_i(x_{i_1},x_{i_2},\cdots,x_{i_k})\end{Vmatrix}^2\Bigr)$ 被称为残差块（Residual block）；参数 $l_j$ 和 $u_j$ 被称为参数下界和上界。

当取损失函数为一恒定值时： $\rho_i(x)=x$ ，此时确立参数上下界限为 $l_j=-\infin,u_j=\infin$ ，也即构建为无界限约束的最小二乘问题。此时，代价函数等同于损失函数，也即残差块。

Ceres库API

采用Ceres库求解最小二乘问题，主要由三部分组成：代价函数的构建、最小二乘问题的构建以及最小二乘问题的求解。

为方便学习Ceres库，此处依旧采用手写高斯-牛顿法为例子，进行展示。问题描述如下：https://blog.csdn.net/weixin_45929038/article/details/122973583

同样，提取点作为观测数据集：

int main()
{
    /*--------  初始参数配置  --------*/
    //  实际曲线参数
    double ar = 1.0, br = 1.0, cr = 1.0;
    //  估计曲线参数初始值
    double ae = 2.0, be = 1.0, ce = 5.0;
    //  采样观测数据点个数
    int N = 100;
    //  噪声标准差及其倒数
    double w_sigma = 1.0;
    //  随机数生成器
    cv::RNG rng;

    /*--------  观测数据生成  --------*/
    vector<double> x_data, y_data;
    for(int i=0; i<N; i++){
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x +br * x + cr) + rng.gaussian(w_sigma*w_sigma));
    }
    //  参数块
    double abc[3] = {ae, be, ce};

    return 0;
}

构建代价函数

代价函数使用C++的仿函数进行构建，其本质为结构体struct或者类class，并对()进行重载，使其具备同函数相同的调用方式，故而称为仿函数。此处使用struct进行定义，主要包含构造函数、重载操作符()两部分：

例如，构建求解曲线参数的代价函数如下：

/*--------  构建CostFunction  --------*/
struct CURVE_FITTING_COST{
    //  构造函数
    CURVE_FITTING_COST(double x, double y): _x(x), _y(y){}

    //  重载操作符()
    template<typename T>
    bool operator()(const T *const abc, T *residual) const
    {
        //  y - exp(ax^2 + bx + c)
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
        return true;
    }

    //  存储值的成员变量
    const double _x, _y;
};

构造函数

构造函数用于向代价函数中传入已知参数的值（传感器的观测数据），当被优化的问题不存在已知参数时，无需写构造函数。

本例中，将100个曲线上的点作为作为输入。

重载操作符()

操作符()为一个模板方法，其返回值为bool类型。所接受的参数为参数块以及残差块。

输入

参数的传入应使用指针一次性传入变量的数组，例如对上述问题，被优化的参数包含 $a 、 b 、 c$ 三个参数，故而传输参数块为*const abc，对应为三个参数的数组。

上述问题为无约束最小二乘问题，故而残差块等同于代价函数 $y_i-exp(ax_i^2+bx_i+c)$ 。

模板类型T

由于在不同开源算法库中，具有自己定义的数据类型（Eigen中的Vector对应Opencv中的Point，Matrix对应Mat），故而操作符的输入、输出统一为模板类型T。

const关键字

在求解最小二乘过程中，不希望因异常操作修改了重载操作符()的内容以及参数块的内容，故而用const进行修饰。

构建最小二乘问题

最小二乘问题的构建主要采用Ceres::Problem类进行：

//  实例化
ceres::Problem problem;

实例化该类后，通过调用对应的成员函数进行构建最小二乘问题。

残差块

使用如下函数传递残差：

problem.AddResidualBlock(CostFunction* cost_function,
                         LossFunction* loss_function,
                         const vector<double *> parameter_blocks);

cost_function：代价函数
loss_function：损失函数，若构建的问题为无约束的最小二乘问题，则传入参数为nullptr
parameter_blocks：参数块，也即最终求解的参数

对于代价函数做出如下说明：

代价函数的求导模型

Ceres提供了三种模板类，用于实现不同精度、效率的求导：

AutoDiffCostFunction：自动求导类，由Ceres库自行决定求导的方式，为最长用的类。
NumericDiffCostFunction：数值导数类，由用户手动编写导数数值求解形式，通常在无法直接调用自动求导时使用，其精度以及效率低于自动求导
Analytic Derivatives：解析导数类，当导数存在闭合解析形式时使用，需要自行管理残差以及雅克比矩阵。

Ceres官方推荐使用自动求导方式进行：

ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);

CostFunctor：即为第一步构建的代价函数
residualDim：残差块纬度，也即模型的输出纬度
paramDim：参数块纬度，也即模型的输入纬度
functor：指针，为代价函数的构造

例如，对手写高斯-牛顿法中的问题，可进行如下构建：

ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i]))

其中，残差纬度为1,也即问题中的误差 $e_i$ ；参数纬度为3，也即曲线参数的 $a 、 b 、 c$ ；传入参数用于构建每一次的代价函数。

则对于该问题，构建残差项的配置如下：

for(int i = 0; i < 100; i++){
    //  残差项配置
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
                             nullptr,
                             abc);
}

此处，未用到损失函数（无约束）故而传入nullptr；参数块为三维矩阵abc

其他

可查看参考博文：Ceres详解（一） Problem类

其中关于Problem::AddParameterBlock()以及其他成员函数的部分内容

求解最小二乘问题

使用ceres::Solve进行求解，其函数原型如下：

void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary)

options：求解器的配置，求解的配置选项
problem：求解的问题，也即我们构建的最小二乘问题
summary：求解的优化信息，用于存储求解过程中的优化信息

对求解器的配置做如下说明：

Solver::Options

minimizer_type：迭代的求解方式，可选如下：
- TRUST_REGION：信赖域方式，默认值
- LINEAR_SEARCH：线性搜索方法
- 参数通常保持默认值即可
trust_region_strategy_type：信赖域策略，可选如下：
- LEVENBERG_MARQUARDT，列文伯格-马夸尔特方法，默认值
- DOGLEG：Dog-leg法，俗称狗腿法
linear_solver_type：求解线性方程组的方式
- DENSE_QR：QR分解法，默认值，用于小规模最小二乘求解
- DENSE_NORMAL_CHOLESKY和 SPARSE_NORMAL_CHOLESKY：CHolesky分解，用于有稀疏性的大规模非线性最小二乘求解
- CGNR：共轭梯度法求解稀疏方程
- DENSE_SCHUR和 SPARSE_SCHUR：SCHUR分解，用于BA问题求解
- ITERATIVE_SCHUR：共轭梯度SCHUR，用于求解BA问题
num_threads：求解使用的线程数
minimizer_progress_to_stdout：是否将优化信息输出至终端
- bool类型，默认为false。若设置为true输出根据迭代方法而输出不同：
- 信赖域方法
  - cost：当前目标函数的数值
  - cost_change：当前参数变化量引起的目标函数变化
  - |gradient|：当前梯度的模长
  - |step|：参数变化量
  - tr_ratio：目标函数实际变化量和信赖域中目标函数变化量的比值
  - tr_radius：信赖域半径
  - ls_iter：线性求解器的迭代次数
  - iter_time：当前迭代耗时
  - total_time：优化总耗时
- 线性搜索方法
  - f：当前目标函数的数值
  - d：当前参数变化量引起的目标函数变化
  - g：当前梯度的模长
  - h：参数变化量
  - s：线性搜索最优步长
  - it：当前迭代耗时
  - tt：优化总耗时

在手写高斯-牛顿法问题中，进行如下配置：

//  Options
ceres::Solver::Options options;
//  cholesky分解
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
//  线程数
options.num_threads = 4;
//  输出优化信息
options.minimizer_progress_to_stdout = true;

Solver::Summary

Solver::Summary为求解器以及各个变量的信息，常用成员函数如下：

BriefReport()：输出单行的简单总结；
FullReport()：输出多行的完整总结。

Ceres库：手写高斯-牛顿法

源代码

代码内容如下：

#include "main.h"
#include <chrono>

/*--------  构建CostFunction  --------*/
struct CURVE_FITTING_COST{
    //  构造函数
    CURVE_FITTING_COST(double x, double y): _x(x), _y(y){}

    //  重载操作符()
    template<typename T>
    bool operator()(const T *const abc, T *residual) const
    {
        //  y - exp(ax^2 + bx + c)
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
        return true;
    }

    //  存储值的成员变量
    const double _x, _y;
};

int main()
{
    /*--------  初始参数配置  --------*/
    //  实际曲线参数
    double ar = 1.0, br = 1.0, cr = 1.0;
    //  估计曲线参数初始值
    double ae = 2.0, be = 1.0, ce = 5.0;
    //  采样观测数据点个数
    int N = 100;
    //  噪声标准差及其倒数
    double w_sigma = 1.0;
    //  随机数生成器
    cv::RNG rng;

    /*--------  观测数据生成  --------*/
    vector<double> x_data, y_data;
    for(int i = 0; i < N; i++){
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x +br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }
    //  参数块
    double abc[3] = {ae, be, ce};

    /*--------  构建最小二乘问题  --------*/
    //  实例化
    ceres::Problem problem;
    for(int i = 0; i < N; i++){
        //  残差项配置
        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
                nullptr,
                abc);
    }

    /*--------  求解最小二乘问题  --------*/
    //  Options
    ceres::Solver::Options options;
    //  cholesky分解
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    //  线程数
    options.num_threads = 4;
    //  输出优化信息
    options.minimizer_progress_to_stdout = true;

    //  summary
    ceres::Solver::Summary summary;
    //  求解开始计时t1
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //  求解
    ceres::Solve(options, &problem, &summary);
    //  求解结束计时t2
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    //  求解总用时
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    /*--------  结果输出  --------*/
    cout << summary.BriefReport() << endl;
    cout << "estimated a,b,c = ";
    for(auto a:abc) cout << a << " ";
    cout << endl;

    return 0;
}

输出

输出内容如下：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.631766e+07    0.00e+00    9.35e+07   0.00e+00   0.00e+00  1.00e+04        0    1.30e-03    1.76e-03
   1  6.190334e+06    4.01e+07    1.27e+07   9.68e-01   8.66e-01  1.65e+04        1    1.29e-03    3.80e-03
   2  8.093942e+05    5.38e+06    1.73e+06   9.43e-01   8.69e-01  2.76e+04        1    1.03e-03    4.89e-03
   3  9.975023e+04    7.10e+05    2.38e+05   8.72e-01   8.77e-01  4.83e+04        1    1.19e-03    6.12e-03
   4  1.050054e+04    8.92e+04    3.31e+04   7.14e-01   8.95e-01  9.53e+04        1    1.34e-03    7.51e-03
   5  7.599604e+02    9.74e+03    4.54e+03   5.14e-01   9.32e-01  2.69e+05        1    1.13e-03    8.70e-03
   6  6.636318e+01    6.94e+02    4.62e+02   5.17e-01   9.78e-01  8.07e+05        1    1.20e-03    9.95e-03
   7  5.102078e+01    1.53e+01    1.40e+01   2.57e-01   9.98e-01  2.42e+06        1    1.13e-03    1.11e-02
   8  5.100209e+01    1.87e-02    1.87e-02   1.44e-02   9.99e-01  7.26e+06        1    9.99e-04    1.22e-02
solve time cost = 0.0123565 seconds. 
Ceres Solver Report: Iterations: 9, Initial cost: 4.631766e+07, Final cost: 5.100209e+01, Termination: CONVERGENCE
estimated a,b,c = 0.877573 1.21245 0.931249