文章目录
重要的Reference
1 install
# install dependencies
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.4 libgflags-dev libgoogle-glog-dev libgtest-dev
# clone
git clone https://github.com/ceres-solver/ceres-solver.git
# build
cd ceres-solver
mkdir build
cmake ..
make
sudo make install
2 最小二乘问题的ceres表示
ceres要求解的问题是最小二乘问题。一般的最小二乘问题定义如下:
min
x
1
2
∑
i
ρ
i
(
∥
f
i
(
x
i
1
,
…
,
x
i
k
)
∥
2
)
s.t.
l
j
≤
x
j
≤
u
j
\begin{array}{cl}{\min _{\mathbf{x}}} & {\frac{1}{2} \sum_{i} \rho_{i}\left(\left\|f_{i}\left(x_{i_{1}}, \ldots, x_{i_{k}}\right)\right\|^{2}\right)} \\ {\text { s.t. }} & {l_{j} \leq x_{j} \leq u_{j}}\end{array}
minx s.t. 21∑iρi(∥fi(xi1,…,xik)∥2)lj≤xj≤uj
- 整个表达式称为ResidualBlock
- ρ i \rho_{i} ρi是LossFucntion,也叫鲁棒核函数,主要是为了应对outliers
- [ x i 1 , … , x i k ] \left[x_{i_{1}}, \ldots, x_{i_{k}}\right] [xi1,…,xik] ParameterBlock
- f i f_i fi 称为CostFunction,通过调用这个函数可以计算残差residual
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
// A templated cost functor that implements the residual r = 10 -
// x. The method operator() is templated so that we can then use an
// automatic differentiation wrapper around it to generate its
// derivatives.
// 定义cost function的调用函数接口,是一个仿函数functor
// ceres::CostFunction通过这个函数来计算残差Residual
struct CostFunctor {
template<typename T>
bool operator()(const T *const x, T *residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
int main(int argc, char **argv) {
google::InitGoogleLogging(argv[0]);
// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
double x = 0.5;
const double initial_x = x;
// 1. 构建求解问题
Problem problem;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
// 2. 定义cost function,此处是使用自动求导
CostFunction *cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
// 3. 添加残差块,具体包括3个项目
// CostFunction, LossFunction, ParameterBlock
problem.AddResidualBlock(cost_function, NULL, &x);
// 4. 进行求解设置,打印输出
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}
3 求导
最小二乘问题描述
3.1 自动求导
上一个入门的例子,展示了如何利用ceres的自动求导
// 定义一个是模板成员函数的仿函数
struct CostFunctor {
template<typename T>
bool operator()(const T *const x, T *residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
// 使用自动求导来定义CostFunction
CostFunction *cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
3.2 Numeric Derivatives
使用数值求导的时候,用户不需要定义模板成员函数的仿函数,只需要定义一个普通的仿函数即可,使用对应的数据类型
// 定义一个普通的仿函数
struct CostFunctor {
bool operator()(const double *const x, double *residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
// 使用数值求导
CostFunction *cost_function = new NumericDiffCostFunction<CostFunctor, CENTRAL, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
通常来讲,更推荐用户使用自动求导。使用cpp模板会使用自动求导效率更高,然而数值求导代价是昂贵的,容易出现数字错误,并导致收敛变慢。
3.3 Analytic Derivatives
在某些情况下,无法使用自动求导。 例如,可能存在这样的情况,即以closed form(闭式解,也叫解析解)计算导数而不是依靠自动求导,这样的话通过使用的链式规则来就到得到雅克比矩阵更为有效。
几个要点:
- 知道残差维度,参数的维度,雅克比维度
- 雅克比计算方法,残差计算方法
// 继承SizedCostFunction,定义CostFunction的类
class QuadraticCostFunction
: public SizedCostFunction<1 /* number of residuals */,
1 /* size of first parameter */> {
public:
virtual ~QuadraticCostFunction() {}
/**
* @brief 重载Evaluate函数,完成jacobian和residuals的计算
* @param parameters
* @param residuals
* @param jacobians
* @return
*/
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const {
double x = parameters[0][0]; // parameters[0]表示取出第一组参数
// f(x) = 10 - x.
residuals[0] = 10 - x; // 计算残差
// f'(x) = -1. Since there's only 1 parameter and that parameter
// has 1 dimension, there is only 1 element to fill in the
// jacobians.
//
// Since the Evaluate function can be called with the jacobians
// pointer equal to NULL, the Evaluate function must check to see
// if jacobians need to be computed.
//
// For this simple problem it is overkill to check if jacobians[0]
// is NULL, but in general when writing more complex
// CostFunctions, it is possible that Ceres may only demand the
// derivatives w.r.t. a subset of the parameter blocks.
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1; //计算雅克比
}
return true;
}
};
// 使用解析求导
CostFunction *cost_function = new QuadraticCostFunction;
problem.AddResidualBlock(cost_function, NULL, &x);
4 Powell’s Function
目标函数是多维度的时候,可以拆成多个CostFunctor来写,每一个维度,对应一个functor。
这样求解其实非常麻烦,也可以直接定义一个CostFunctor即可,只是要注意残差维度和参数维度。
下面给出两种写法,两种写法经过验证是等价的。
这里使用的是自动求导方式。
最小二乘问题描述
4.1 Powell’s Function的多个CostFunctor写法
// 定义cost funtor
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;
}
};
// 使用cost functor, 逐个添加参数块
double x1 = 3.0;
double x2 = -1.0;
double x3 = 0.0;
double x4 = 1.0;
problem.AddResidualBlock(new AutoDiffCostFunction<F1, 1, 1, 1>(new F1),
NULL,
&x1, &x2);
problem.AddResidualBlock(new AutoDiffCostFunction<F2, 1, 1, 1>(new F2),
NULL,
&x3, &x4);
problem.AddResidualBlock(new AutoDiffCostFunction<F3, 1, 1, 1>(new F3),
NULL,
&x2, &x3);
problem.AddResidualBlock(new AutoDiffCostFunction<F4, 1, 1, 1>(new F4),
NULL,
&x1, &x4);
4.2 写成一个cost functor的方法
// 定义
struct CostFunctor {
template<typename T>
bool operator()(const T *const x, T *residual) const {
T x1 = x[0], x2 = x[1], x3 = x[2], x4 = x[3];
residual[0] = x1 + 10.0*x2;
residual[1] = sqrt(5.0)*(x3 - x4);
residual[2] = (x2 - 2.0*x3)*(x2 - 2.0*x3);
residual[3] = sqrt(10.0)*(x1 - x4)*(x1 - x4);
return true;
}
};
// 使用
double x[4] = {3,-1,0,1};
problem.AddResidualBlock(new AutoDiffCostFunction<CostFunctor, 4, 4>(new CostFunctor),
nullptr,
x);
5 Curve Fitting
拟合曲线参数,问题描述如下。相比之前的几个例子,曲线拟合需要向cost functor中传入参数。
分别使用自动求导方式求解和解析求导方式求解
5.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_;
};
// 逐个添加参数块
double m = 0.0;
double c = 0.0;
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);
}
5.2 解析求导
注意在使用解析求导的时候,**求导的正负号一定不要弄错,也就是你应该是对cost function进行求导。**比如本问题的求导结果应该是
jacobians[0][0] = -x_*exp(m*x_ + c);
jacobians[0][4] = -exp(m*x_ + c);
如果符号弄反了,结果为0,求解错误。。。
//-------------------------------------------
// 解析求导方式
//-------------------------------------------
class AnalyticCostFunction
: public ceres::SizedCostFunction<1 /* number of residuals */,
2 /* size of first parameter */> {
public:
AnalyticCostFunction(double x, double y) : x_(x), y_(y) {}
virtual ~AnalyticCostFunction() {}
/**
* @brief 重载Evaluate函数,完成jacobian和residuals的计算
* @param parameters
* @param residuals
* @param jacobians
* @return
*/
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const {
double m = parameters[0][0]; // parameters[0]表示取出第一组参数
double c = parameters[0][5];
// 计算残差
residuals[0] = y_ - exp(m*x_ + c);
// 计算雅克比
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -x_*exp(m*x_ + c);
jacobians[0][6] = -exp(m*x_ + c);
}
return true;
}
private:
const double x_;
const double y_;
};
// 逐个添加参数块
double x[2] = {0, 0};
for (int i = 0; i < kNumObservations; ++i) {
CostFunction *cost_function = new AnalyticCostFunction(data[2*i], data[2*i + 1]);
problem.AddResidualBlock(cost_function, NULL, x);
}
5.3 使用核函数 Robust Curve Fitting
使用核函数求解和不使用的对比。可以发现,使用鲁棒核函数,对于outliers有更鲁棒的效果,求解结果更接近真值。
- 不使用
- 使用
6 Bundle Adjustment
One of the main reasons for writing Ceres was our need to solve large scale bundle adjustment problems.
ceres的出现就是为了求解大规模BA问题。
同理,使用自动求导或者解析求导都可以。
7 关于求导的一些高阶知识
ceres一共支持三种求导方式,你必须知道的三个原则:
- 尽可能使用自动求导
- 某些时候,使用解析求导是有必要的,比如对于复杂的slam问题
- 避免使用数值求导。使用它作为最后手段,主要是为了与外部库接口。
7.1 解析求导
7.1.1 关于解析求导的trick
例子说明了,在使用解析求导的时候,对于求导过程中用到的共同计算部分,应该尽可能多的使用中间变量,提前计算好,节省计算量。
这个有时候非常重要!!
例子
经过使用中间变变量优化的结果如下:
7.1.2 什么时候使用解析求导?
- 参数是过参数化的形式,或者在manifold space的参数,必须解析求导
有个例子,a-loam优化xyzrpy使用自动求导完成,猜测原因就是仍然在三维空间进行优化的,并不是manifold或者过参数化。 - cost function简单,eg: linear
- 可以使用Maple,Mathematica,Sympy快速得到你的解析导数
- 考虑性能的话,解析导数比自动微分更好
- 你非常喜欢推公式(haha,我就喜欢!!)
什么时候使用自动求导?
- 如果得到这些导数太麻烦,那就是用自动求导
- 有些时候,没有其他计算导数的方法,例如,您希望计算多项式根的导数,参看 Inverse Function Theorem
7.2 数值求导
三种方式的不同和性能差别,具体参看地址
7.2.1 三种数值求导方法
- Forward Differences
D f ( x ) ≈ f ( x + h ) − f ( x ) h D f(x) \approx \frac{f(x+h)-f(x)}{h} Df(x)≈hf(x+h)−f(x)
其中, h h h是一个非常小的数,趋近于0
the error in the forward difference formula is O ( h ) O(h) O(h) - Central Differences
D f ( x ) ≈ f ( x + h ) − f ( x − h ) 2 h D f(x) \approx \frac{f(x+h)-f(x-h)}{2 h} Df(x)≈2hf(x+h)−f(x−h)
the error in the forward difference formula is O ( h 2 ) O(h^2) O(h2) - Ridders’ Method
the error in the forward difference formula is O ( h 4 ) O(h^4) O(h4)
7.2.2三种方法对比
三种方法的求导误差对比图
耗时对比图
Central方法是Forward方法耗时的两倍,Ridders’的方法则耗时增加了一个数量级。
7.2.3 什么时候使用数值求导
当不能用解析法或自动微分法计算导数时,应使用数值微分法。通常情况下,当您调用一个您不知道其解析形式的外部库或函数时,或者即使您知道,您也无法以使用自动导数所需的方式重新编写它。
当使用数值微分时,至少使用中心差分(Central方法),如果执行时间不是问题,或者目标函数很难确定一个好的静态相对步长,则推荐使用Ridders方法。
7.3 自动求导
这是一种能够快速计算精确导数的技术,同时需要用户付出与使用数值求导所需的相同的努力。因为相比数值求导,代码量一样。
自动求导如何工作的,参看具体官方文档
7.4 求导方式的总结
-
从耗时来看
解析求导 < 自动求导 < 数值求导Central < 数值求导Ridders方法 -
从精度来看
优先选择解析求导,自动求导