Ceres-Solver 从入门到上手视觉SLAM位姿优化问题
https://cggos.github.io/slam/ceres-solver.html
概述
Ceres Solver is an open source C++ library for modeling and solving large, complicated optimization problems.
使用 Ceres Solver 求解非线性优化问题，主要包括以下几部分：
- 构建代价函数(cost function) 或 残差(residual)
- 构建优化问题(
ceres::Problem)：通过AddResidualBlock添加代价函数(cost function)、损失函数(loss function) 和 待优化状态量- LossFunction: a scalar function that is used to reduce the influence of outliers on the solution of non-linear least squares problems.
- 配置求解器(
ceres::Solver::Options) - 运行求解器(
ceres::Solve(options, &problem, &summary))
注意：
Ceres Solver 只接受最小二乘优化，也就是 $\min r^T r$；若要对残差加权重，使用马氏距离，即 $\min r^T P^{-1} r$，则要对 信息矩阵$P^{-1}$ 做 Cholesky分解，即 $LL^T=P^{−1}$，则 $d = r^T (L L^T) r = (L^T r)^T (L^T r)$，令 $r’ = (L^T r)$，最终 $\min r’^T r’$。
入门
先以最小化下面的函数为例，介绍 Ceres Solver 的基本用法
\[\frac{1}{2} (10 - x)^2\]
Part 1: Cost Function
（1）AutoDiffCostFunction（自动求导）
- 构造 代价函数结构体（例如：
struct CostFunctor），在其结构体内对 模板括号()重载，定义残差 - 在重载的
()函数 形参 中，最后一个为残差，前面几个为待优化状态量
struct CostFunctor {
template<typename T>
bool operator()(const T *const x, T *residual) const {
residual[0] = 10.0 - x[0]; // r(x) = 10 - x
return true;
}
};
- 创建代价函数的实例，对于模板参数的数字，第一个为残差的维度，后面几个为待优化状态量的维度
ceres::CostFunction *cost_function;
cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
（2） NumericDiffCostFunction
- 数值求导法 也是构造 代价函数结构体，但在重载 括号
()时没有用模板
struct CostFunctorNum {
bool operator()(const double *const x, double *residual) const {
residual[0] = 10.0 - x[0]; // r(x) = 10 - x
return true;
}
};
- 并且在实例化代价函数时也稍微有点区别，多了一个模板参数
ceres::CENTRAL
ceres::CostFunction *cost_function;
cost_function =
new ceres::NumericDiffCostFunction<CostFunctorNum, ceres::CENTRAL, 1, 1>(new CostFunctorNum);
（3） 自定义 CostFunction
- 构建一个 继承自
ceres::SizedCostFunction<1,1>的类，同样，对于模板参数的数字，第一个为残差的维度，后面几个为待优化状态量的维度 - 重载 虚函数
virtual bool Evaluate(double const* const* parameters, double *residuals, double **jacobians) const，根据 待优化变量，实现 残差和雅克比矩阵的计算
class SimpleCostFunctor : public ceres::SizedCostFunction<1,1> {
public:
virtual ~SimpleCostFunctor() {};
virtual bool Evaluate(
double const* const* parameters, double *residuals, double** jacobians) const {
const double x = parameters[0][0];
residuals[0] = 10 - x; // r(x) = 10 - x
if(jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1; // r'(x) = -1
}
return true;
}
};
Part 2: AddResidualBlock
- 声明
ceres::Problem problem; - 通过
AddResidualBlock将 代价函数(cost function)、损失函数(loss function) 和 待优化状态量 添加到problem
ceres::CostFunction *cost_function;
cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
ceres::Problem problem;
problem.AddResidualBlock(cost_function, NULL, &x);
Part 3: Config & Solve
配置求解器，并计算，输出结果
ceres::Solver::Options options;
options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
Simple Example Code
#include "ceres/ceres.h"
#include "glog/logging.h"
struct CostFunctor {
template<typename T>
bool operator()(const T *const x, T *residual) const {
residual[0] = 10.0 - x[0]; // f(x) = 10 - x
return true;
}
};
struct CostFunctorNum {
bool operator()(const double *const x, double *residual) const {
residual[0] = 10.0 - x[0]; // f(x) = 10 - x
return true;
}
};
class SimpleCostFunctor : public ceres::SizedCostFunction<1,1> {
public:
virtual ~SimpleCostFunctor() {};
virtual bool Evaluate(
double const* const* parameters, double *residuals, double **jacobians