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

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

ρ 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

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

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

2.实例分析——HelloWorld

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

先看源码:

#include 
#include 

using namespace std;
using namespace ceres;
//第一部分:构建代价函数
struct CostFunctor {
    template 
    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(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)=10x,也就是说我们要寻找最优的 x x x值使 f ( x ) f(x) f(x)最小。所以我们的误差项为 10.0 − x [ 0 ] 10.0-x[0] 10.0x[0]

struct CostFunctor {
  template  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(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 
#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  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(
                        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=10x。但是在此处,我们待求解的参数是 m m m c c c,相应的残差项为 r e s = y − e m ∗ x − c res = y - e^{m*x-c} res=yemxc。相对来说,上一节的代价函数结构体中并没有真正意义上利用仿函数的优势,而此节中的代价函数结构体则充分体现了放函数的优势。

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

struct ExponentialResidual {
    ExponentialResidual(double x, double y)
            : x_(x), y_(y) {}
    //重定义()操作符
    template  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} min21iyiemxi+c2

因此我们需要用 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(
                        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/cqrtxwd/article/details/78956227

你可能感兴趣的:(SLAM,slam,ceres)