目录
- 0 专栏介绍
- 1 `Ceres`库介绍
- 2 `Ceres`库安装
- 3 `Ceres`库概念
- 3.1 构建最小二乘问题
- 3.1.1 残差块
- 3.1.2 代价函数
- 3.2 求解最小二乘问题
- 4 `Ceres`库案例
- 4.1 Powell函数优化
- 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深度学习实战》
- 《机器学习强基计划》
- 《运动规划实战精讲》
- …