优化求解 | 非线性最小二乘优化器Ceres安装教程与应用案例

目录

• [0 专栏介绍](#0 专栏介绍)
• [1 `Ceres`库介绍](#1 Ceres库介绍)
• [2 `Ceres`库安装](#2 Ceres库安装)
• [3 `Ceres`库概念](#3 Ceres库概念)
• • [3.1 构建最小二乘问题](#3.1 构建最小二乘问题)
  • • [3.1.1 残差块](#3.1.1 残差块)
    • [3.1.2 代价函数](#3.1.2 代价函数)
  • [3.2 求解最小二乘问题](#3.2 求解最小二乘问题)
• [4 `Ceres`库案例](#4 Ceres库案例)
• • [4.1 Powell函数优化](#4.1 Powell函数优化)
  • [4.2 非线性曲线拟合](#4.2 非线性曲线拟合)

0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

---

1 Ceres库介绍

Ceres是一个开源的 C++ 库，用于求解大规模非线性最小二乘问题。它非常适用于优化问题，尤其是涉及到参数估计、图像处理、机器学习、机器人学、计算机视觉和计算几何等领域的问题。Ceres的设计强调高效性、灵活性和易用性，支持高效求解带有复杂约束的最优化问题，具有很强的扩展性。

非线性最小二乘应用于图像配准

非线性最小二乘应用于位姿估计

为了更好地使用Ceres，首先得了解什么是最小二乘问题

最小二乘问题表述如下

min ⁡ x F ( x ) = 1 2 min ⁡ x ∑ i ρ i ( ∥ f i ( x ) ∥ 2 2 ) s . t . l ≤ x ≤ u \min _{\boldsymbol{x}}F\left( \boldsymbol{x} \right) =\frac{1}{2}\min {\boldsymbol{x }}\sum{i}{\rho_i( \left\| f_i\left( \boldsymbol{x} \right) \right\| _{2}^{2})} \\ s.t.\ \ \ \ \boldsymbol{l} \le \boldsymbol{x} \le \boldsymbol{u} xminF(x)=21xmini∑ρi(∥fi(x)∥22)s.t. l≤x≤u

其中 ρ i ( ∥ f i ( x ) ∥ 2 2 ) \rho_i( \left\| f_i\left( \boldsymbol{x} \right) \right\| _{2}^{2}) ρi(∥fi(x)∥22)称为残差块(residual block)，一个优化问题通常由多个残差块组成，每个残差块负责计算一个或多个残差； f i ( ⋅ ) f_i\left( \cdot \right) fi(⋅)是代价函数， ρ i ( ⋅ ) \rho_i\left( \cdot \right) ρi(⋅)是损失函数； x \boldsymbol{x} x是优化变量或称为参数块(parameter blocks)。

Ceres建模了上述有界非线性最小二乘问题，通过更新优化变量，使残差达到最小，此时的优化变量就是该问题的数值最优解

2 Ceres库安装

Ceres库的安装需要很多依赖项，比较繁琐。为了方便起见，本文采用Conan在不影响系统文件的情况下一键安装

Conan是一个开源的C/C++包管理器，类似于Python中的第三方管理Anaconda，开发人员可以通过Conan轻松地下载、构建、安装和管理各种C/C++库，并将它们集成到他们的项目中，加快构建过程，提高项目构建的效率。具体请参考Conan安装与C++第三方环境配置保姆级图文教程(附速查字典)

首先新建3rd目录，编写conanfile.py文件

from conan import ConanFile
from conan.tools.cmake import cmake_layout

class ExampleRecipe(ConanFile):
    settings = "os", "compiler", "build_type", "arch"
    generators = "CMakeDeps", "CMakeToolchain", "cmake_find_package", "cmake"
    
    def configure(self):
        self.options["ceres-solver"].shared = True
        self.options["ceres-solver"].use_glog = True
        self.options["glog"].shared = True
        self.options["glog"].with_threads = True
        self.options["gflags"].shared = True

    def requirements(self):
        self.requires("ceres-solver/1.14.0")

    def layout(self):
        cmake_layout(self)

接着只要在该目录启动终端运行

conan install . --output-folder=build --build=missing -s compiler.libcxx=libstdc++11

Conan会自动从云端拉取Ceres库所需要的依赖，并自动在本地编译

编译好的文件在~/.conan/data目录

3 Ceres库概念

Ceres的求解过程包括构建最小二乘和求解最小二乘问题两部分

3.1 构建最小二乘问题

3.1.1 残差块

Ceres库通过向Ceres::Problem添加残差块构建最小二乘问题，因此其中最核心的函数为Problem::AddResidualBlock( )

ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
										  LossFunction *loss_function, 
										  const vector<double *> parameter_blocks)
										  
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
										  LossFunction *loss_function,
										  double *x0, double *x1, ...)

• 代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式，传入的参数模块需要和代价函数模块中定义的维数一致，维度不一致时程序会强制退出
• 损失函数 ：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等；该参数可以取nullptr，此时损失函数为单位函数。
• 参数模块 ：待优化的参数，可一次性传入所有参数的指针容器vector<double*>或依次传入所有参数的指针double*。

3.1.2 代价函数

与其他非线性优化工具包一样，Ceres库的性能很大程度上依赖于导数计算的精度和效率。这部分工作在Ceres库中用CostFunction类封装，较为常用的包括以下三种：

• 自动导数(AutoDiffCostFunction)：由Ceres库自行决定导数的计算方式，最常用的求导方式，其中的计算原理类似于深度学习框架中采用的计算图，几乎没有数值误差；
• 数值导数(NumericDiffCostFunction)：由用户手动编写导数的数值求解形式，针对无法定义自动求导的模板仿函数的情况, 比如参数的估计调用了无法控制的库函数或外部函数，实现原理上通过有限差分实现导数的计算
• 解析导数(Analytic Derivatives)：当导数存在闭合解析形式时使用，用于可基于CostFunciton基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则不建议使用

代价函数的定义采用仿函数，必须遵守一定的接口规范

考虑如下方程：
y = e a x 2 + b x + c + w \boldsymbol{y}=e^{a\boldsymbol{x}^2+b\boldsymbol{x}+c}+\boldsymbol{w} y=eax2+bx+c+w

其中 a a a、 b b b、 c c c是待估计的曲线参数， w \boldsymbol{w} w是高斯噪声，假设有 n n n个观测样本 ( x i , y i ) (x_i,y_i) (xi,yi)，希望根据这组样本得到最佳的曲线参数，构造如下的优化问题
F ( θ ) = min ⁡ 1 2 n ∑ i = 1 n ( y i − e a x i 2 + b x i + c ) 2 F(\boldsymbol{\theta})=\min \frac{1}{2n}\sum_{i=1}^{n}{(y_i-e^{ax_i^2+bx_i+c})^2} F(θ)=min2n1i=1∑n(yi−eaxi2+bxi+c)2

此时代价函数定义为

struct ExponentialResidual
{
  ExponentialResidual(double x, double y) : x_(x), y_(y)
  {
  }
  template <typename T>
  bool operator()(const T* const a, const T* const b, const T* const c, T* residual) const
  {
    residual[0] = y_ - exp(a[0] * x_ * x_ + b[0] * x_ + c[0]);
    return true;
  }

private:
  const double x_;
  const double y_;
};

const double data[] = {...};
double a = 0, b = 0, c = 0;
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1, 1>(
                                 new ExponentialResidual(data[2 * i], data[2 * i + 1])),
                             nullptr, &a, &b, &c);

在CostFunction模板中：

• 第一个参数FunctorType：表示代价函数的类型
• 第二个参数kNumResiduals：表示代价函数的残差维度，如果代价函数是标量，则kNumResiduals是1；如果代价函数是向量（例如多个误差项的和），则kNumResiduals是该向量的维度
• 后面若干参数kNumParameters：表示每个优化参数的维度，代价函数中有多少个输入的优化变量，就需要在模板里写多少个该参数

所以上面的引例也可以写为

struct ExponentialResidual
{
  ExponentialResidual(double x, double y) : x_(x), y_(y)
  {
  }
  template <typename T>
  bool operator()(const T* const params, T* residual) const
  {
    residual[0] = y_ - exp(params[0] * x_ * x_ + params[1] * x_ + params[2]);
    return true;
  }

private:
  const double x_;
  const double y_;
};

const double data[] = {...};
std::vector<double> params(3);
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 3>(
                                 new ExponentialResidual(data[2 * i], data[2 * i + 1])),
                             nullptr, params.data());

3.2 求解最小二乘问题

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

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

其中

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

对求解器可选的配置Solver::Options做如下说明：

• minimizer_type：迭代的求解方式，可选如下：
  • TRUST_REGION：信赖域方式，默认值
  • LINEAR_SEARCH：线性搜索方法
• trust_region_strategy_type：信赖域策略，可选如下：
  • LEVENBERG_MARQUARDT：列文伯格-马夸尔特方法，默认值
  • DOGLEG：狗腿法
• 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：是否将优化信息输出至终端，默认为false。若设置为true输出根据迭代方法而输出不同：
  • 信赖域方法
    • cost：当前目标函数的数值
    • cost_change：当前参数变化量引起的目标函数变化
    • |gradient|：当前梯度的模长
    • |step|：参数变化量
    • tr_ratio：目标函数实际变化量和信赖域中目标函数变化量的比值
    • tr_radius：信赖域半径
    • ls_iter：线性求解器的迭代次数
    • iter_time：当前迭代耗时
    • total_time：优化总耗时
  • 线性搜索方法
    • f：当前目标函数的数值
    • d：当前参数变化量引起的目标函数变化
    • g：当前梯度的模长
    • h：参数变化量
    • s：线性搜索最优步长
    • it：当前迭代耗时
    • tt：优化总耗时

4 Ceres库案例

4.1 Powell函数优化

Powell函数是数值优化实验中常用的测试函数，其表达式为

min ⁡ x 1 2 ∥ F ( x ) ∥ 2 \min _{\boldsymbol{x}} \frac{1}{2}\|F(\boldsymbol{x})\|^2 xmin21∥F(x)∥2

其中 x = [ x 1 , x 2 , x 3 , x 4 ] T \boldsymbol{x}=[x_1,x_2, x_3, x_4]^T x=[x1,x2,x3,x4]T

f 1 ( x ) = x 1 + 10 x 2 f 2 ( x ) = 5 ( x 3 − x 4 ) f 3 ( x ) = ( x 2 − 2 x 3 ) 2 f 4 ( x ) = 10 ( x 1 − x 4 ) 2 F ( x ) = [ f 1 ( x ) , f 2 ( x ) , f 3 ( x ) , f 4 ( x ) ] \begin{aligned}f_1(\boldsymbol{x})&=x_1 + 10x_2 \\ f_2(\boldsymbol{x})&=\sqrt{5}(x_3 -x_4) \\ f_3(\boldsymbol{x})&=(x_2 -2x_3)^2 \\ f_4(\boldsymbol{x})&=\sqrt{10}(x_1 -x_4)^2 \\ F(\boldsymbol{x}) &= [f_1(\boldsymbol{x}), f_2(\boldsymbol{x}), f_3(\boldsymbol{x}), f_4(\boldsymbol{x})] \end{aligned} f1(x)f2(x)f3(x)f4(x)F(x)=x1+10x2=5 (x3−x4)=(x2−2x3)2=10 (x1−x4)2=[f1(x),f2(x),f3(x),f4(x)]

首先构造四个仿函数

struct F1 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x2, T* residual) const {
    // f1 = x1 + 10 * x2;
    residual[0] = x1[0] + 10.0 * x2[0];
    return true;
  }
};
struct F2 {
  template <typename T>
  bool operator()(const T* const x3, const T* const x4, T* residual) const {
    // f2 = sqrt(5) (x3 - x4)
    residual[0] = sqrt(5.0) * (x3[0] - x4[0]);
    return true;
  }
};
struct F3 {
  template <typename T>
  bool operator()(const T* const x2, const T* const x3, T* residual) const {
    // f3 = (x2 - 2 x3)^2
    residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]);
    return true;
  }
};
struct F4 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x4, T* residual) const {
    // f4 = sqrt(10) (x1 - x4)^2
    residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
    return true;
  }
};

接着构建最小二乘问题

double x1 = 3.0;
double x2 = -1.0;
double x3 = 0.0;
double x4 = 1.0;
ceres::Problem problem;
problem.AddResidualBlock(
    new ceres::AutoDiffCostFunction<F1, 1, 1, 1>(new F1), nullptr, &x1, &x2);
problem.AddResidualBlock(
    new ceres::AutoDiffCostFunction<F2, 1, 1, 1>(new F2), nullptr, &x3, &x4);
problem.AddResidualBlock(
    new ceres::AutoDiffCostFunction<F3, 1, 1, 1>(new F3), nullptr, &x2, &x3);
problem.AddResidualBlock(
    new ceres::AutoDiffCostFunction<F4, 1, 1, 1>(new F4), nullptr, &x1, &x4);

最后优化求解

ceres::Solver::Options options;
options.max_num_iterations = 100;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = false;
// clang-format off
std::cout << "Initial x1 = " << x1
          << ", x2 = " << x2
          << ", x3 = " << x3
          << ", x4 = " << x4
          << "\n";
// clang-format on
// Run the solver!
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
// std::cout << summary.FullReport() << "\n";
// clang-format off
std::cout << "Final x1 = " << x1
          << ", x2 = " << x2
          << ", x3 = " << x3
          << ", x4 = " << x4
          << "\n";
// clang-format on
return 0;

输出结果

Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1
Final x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05

4.2 非线性曲线拟合

考虑如下方程：
y = e m x + c + w y=e^{m{x}+c}+\boldsymbol{w} y=emx+c+w

其中 m m m、 c c c是待估计的曲线参数， w w w是高斯噪声，假设有 n n n个观测样本 ( x i , y i ) (x_i,y_i) (xi,yi)，希望根据这组样本得到最佳的曲线参数，构造如下的优化问题

F ( θ ) = min ⁡ 1 2 n ∑ i = 1 n ( y i − e m x i 2 + c ) 2 F(\boldsymbol{\theta})=\min \frac{1}{2n}\sum_{i=1}^{n}{(y_i-e^{mx_i^2+c})^2} F(θ)=min2n1i=1∑n(yi−emxi2+c)2

struct ExponentialResidual
{
  ExponentialResidual(double x, double y) : x_(x), y_(y)
  {
  }
  template <typename T>
  bool operator()(const T* const params, T* residual) const
  {
    residual[0] = y_ - exp(params[0] * x_ + params[1]);
    return true;
  }

private:
  const double x_;
  const double y_;
};

int main(int argc, char** argv)
{
  std::vector<double> pt_x, pt_y, ground_truth_x, ground_truth_y, predict_x, predict_y;
  const double initial_m = 0.0;
  const double initial_c = 0.0;
  std::vector<double> params(2);
  ceres::Problem problem;
  for (int i = 0; i < kNumObservations; ++i)
  {
    pt_x.push_back(data[2 * i]);
    pt_y.push_back(data[2 * i + 1]);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 2>(
                                 new ExponentialResidual(data[2 * i], data[2 * i + 1])),
                             nullptr, params.data());
  }
  ceres::Solver::Options options;
  options.max_num_iterations = 25;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << "Initial m: " << initial_m << " c: " << initial_c << "\n";
  std::cout << "Final   m: " << params[0] << " c: " << params[1] << "\n";

  double dx = 0.1, x = 0.0;
  while (x < 5.0)
  {
    ground_truth_x.push_back(x);
    ground_truth_y.push_back(std::exp(0.3 * x + 0.1));
    predict_x.push_back(x);
    predict_y.push_back(std::exp(params[0] * x + params[1]));
    x += dx;
  }
  plt::plot(ground_truth_x, ground_truth_y,
            { { "color", "r" }, { "linewidth", "2.0" }, { "linestyle", "-" }, { "label", "ground truth" } });
  plt::plot(predict_x, predict_y,
            { { "color", "b" }, { "linewidth", "2.0" }, { "linestyle", "--" }, { "label", "predict" } });
  plt::scatter(pt_x, pt_y, 40, { { "color", "k" }, { "marker", "x" }, { "label", "noisy data" } });
  plt::xlabel("x");
  plt::ylabel("y");
  plt::title("curve fitting test");
  plt::legend();
  plt::grid(true);
  plt::show();
  return 0;
}

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• ...

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇