Reference:
- 高翔,张涛 《视觉SLAM十四讲》
相关文章跳转:
1. Ceres 简介
Ceres 是一个广泛使用的最小二乘问题求解库。在 Ceres 中,我们作为用户,只需按照一定步骤定义待解的优化问题,然后交给求解器计算。Ceres 求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘):
min
x
1
2
∑
i
ρ
i
(
∥
f
i
(
x
i
1
,
…
x
i
n
)
∥
2
)
s.t.
l
j
≤
x
j
≤
u
j
\begin{array}{ll} \min _x & \frac{1}{2} \sum_i \rho_i\left(\left\|f_i\left(x_{i_1}, \ldots x_{i_n}\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,…xin)∥2)lj≤xj≤uj
在这个问题中,
x
1
,
.
.
.
x
n
x_1,...x_n
x1,...xn 为优化变量
,又称参数块(Parameter blocks)
,
f
i
f_i
fi 称为代价函数(Cost function)
,也称为残差块(Residual blocks)
,在 SLAM 中也可理解为误差项。
l
j
l_j
lj 和
u
j
u_j
uj 为第
j
j
j 个优化变量的上限和下限。在最简单的情况下,取
l
j
=
−
∞
l_j=-\infty
lj=−∞,
u
j
=
∞
u_j=\infty
uj=∞ (不限制优化变量的边界)。此时,目标函数由许多平方项经过一个核函数
ρ
(
⋅
)
\rho(\cdot)
ρ(⋅) 之后求和组成。同样,可以取
ρ
\rho
ρ 为恒等函数,那么目标函数即为许多项的平方和,我们就得到了无约束的最小二乘问题。
为了让 Ceres 帮我们求解这个问题,我们需要做以下几件事:
- 定义每个参数块。参数块通常为平凡的向量,但是在 SLAM 里也可以定义成四元数、李代数这种特殊的结构。如果是向量,那么我们需要为每个参数块分配一个 double 数组来存储变量的值。
- 定义残差块的计算方式。残差块通常关联若干个参数块,对它们进行一些自定义的计算,然后返回残差值。Ceres 对它们求平方和之后,作为目标函数的值。
- 残差块往往也需要定义雅克比的计算方式。在 Ceres 中,可以使用它提供的“自动求导”功能,也可以手动指定雅克比的计算过程。如果要使用自动求导,那么残差块需要按照特定的写法书写:残差的计算过程应该是一个带模板的括号运算符。
- 把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中,调用 Solve 函数求解即可。求解之前,我们可以传入一些配置信息,例如迭代次数、终止条件等,也可以使用默认的配置。
2. 使用 Ceres 拟合曲线
下面的代码演示了如何使用 Ceres 求解问题:
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
// 代价函数的计算模型
struct CURVE_FITTING_COST {
CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
// 残差的计算
template<typename T>
bool operator()(
const T *const abc, // 模型参数,有3维
T *residual) const {
// 这里残差的计算公式:y-exp(ax^2+bx+c)
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
return true;
}
const double _x, _y; // x,y数据
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
/** @code 生成一组带噪声的数据
*/
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
/** @endcode */
double abc[3] = {ae, be, ce};
// 构建最小二乘问题
ceres::Problem problem;
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导,模板参数:误差类型,输出维度,输入维度,维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数,这里不使用,为空
abc // 待估计参数
);
}
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 输出到cout
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}
程序需要说明的地方均已加注释。可以看到,我们利用 OpenCV 的噪声生成器生成了 100 100 100 个带高斯噪声的数据,随后利用 Ceres 进行拟合。这里演示的 Ceres 用法有如下几项:
- 定义残差块的类。方法是书写一个类(或结构体),并在类中定义带模板参数的
() 运算符
,这样该类就成为了一个拟函数(Functor)
。这种定义方式使得 Ceres 可以像调用函数一样,对该类的某个对象(比如说 a)调用 a() 方法。事实上,Ceres 会把雅克比矩阵作为类型参数传入此函数,从而实现自动求导的功能。 - 程序中的 double abc[3] 即参数块,而对于残差块,我们对每一个数据构造 CURVE_FITTING_COST 对象,然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,我们有若干种选择:
(1)使用 Ceres 的自动求导(Auto Diff);
(2)使用数值求导(Numeric Diff);
(3)自行推导解析的导数形式,提供给 Ceres。
因为自动求导在编码上是最方便的,于是我们使用自动求导(1)。 - 自动求导需要指定误差项和优化变量的维度(即AutoDiffCostFunction需要传入输入维度和输出维度)。这里的误差项是标量,维度为 1;优化的是 a, b, c 三个量,维度为 3。于是,在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为 1、3。
- 设定好问题后,调用 Solve 函数进行求解。你可以在 options 里配置(非常详细的)优化选项。例如,我们可以选择使用 Line Search 还是 Trust Region,迭代次数,步长等等。读者可以查看 Options 的定义,看看有哪些优化方法可选,当然默认的配置已经可以用在很广泛的问题上了。
2.1 数值求导与自动求导的区别
We will now consider automatic differentiation. It is a technique that can compute exact derivatives, fast, while requiring about the same effort from the user as is needed to use numerical differentiation.
3. 数值解的使用方法
// 1.这里使用的是ceres的自动求导
struct FITTING_COST{
FITTING_COST(double molecule,double denominator,double delta_combine):
_molecule(molecule),_denominator(denominator),_delta_distance(delta_combine){}
// 残差的计算
template<typename T>
bool operator()(const T *const res, T *residual) const{
residual[0] = (res[0] + _delta_distance) * (_denominator - res[1]) - _molecule;
return true;
}
const double _molecule, _denominator, _delta_distance;
};
// 2.这里使用的是解析解求导方法
// A CostFunction implementing analytically derivatives for the function
class FITTING_COST_EXTRA
: public ceres::SizedCostFunction<1 /* number of residuals */,
2 /* size of first parameter */> {
public:
FITTING_COST_EXTRA(double molecule,double denominator,double delta_combine):
_molecule(molecule),_denominator(denominator),_delta_distance(delta_combine){}
virtual ~FITTING_COST_EXTRA() {}
virtual bool Evaluate(double const* const* res,
double* residual,
double** jacobians) const {
residual[0] = (res[0][0] + _delta_distance) * (_denominator - res[0][1]) - _molecule;
// 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] = _denominator - res[0][1];
jacobians[0][1] = -res[0][0] - _delta_distance;
}
return true;
}
const double _molecule, _denominator, _delta_distance;
};
int main(){
......................................................................
double res[2] = {res_distance, res_delta_x};
//build the least square problem
ceres::Problem problem;
// 1.自动求导方法
// for (size_t j = 0; j < bucket.size(); j++)
// {
// problem.AddResidualBlock( //add error terms
// new ceres::AutoDiffCostFunction<FITTING_COST,1,2>(
// new FITTING_COST(
// bucket[j]._pre_molecule,
// bucket[j]._pre_denominator,
// bucket[j]._pre_delta_distance)
// ),
// nullptr,
// res
// );
// }
// 2.自行推导导数方式
for (size_t j = 0; j < bucket.size(); j++)
{
problem.AddResidualBlock( //add error terms
new FITTING_COST_EXTRA(
bucket[j]._pre_molecule,
bucket[j]._pre_denominator,
bucket[j]._pre_delta_distance),
nullptr,
res
);
}
problem.SetParameterUpperBound(res,1,0.6);
problem.SetParameterLowerBound(res,1,-0.5);
// config solvers
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = false;
//options.min_line_search_step_size = 0.5;
//options.max_num_iterations = 10;
ceres::Solver::Summary summary; //optimize info
ceres::Solve(options, &problem, &summary); //start optimization
......................................................................
}