欢迎访问我的博客首页。
Ceres-Solver
ceres-solver 简称 ceres，它和 g2o 都是 SLAM 中常用的非线性优化库。比如，cartographer 使用的是 ceres，ORB-SLAM 使用的是 g2o。ceres 有官方文档。
1. 安装 Ceres-Solver
安装 Ceres-Solver 有点麻烦，因为它的依赖较多。按照顺序依次安装下面的库，选中 BUILD_SHARED_LIBS：
- gflags。
- gtest。
- glog。依赖 gflags 和 gtest。
- lapack。会安装 lapack 和 blas。出现 multiple definition of `_gfortran_concat_string’ 错误时，参考它的 issues/305。
- suitesparse。该安装包中有一个编译好了的 lapack_windows 文件夹。先点击 cmake-gui 的 configure，出现 LAPACK_DIR，然后把这个目录设置为上面我们自己编译的 lapack，最后删除 lapack_windows 文件夹。会同时安装 amd、btf、camd、ccolamd、colamd、cholmod、cxsparse、klu、ldl、umfpack、spqr、suitesparseconfig。INCLUDE 目录选这些库的 .h 文件所在目录，库文件选择它们的 .a 文件。编译时不要选择 METIS，否则需要先安装 GKlib。
- Eigen。在官网下载或从官网去 gitlab 都可以下载。
- ceres-solver。按照需要设置上面的依赖路径。我安装的是版本是 2.1.0rc2。
2. 曲线拟合
使用 Ceres 拟合函数 y = x 2 + 2 x + 3 y = x^2 + 2x + 3 y=x2+2x+3，即，已知若干对 ( x , y ) (x, y) (x,y) 值，求其系数 a , b , c a, b, c a,b,c。参考《视觉 SLAM 十四讲》第 6.3.3 节 《使用 Ceres 拟合曲线》，其源码在 Github。
2.1 拟合算法
使用数组 double abc[3] 存放待估计参数，即一元二次方程的 3 个系数。
#include <ceres/ceres.h>
#include <iostream>
#include <random>
#include <vector>
// 误差类。
struct Function {
Function(double _x, double _y) : x(_x), y(_y) {}
template <typename T>
bool operator()(const T* const abc, T* residual) const {
residual[0] = T(y) - (abc[0] * T(x) * T(x) + abc[1] * T(x) + abc[2]);
return true;
}
const double x, y;
};
// 使用误差类创建代价函数。
ceres::CostFunction* create(double x, double y) {
Function* function = new Function(x, y);
return new ceres::AutoDiffCostFunction<Function, 1, 3>(function);
}
int main(void) {
// 设置随机数产生方法。
std::random_device rd;
std::mt19937 gen(rd());
std::normal_distribution<double> normal(0.0, 1.0);
// 1.产生 N 组观测数据，y = x^2 + 2x + c + 噪声。
const int N = 100;
std::vector<double> data_x;
std::vector<double> data_y;
data_x.reserve(N);
data_y.reserve(N);
for (int i = 0; i < N; i++) {
data_x[i] = i;
data_y[i] = 1 * i * i + 2 * i + 3 + normal(gen);
}
// 2.待优化参数块，随意的初始值。
double abc[] = {0, 0, 0};
// double tmp[] = {0, 0, 0};
// std::vector<double *> abc = {tmp + 0};
// 3.定义问题，添加残差。
ceres::Problem problem;
for (int i = 0; i < N; i++) {
ceres::CostFunction *cost_function = create(data_x[i], data_y[i]);
problem.AddResidualBlock(cost_function, nullptr, abc);
}
// 4.配置求解器，优化并输出日志。
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;
// 5.获取优化结果。
std::cout << abc[0] << ' ' << abc[1] << ' ' << abc[2] << std::endl;
// std::cout << *(abc[0] + 0) << ' ' << *(abc[0] + 1) << ' ' << *(abc[0] + 2) << std::endl;
return 0;
}
代码开头先定义了一个 Function 类，这是一个函数对象，因为它定义了调用运算符。我们把它称为误差类。然后 main 函数的开头定义了噪声的产生方法：使用 C++ 标准库产生高斯噪声，normal 的两个参数分别是均值和标准差。接下来可以分为 5 部分：
第 1 部分产生观测数据。x 为 0 到 N 的整数， y = a x 2 + b x + c + n o i s e y = a x^2 + b x + c + noise y=ax2+bx+c+noise，其中 a = 1，b=2，c=3，noise 是高斯噪声。
第 2 部分是待估计的系数 a，b，c，初始值都是 0。
第 3 部分定义问题和损失函数，添加残差。这部分又分为 3 步：
-
创建误差对象，参数是一组观测数据 ( x , y ) (x, y) (x,y)。误差类的类名是随意的，但它必须有一个模板类型的括号运算符重载函数，用于计算残差 r e s i d u a l i = y i − ( a x i 2 + b x i + c ) residual_i = y_i - (a x_i^2 + b x_i + c) residuali=yi−(axi2+bxi+c)。该重载函数可以有多个参数，但最后一个参数必须是残差。
-
创建代价函数对象。我们使用 Ceres 的自动求导代价函数 AutoDiffCostFunction，它的模板参数分别是：Function 类，残差的维度(Function 类重载函数的最后一个参数维度)，各参数块的维度(Function 类重载函数第 1 到倒数第 2 个参数的维度)。
-
添加残差块。参数分别是上一步创建的代价函数，损失函数，待优化参数。
第 4、5 部分开始优化并获取优化结果。BriefReport 输出简单摘要，FullReport 输出详细摘要。
2.2 参数维度
现在我们来详细说一下 2.1 第 3 部分的红色文字。在 2.1 中，我们相当于把待估计参数看成 1 个三维数据。现在我们把待估计参数看成是 1 个一维数据和 2 个二维数据。
// 误差类。
struct Function {
Function(double _x, double _y) : x(_x), y(_y) {}
template <typename T>
bool operator()(const T* const a, const T* const bc, T* residual) const {
residual[0] = T(y) - (a[0] * T(x) * T(x) + bc[0] * T(x) + bc[1]);
return true;
}
const double x, y;
};
// 使用误差类创建代价函数。
ceres::CostFunction* create(double x, double y) {
Function* function = new Function(x, y);
return new ceres::AutoDiffCostFunction<Function, 1, 1, 2>(function);
}
int main(void) {
// ...
problem.AddResidualBlock(cost_function, nullptr, abc, abc + 1);
// ...
}
下面我们用一幅图来理清三部分的关系。紫色直线连接的二个框从上到下分别是指向残差变量的指针、残差的维度，绿色直线连接的三个框从上到下分别是指向第一个待估计参数的指针、第一个待估计参数的维度、第一个待估计参数的首地址，蓝色直线连接的三个框从上到下分别是指向第二个待估计参数的指针、第二个待估计参数的维度、第二个待估计参数的首地址。
2.3 配置文件
配置文件 CMakeLists.txt：
cmake_minimum_required(VERSION 3.5.1)
project(demo)
# 1. 查找依赖。
find_package(Ceres REQUIRED)
# 2. 设置包含目录和库目录。
include_directories(
${CERES_INCLUDE_DIRS}
)
# 3. 生成可执行程序。
add_executable(main
main.cc
)
target_link_libraries(main
${CERES_LIBRARIES}
)
3. 使用方法
Ceres-Solver 有三种求导方式，上面使用最简单的自动求导。
SetParameterBlockConstant
AddParameterBlock
AddResidualBlock
3.1 AddResidualBlock
该函数的声明如下，它的参数分别是代价函数类指针、损失函数、待优化参数。
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function,
LossFunction *loss_function,
const vector<double *> parameter_blocks)
template <typename Ts...>
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function,
LossFunction *loss_function,
double *x0,
Ts... xs)
代价函数类定义了误差的计算方法。代价函数还需要定义误差对优化对象的求导方法，有自动求导、数值求导和解析求导三种可选。
优化问题的目的是使代价函数值趋向 0，因此代价函数的绝对值与梯度的绝对值呈正相关。为了使代价函数值很大时梯度不至于过分的大，代价函数值很小时梯度不至于过分的小，引入了损失函数。AddResidualBlock 中的损失函数可以是 nullptr，此时代价函数值就是误差向量各项的平方之和。
待优化参数可以是 vector<double*> 或 double* 类型。
4. 求导
为 ceres 的代价函数定义误差类时，有自动求导、数值求导和解析求导 3 种求导方式可选。下面我们还用第 2 节拟合曲线的例子，讲述 3 种求导方法的使用。
4.1 自动求导
第 2 节拟合曲线的例子使用的是自动求导，我们把创建代价函数的代码复制过来。
// 误差类。
struct Function {
Function(double _x, double _y) : x(_x), y(_y) {}
template <typename T>
bool operator()(const T* const abc, T* residual) const {
residual[0] = T(y) - (abc[0] * T(x) * T(x) + abc[1] * T(x) + abc[2]);
return true;
}
const double x, y;
};
// 使用误差类创建代价函数。
ceres::CostFunction* create(double x, double y) {
Function* function = new Function(x, y);
return new ceres::AutoDiffCostFunction<Function, 1, 3>(function);
}
4.2 数值求导
自动求导需要使用模板，而有时我们无法定义模板类型的误差类，比如计算误差时用到第三方库。此时可以使用数值求导。
// 误差类。
struct Function {
Function(double _x, double _y) : x(_x), y(_y) {}
bool operator()(const double* const abc, double* residual) const {
residual[0] = y - (abc[0] * x * x + abc[1] * x + abc[2]);
return true;
}
const double x, y;
};
// 使用误差类创建代价函数。
ceres::CostFunction* create(double x, double y) {
Function* function = new Function(x, y);
return new ceres::NumericDiffCostFunction<Function, ceres::CENTRAL, 1, 3>(function);
}
可以看出，使用数值求导无需定义模板函数，因此较自动求导更简便。但使用 C++ 模板的自动求导更高效，数值求导却代价高昂，容易出现数值错误，并导致收敛速度较慢。因此应该优先选择自动求导。
4.3 解析求导
解析求导需要自定义雅可比矩阵。我们的误差 e 是关于优化对象 a, b, c 的函数，假设观测数据是 (x, y)，那么
e = f ( a , b , c ) = y − ( a x 2 + b x + c ) , e = f(a, b, c) = y - (ax^2 + bx +c), e=f(a,b,c)=y−(ax2+bx+c),
于是雅可比矩阵
J = [ d e d a d e d b d e d c ] = [ − x 2 − x − 1 ] . {\bf J} = \begin{bmatrix} \frac{de}{da} & \frac{de}{db} & \frac{de}{dc} \end{bmatrix} = \begin{bmatrix} -x^2 & -x & -1 \end{bmatrix}. J=[dadedbdedcde]=[−x2−x−1].
据此，我们定义如下代价类。
// 误差类。
struct Function : public ceres::SizedCostFunction<1, 3> {
Function(double _x, double _y) : x(_x), y(_y) {}
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
residuals[0] = y - (parameters[0][0] * x * x + parameters[0][1] * x + parameters[0][2]);
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -x * x;
jacobians[0][1] = -x;
jacobians[0][2] = -1;
}
return true;
}
virtual ~Function() {}
const double x, y;
};
// 使用误差类创建代价函数。
ceres::CostFunction* create(double x, double y) {
Function* function = new Function(x, y);
return function;
}
与 ceres::AutoDiffCostFunction 和 ceres::NumericDiffCostFunction 一样，ceres::SizedCostFunction 的模板参数也是残差的维度和各参数块的维度。
当误差的维度和各参数块的维度已知且固定不变时，我们可以使用 ceres::SizedCostFunction。否则我们可以使用 ceres::CostFunction，通过 set_num_residuals 指定残差的维度，通过 mutable_parameter_block_sizes 指定各参数块的维度。继承 ceres::CostFunction 的代价函数类可以处理多种维度的误差和参数块，灵活性更高。
struct Function : public ceres::CostFunction {
Function(int num_residuals, std::vector<int32_t> parameter_block_sizes, double _x, double _y)
: x(_x)
, y(_y) {
set_num_residuals(num_residuals);
// *mutable_parameter_block_sizes() = parameter_block_sizes;
for (int32_t x : parameter_block_sizes) {
mutable_parameter_block_sizes()->push_back(x);
}
}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
residuals[0] = y - (parameters[0][0] * x * x + parameters[0][1] * x + parameters[0][2]);
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -x * x;
jacobians[0][1] = -x;
jacobians[0][2] = -1;
}
return true;
}
virtual ~Function() {}
const double x, y;
};
ceres::CostFunction *create(double x, double y) {
Function *function = new Function(1, {3}, x, y);
return function;
}
4.4 求导总结
自动求导：代价函数须是模板类型。相比非模板类型，模板类型使得求导较高效且健壮。Ceres 推荐我们使用自动求导，因为速度快且准确性高。
数值求导：代价函数的定义最为简单，但效率和健壮性不如自动求导。因此只有在无法定义模板类型的代价函数时才考虑使用数值求导。
解析求导：明确定义雅可比矩阵，求导效率高。需要计算雅可比矩阵，因此代价函数的定义变得复杂。因此只有对性能有较高要求时才考虑解析求导。
5. 求解 ICP
ICP 问题：已知两组三维点，求它们的相对位姿。我们要验证相对位姿是不是
{ R = [ 0 1 0 − 1 0 0 0 0 1 ] t = [ 0 , − 1 , 0 ] . \left\{\begin{aligned} R &= \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} \\ t &= [0, -1, 0] \end{aligned}\right.. ⎩ ⎨ ⎧Rt= 0−10100001 =[0,−1,0].
本文的例子参照 A-LOAM。本例中，旋转矩阵用四元数 Eigen::Quaternion 表示，平移向量用矩阵 Eigen::Matrix 表示。
5.1 定义误差类
class Function {
public:
Function(Eigen::Vector3d _point_a, Eigen::Vector3d _point_b) {
point_a = _point_a;
point_b = _point_b;
}
template<typename T>
bool operator()(const T* para_q, const T* para_t, T* residual) const {
Eigen::Matrix<T, 3, 1> matrix_a{ T(point_a.x()), T(point_a.y()) ,T(point_a.z()) };
Eigen::Matrix<T, 3, 1> matrix_b{ T(point_b.x()), T(point_b.y()) ,T(point_b.z()) };
Eigen::Quaternion<T> rotation{ para_q[3],para_q[0], para_q[1], para_q[2] };
Eigen::Matrix<T, 3, 1> translation{ para_t[0],para_t[1],para_t[2] };
Eigen::Matrix<T, 3, 1> temp = rotation * matrix_a + translation;
residual[0] = temp[0] - matrix_b[0];
residual[1] = temp[1] - matrix_b[1];
residual[2] = temp[2] - matrix_b[2];
return true;
}
private:
Eigen::Vector3d point_a;
Eigen::Vector3d point_b;
};
和第一个拟合函数的例子一样，该误差类的构造函数接收一个观测值 point_a 和一个实际值 point_b。重载函数的参数分别是表示旋转的四元数、平移向量、误差。在重载函数体内，利用观测值 point_a、旋转矩阵、平移向量得到测量值 temp，然后把测量值与实际值的误差赋值给重载函数的第三个参数。
5.2 创建代价函数对象
ceres::CostFunction* create(Eigen::Vector3d point_a, Eigen::Vector3d point_b) {
Function* function = new Function(point_a, point_b);
return new ceres::AutoDiffCostFunction<Function, 3, 4, 3>(function);
}
和第一个拟合函数的例子一样，首先使用 new 操作符创建一个我们刚才定义的误差类的对象。然后用指针作为参数创建 ceres::AutoDiffCostFunction 对象。
5.3 求解 ICP
#include <iostream>
#include <cmath>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
using namespace std;
int main() {
cout << setiosflags(ios::fixed) << setiosflags(ios::right) << setprecision(2);
// 1. 观测数据。
vector<Eigen::Vector3d> points_a = { {-4,2,1},{1,2,3},{1,3,2},{2,1,1},{-1,4,2},{7,0,3} };
vector<Eigen::Vector3d> points_b = { {2,3,1},{2,-2,3},{3,-2,2},{1,-3,1},{4,0,2},{0,-8,3} };
// 2.待优化的参数。
double para_q[4] = { 0,0,0,1 };
double para_t[3] = { 0,0,0 };
// 3.定义问题和损失函数，添加残差。
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
ceres::LossFunction* loss_function = new ceres::HuberLoss(0.1);
// 3.1添加优化对象。
ceres::LocalParameterization* q_parameterization = new ceres::EigenQuaternionParameterization();
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);
// 3.2.定义损失函数，添加残差。
for (size_t i = 0; i < points_a.size(); i++) {
ceres::CostFunction* cost_function = create(points_a[i], points_b[i]);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
}
// 4.配置求解器，优化并输出日志。
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 10;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout << summary.BriefReport() << endl;
// 5.获取优化结果。
cout << para_q[0] << " " << para_q[1] << " " << para_q[2] << " " << para_q[3] << endl;
cout << para_t[0] << " " << para_t[1] << " " << para_t[2] << endl;
// 6.四元数转旋转矩阵。
Eigen::Quaterniond q(Eigen::Vector4d(para_q[0], para_q[1], para_q[2], para_q[3]));
Eigen::Matrix3d R = q.toRotationMatrix();
cout << R << endl;
}
其中 3.1 部分不是必须的，可以直接删掉。这部分的内容可以查找资料看一下。
6.协方差
ceres::Covariance
7. 参考
- Ceres-Solver 下载，github。
- Ceres-Solver 安装，CSDN。
- Ceres-Solver 编译错误 “(”:“::”右边的非法标记，CSDN。
- Ceres-Solver 三种求导，博客园。
- Ceres 官方曲线拟合、损失函数的作用，博客园，2021。
- Ceres 三种求导方法，古月居，2021。
- Ceres求解直接法BA实现自动求导，CSDN，2020。
- Ceres Solver 协方差的求解思路，CSDN，2022。
- Ceres学习笔记建模篇，CSDN，2022。
851
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
