360急速网址导航_建设官网兴田德润怎么联系_网站页面设计模板_百度指数api

0 专栏介绍

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

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

---

1 Ceres库介绍

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

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

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

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

最小二乘问题表述如下

$$\begin{gathered} \underset{\mathbf{x}}{\min }F\left(\mathbf{x}\right)=\frac{1}{2}\underset{\mathbf{x}}{\min }\sum _i{\rho }_i({\Vert f_i\left(\mathbf{x}\right)\Vert }_2^2) \\ s.t.\mathbf{l}\le \mathbf{x}\le \mathbf{u} \end{gathered}$$

其中${\rho }_i({\Vert f_i\left(\mathbf{x}\right)\Vert }_2^2)$称为残差块(residual block)，一个优化问题通常由多个残差块组成，每个残差块负责计算一个或多个残差；$f_i\left(\cdot \right)$是代价函数，${\rho }_i\left(\cdot \right)$是损失函数；$\mathbf{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_layoutclass ExampleRecipe(ConanFile):settings = "os", "compiler", "build_type", "arch"generators = "CMakeDeps", "CMakeToolchain", "cmake_find_package", "cmake"def configure(self):self.options["ceres-solver"].shared = Trueself.options["ceres-solver"].use_glog = Trueself.options["glog"].shared = Trueself.options["glog"].with_threads = Trueself.options["gflags"].shared = Truedef 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基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则不建议使用

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

考虑如下方程：
$$\mathbf{y}=e^{a{\mathbf{x}}^2+b\mathbf{x}+c}+\mathbf{w}$$
其中$a$、$b$、$c$是待估计的曲线参数，$\mathbf{w}$是高斯噪声，假设有$n$个观测样本$(x_i,y_i)$，希望根据这组样本得到最佳的曲线参数，构造如下的优化问题
$$F(\mathbf{\theta })=\min \frac{1}{2n}\sum _{i=1}^n(y_i-e^{a{x}_i^2+b{x}_i+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函数是数值优化实验中常用的测试函数，其表达式为

$$\underset{\mathbf{x}}{\min }\frac{1}{2}\Vert F(\mathbf{x}){\Vert }^2$$

其中$\mathbf{x}=[x_1,x_2,x_3,x_4{]}^T$

$$\begin{array}{rl}{f}_1(\mathbf{x})& =x_1+10x_2\\ f_2(\mathbf{x})& =\sqrt{5}(x_3-x_4)\\ f_3(\mathbf{x})& =(x_2-2x_3{)}^2\\ f_4(\mathbf{x})& =\sqrt{10}(x_1-x_4{)}^2\\ F(\mathbf{x})& =[f_1(\mathbf{x}),f_2(\mathbf{x}),f_3(\mathbf{x}),f_4(\mathbf{x})]\end{array}$$

首先构造四个仿函数

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)^2residual[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)^2residual[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^{mx+c}+\mathbf{w}$$
其中$m$、$c$是待估计的曲线参数，$w$是高斯噪声，假设有$n$个观测样本$(x_i,y_i)$，希望根据这组样本得到最佳的曲线参数，构造如下的优化问题

$$F(\mathbf{\theta })=\min \frac{1}{2n}\sum _{i=1}^n(y_i-e^{m{x}_i^2+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深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

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