Ceres入门——Ceres的基本使用方法

绝地灬酷狼 2022-03-15 15:15 1670阅读 0赞

### Ceres入门——Ceres的基本使用方法 ###

*  1.使用流程
 *  2.实例分析——HelloWorld
 *   *  2.1 构建代价函数（cost function）
     *  2.2 构建寻优问题
     *  2.3 配置并运行求解器
     *  2.4 测试结果
 *  3.实例分析——非线性拟合
 *   *  3.1 构建代价函数
     *  3.2 构建寻优问题
     *  3.3 配置并运行求解器
     *  3.4 测试结果

Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。Ceres官网上的文档非常详细地介绍了其具体使用方法，相比于另外一个在slam中被广泛使用的图优化库G2O，ceres的文档可谓相当丰富详细。本篇博客介绍一下Ceres库的基本使用方法。

Ceres可以解决边界约束鲁棒非线性最小二乘法优化问题。这个概念可以用以下表达式表示：  
![在这里插入图片描述][20190304084149561.png_pic_center]  
这一表达式在工程和科学领域有非常广泛的应用。比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。

ρ i ( ∣ ∣ f i ( x i 1 , . . . , x i k ) ∣ ∣ 2 ) \\rho\_i(||f\_i(x\_\{i\_1\},...,x\_\{i\_k\}) ||^2) ρi(∣∣fi(xi1,...,xik)∣∣2)这一部分被成为残差块（ResidualBlock），其中的  f i ( ⋅ ) f\_i (\\cdot) fi(⋅) 叫做代价函数（CostFunction）。代价函数依赖于一系列参数 \[ x i 1 \] \[x\_\{i\_1\}\] \[xi1\] ，这一系列参数（均为标量）称为参数块（ParameterBlock）。当然参数块中也可以只含有一个变量。 l j l\_j lj和  u j u\_j uj分别是变量块 x j x\_j xj的上下边界。

ρ i \\rho\_i ρi是损失函数（LossFunction） 。按照损失函数的是一个标量函数，其作用是减少异常值（Outliers）对优化结果的影响。其效果类似于对函数的过滤。

# 1.使用流程 #

使用Ceres求解非线性优化问题，可以分为三个步骤：

**1. 构建代价函数（cost function）**  
代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符。关于仿函数的内容可以参考这篇博客：[c++仿函数 functor][c_ functor]

**2.通过代价函数构建待求解的优化问题**

**3.配置求解器参数并求解问题**  
这个步骤就是设置方程怎么求解、求解过程是否输出等

# 2.实例分析——HelloWorld #

以Ceres安装文件中example文件夹下的helloworld.cc示例程序来对Ceres库的基本使用过程进行分析。在Hello World这个例子中，待优化的函数是  f ( x ) = 10 − x f(x)=10−x f(x)=10−x 。

先看源码：

#include <iostream>
    #include <ceres/ceres.h>
    
    using namespace std;
    using namespace ceres;
    //第一部分：构建代价函数
    struct CostFunctor {
        template <typename T>
        bool operator()(const T* const x, T* residual) const {
            residual[0] = T(10.0) - x[0];
            return true;
        }
    };
    
    //主函数
    int main(int argc, char** argv) {
        google::InitGoogleLogging(argv[0]);
    
        // 寻优参数x的初始值，为5
        double initial_x = 5.0;
        double x = initial_x;
    
        // 第二部分：构建寻优问题
        Problem problem;
        CostFunction* cost_function =
                new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
        problem.AddResidualBlock(cost_function, NULL, &x); //向问题中添加误差项，本问题比较简单，添加一个就行。
    
        //第三部分： 配置并运行求解器
        Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
        options.minimizer_progress_to_stdout = true;//输出到cout
        Solver::Summary summary;//优化信息
        Solve(options, &problem, &summary);//求解!!!
    
        std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
        //最终结果
        std::cout << "x : " << initial_x
                  << " -> " << x << "\n";
        return 0;
    }

## 2.1 构建代价函数（cost function） ##

待优化函数为 f ( x ) = 10 − x f(x)=10−x f(x)=10−x，也就是说我们要寻找最优的 x x x值使  f ( x ) f(x) f(x)最小。所以我们的误差项为 10.0 − x \[ 0 \] 10.0-x\[0\] 10.0−x\[0\]。

struct CostFunctor {
      template <typename T> bool operator()(const T* const x, T* residual) const {
        residual[0] = 10.0 - x[0];//误差项
        return true;
      }
    };

## 2.2 构建寻优问题 ##

首先，定义Problem类型的变量，然后将构建的代价函数添加到寻优问题中。`AutoDiffCostFunction`将刚刚建立的`CostFunctor` 结构的一个实例作为输入，自动生成其微分并且赋予其一个`CostFunction` 类型的接口。

Problem problem;
      CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
        problem.AddResidualBlock(cost_function, NULL, &x); //向问题中添加误差项，本问题比较简单，添加一个就行。

## 2.3 配置并运行求解器 ##

为求解这个优化问题，我们需要做一些配置。这一部分很好理解，创建一个Option，配置一下求解器的配置，创建一个Summary。最后调用Solve方法，求解。

Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
        options.minimizer_progress_to_stdout = true;//输出到cout
        Solver::Summary summary;//优化信息
        Solve(options, &problem, &summary);//求解!!!

## 2.4 测试结果 ##

测试结果如下，经过3次迭代之后求出x的最优解为10.

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
       0  1.250000e+01    0.00e+00    5.00e+00   0.00e+00   0.00e+00  1.00e+04        0    1.91e-05    6.60e-05
       1  1.249750e-07    1.25e+01    5.00e-04   5.00e+00   1.00e+00  3.00e+04        1    3.00e-05    1.31e-04
       2  1.388518e-16    1.25e-07    1.67e-08   5.00e-04   1.00e+00  9.00e+04        1    7.87e-06    1.46e-04
    Ceres Solver Report: Iterations: 3, Initial cost: 1.250000e+01, Final cost: 1.388518e-16, Termination: CONVERGENCE
    x : 5 -> 10

# 3.实例分析——非线性拟合 #

以Ceres安装文件中另一个示例是使用Ceres来进行非线性拟合。它指定一系列的点对来拟合一个曲线的系数。这一系列点对是通过曲线 y = e 0.3 x + 0.1 y=e^\{0.3x+0.1\} y=e0.3x\+0.1插值的点然后添加了标准差的高斯噪声。我们要拟合的曲线形式为： y = e m x + c y = e^\{mx+c\} y=emx\+c

先看代码：

#include <iostream>
    #include "ceres/ceres.h"
    
    using ceres::AutoDiffCostFunction;
    using ceres::CostFunction;
    using ceres::Problem;
    using ceres::Solver;
    using ceres::Solve;
    
    
    const int kNumObservations = 67;
    //观测值
    const double data[] = {
            0.000000e+00, 1.133898e+00,
            7.500000e-02, 1.334902e+00,
            1.500000e-01, 1.213546e+00,
            2.250000e-01, 1.252016e+00,
            3.000000e-01, 1.392265e+00,
            3.750000e-01, 1.314458e+00,
            4.500000e-01, 1.472541e+00,
            5.250000e-01, 1.536218e+00,
            6.000000e-01, 1.355679e+00,
            6.750000e-01, 1.463566e+00,
            7.500000e-01, 1.490201e+00,
            8.250000e-01, 1.658699e+00,
            9.000000e-01, 1.067574e+00,
            9.750000e-01, 1.464629e+00,
            1.050000e+00, 1.402653e+00,
            1.125000e+00, 1.713141e+00,
            1.200000e+00, 1.527021e+00,
            1.275000e+00, 1.702632e+00,
            1.350000e+00, 1.423899e+00,
            1.425000e+00, 1.543078e+00,
            1.500000e+00, 1.664015e+00,
            1.575000e+00, 1.732484e+00,
            1.650000e+00, 1.543296e+00,
            1.725000e+00, 1.959523e+00,
            1.800000e+00, 1.685132e+00,
            1.875000e+00, 1.951791e+00,
            1.950000e+00, 2.095346e+00,
            2.025000e+00, 2.361460e+00,
            2.100000e+00, 2.169119e+00,
            2.175000e+00, 2.061745e+00,
            2.250000e+00, 2.178641e+00,
            2.325000e+00, 2.104346e+00,
            2.400000e+00, 2.584470e+00,
            2.475000e+00, 1.914158e+00,
            2.550000e+00, 2.368375e+00,
            2.625000e+00, 2.686125e+00,
            2.700000e+00, 2.712395e+00,
            2.775000e+00, 2.499511e+00,
            2.850000e+00, 2.558897e+00,
            2.925000e+00, 2.309154e+00,
            3.000000e+00, 2.869503e+00,
            3.075000e+00, 3.116645e+00,
            3.150000e+00, 3.094907e+00,
            3.225000e+00, 2.471759e+00,
            3.300000e+00, 3.017131e+00,
            3.375000e+00, 3.232381e+00,
            3.450000e+00, 2.944596e+00,
            3.525000e+00, 3.385343e+00,
            3.600000e+00, 3.199826e+00,
            3.675000e+00, 3.423039e+00,
            3.750000e+00, 3.621552e+00,
            3.825000e+00, 3.559255e+00,
            3.900000e+00, 3.530713e+00,
            3.975000e+00, 3.561766e+00,
            4.050000e+00, 3.544574e+00,
            4.125000e+00, 3.867945e+00,
            4.200000e+00, 4.049776e+00,
            4.275000e+00, 3.885601e+00,
            4.350000e+00, 4.110505e+00,
            4.425000e+00, 4.345320e+00,
            4.500000e+00, 4.161241e+00,
            4.575000e+00, 4.363407e+00,
            4.650000e+00, 4.161576e+00,
            4.725000e+00, 4.619728e+00,
            4.800000e+00, 4.737410e+00,
            4.875000e+00, 4.727863e+00,
            4.950000e+00, 4.669206e+00,
    };
    //1.代价函数结构体
    struct ExponentialResidual {
        ExponentialResidual(double x, double y)
                : x_(x), y_(y) {}
    
        template <typename T> bool operator()(const T* const m, const T* const c, T* residual) const {
            residual[0] = y_ - exp(m[0] * x_ + c[0]);
            return true;
        }
    
    private:
        const double x_;
        const double y_;
    };
    
    int main(int argc, char** argv) {
    
        double m = 0.0;
        double c = 0.0;
      
      //构建寻优问题
        Problem problem;
        for (int i = 0; i < kNumObservations; ++i) {
            problem.AddResidualBlock(
                    new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
                            new ExponentialResidual(data[2 * i], data[2 * i + 1])),
                    NULL,
                    &m, &c);
        }
       //配置并运行求解器
        Solver::Options options;
        options.max_num_iterations = 25;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;
    
        Solver::Summary summary;
        Solve(options, &problem, &summary);
        std::cout << summary.BriefReport() << "\n";
        std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
        std::cout << "Final   m: " << m << " c: " << c << "\n";
        return 0;
    }

## 3.1 构建代价函数 ##

这里的代价函数结构体和第二节中的代价函数结构体有些区别。在第二节中，我们待求的参数是 x x x，所以残差项是 y = 10 − x y=10-x y=10−x。但是在此处，我们待求解的参数是 m m m和 c c c，相应的残差项为 r e s = y − e m ∗ x − c res = y - e^\{m\*x-c\} res=y−em∗x−c。相对来说，上一节的代价函数结构体中并没有真正意义上利用仿函数的优势，而此节中的代价函数结构体则充分体现了放函数的优势。

ExponentialResidual结构体定义了两个私有成员变量 x\_ 和 y\_ ，并且在构造函数中将构造函数的参数赋值给这两个变量。这样就可以在重定义 () 操作符时，仅通过输入两个参数 m m m和 c c c就能计算残差。

struct ExponentialResidual {
        ExponentialResidual(double x, double y)
                : x_(x), y_(y) {}
        //重定义()操作符
        template <typename T> bool operator()(const T* const m, const T* const c, T* residual) const {
            residual[0] = y_ - exp(m[0] * x_ + c[0]); //残差项 res = y - exp(m[0]*x-c[0])
            return true;
        }
    
    private:
        const double x_;
        const double y_;
    };

## 3.2 构建寻优问题 ##

我们要拟合的曲线是 y = e m x + c y=e^\{mx+c\} y=emx\+c 此时我们有67组观测值。想要求得参数 m m m和 c c c的值，我们需要构建如下目标函数： m i n 1 2 ∑ i ∣ ∣ y i − e m x i + c ∣ ∣ 2 min\\frac\{1\}\{2\}\\sum\_i\{||y\_i-e^\{mx\_i+c\}||^2\} min21i∑∣∣yi−emxi\+c∣∣2

因此我们需要用  f o r for for 循环将残差项加入到我们定义的  p r o b l e m problem problem 当中。

Problem problem;
        for (int i = 0; i < kNumObservations; ++i) {
            problem.AddResidualBlock(
                    new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
                            new ExponentialResidual(data[2 * i], data[2 * i + 1])),
                    NULL,
                    &m, &c);
        }

## 3.3 配置并运行求解器 ##

在这部分中与第二节不同的是我们设定了最大迭代次数，其他无异。

//配置并运行求解器
        Solver::Options options;
        options.max_num_iterations = 25;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;
    
        Solver::Summary summary;
        Solve(options, &problem, &summary);

## 3.4 测试结果 ##

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
       0  1.211734e+02    0.00e+00    3.61e+02   0.00e+00   0.00e+00  1.00e+04        0    2.91e-04    3.44e-04
       1  2.334822e+03   -2.21e+03    0.00e+00   7.52e-01  -1.87e+01  5.00e+03        1    3.31e-05    4.15e-04
       2  2.331438e+03   -2.21e+03    0.00e+00   7.51e-01  -1.86e+01  1.25e+03        1    1.10e-05    4.34e-04
       3  2.311313e+03   -2.19e+03    0.00e+00   7.48e-01  -1.85e+01  1.56e+02        1    1.00e-05    4.49e-04
       4  2.137268e+03   -2.02e+03    0.00e+00   7.22e-01  -1.70e+01  9.77e+00        1    9.06e-06    4.62e-04
       5  8.553131e+02   -7.34e+02    0.00e+00   5.78e-01  -6.32e+00  3.05e-01        1    1.41e-05    4.80e-04
       6  3.306595e+01    8.81e+01    4.10e+02   3.18e-01   1.37e+00  9.16e-01        1    2.83e-04    7.67e-04
       7  6.426770e+00    2.66e+01    1.81e+02   1.29e-01   1.10e+00  2.75e+00        1    2.93e-04    1.07e-03
       8  3.344546e+00    3.08e+00    5.51e+01   3.05e-02   1.03e+00  8.24e+00        1    2.82e-04    1.36e-03
       9  1.987485e+00    1.36e+00    2.33e+01   8.87e-02   9.94e-01  2.47e+01        1    2.81e-04    1.64e-03
      10  1.211585e+00    7.76e-01    8.22e+00   1.05e-01   9.89e-01  7.42e+01        1    2.81e-04    1.93e-03
      11  1.063265e+00    1.48e-01    1.44e+00   6.06e-02   9.97e-01  2.22e+02        1    2.82e-04    2.22e-03
      12  1.056795e+00    6.47e-03    1.18e-01   1.47e-02   1.00e+00  6.67e+02        1    2.80e-04    2.50e-03
      13  1.056751e+00    4.39e-05    3.79e-03   1.28e-03   1.00e+00  2.00e+03        1    2.83e-04    2.79e-03
    Ceres Solver Report: Iterations: 14, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: CONVERGENCE
    Initial m: 0 c: 0
    Final   m: 0.291861 c: 0.131439

可以看出，经过14次迭代之后，计算出 m = 0.291861 m=0.291861 m=0.291861， c = 0.131439 c = 0.131439 c=0.131439，与真实值差异不大。

参考博客：  
[https://blog.csdn.net/wzheng92/article/details/79634069][https_blog.csdn.net_wzheng92_article_details_79634069]  
[https://blog.csdn.net/cqrtxwd/article/details/78956227][https_blog.csdn.net_cqrtxwd_article_details_78956227]

[20190304084149561.png_pic_center]: /images/20220315/c06a1178dc614354a6d74194e5432f14.png
[c_ functor]: https://www.cnblogs.com/decade-dnbc66/p/5347088.html
[https_blog.csdn.net_wzheng92_article_details_79634069]: https://blog.csdn.net/wzheng92/article/details/79634069
[https_blog.csdn.net_cqrtxwd_article_details_78956227]: https://blog.csdn.net/cqrtxwd/article/details/78956227