【ceres】【ceres实践】【ceres的使用学习记录】
【ceres】【ceres实践】【ceres的使用学习记录】
- 0 前言
- 1 下载安装ceres
- 2 ceres使用
-
- 2.1 头文件的使用
- 2.2 CMakeLists.txt的使用
- 2.3 代码的使用
-
- 2.3.1 简单例子
-
- 2.3.1.1 代价函数的计算模型
- 2.3.1.2 构建最小二乘问题
-
- 2.3.1.2.1 自动求导
- 2.3.1.3 配置求解器
- 2.3.1.4 配置优化信息
- 2.3.1.5 开始优化
- 2.5.1.6 允许输出结果
- 2.3.2 使用ceres优化pnp
-
- 2.3.2.1 代价函数的计算模型
- 2.3.2.2 输入参数初始化
- 2.3.2.3 构建最小二乘问题
-
- 2.3.2.3.1 自动求导
- 2.3.2.4 配置求解器
- 2.3.2.5 开始优化
- 2.3.3 使用ceres实现BA
-
- 2.3.3.1 构建ceres最小二乘问题
-
- 2.3.3.1.1 代价函数
-
- 2.3.3.1.1.1 残差函数构建
- 2.3.3.1.2 损失函数
- 2.3.3.1.3 问题定义
- 2.3.3.2 配置函数设置（稀疏和边缘化）
- 2.3.3.3 进行优化
- 3 ceres的说明
-
- 3.1 ceres使用说明理解
0 前言
1 下载安装ceres
- 参考【slam十四讲第二版】【课本例题代码向】【第六讲~非线性优化】【安装对应版本ceres2.0.0和g2o教程】【手写高斯牛顿、ceres2.0.0、g2o拟合曲线及报错解决方案】的2.1 需要安装ceres
- 参考：
- 优化库——ceres（二）深入探索ceres::Problem
2 ceres使用
2.1 头文件的使用
- 必需的
#include 2.2 CMakeLists.txt的使用
- 好像没有也可以，但是如果实在编译不通过，可以加上试试，文件自取链接: https://pan.baidu.com/s/1hT8TDvk0qLSEkYEOjEU7oQ 提取码: 0sro
# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
- 必需的
# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )
add_executable( xxx src/xxx.cpp )
# 与Ceres和OpenCV链接
target_link_libraries( xxx ${CERES_LIBRARIES} )
2.3 代码的使用
2.3.1 简单例子
- 参考例程为【slam十四讲第二版】【课本例题代码向】【第六讲~非线性优化】【安装对应版本ceres2.0.0和g2o教程】【手写高斯牛顿、ceres2.0.0、g2o拟合曲线及报错解决方案】2 使用ceres进行曲线拟合
2.3.1.1 代价函数的计算模型
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维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理，而不是设3个变量，之后在()重载函数的形式参数个数变为3个
T* residual ) const // 残差
{
residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y数据
};
2.3.1.2 构建最小二乘问题
2.3.1.2.1 自动求导
- 程序中的double abc[3]即参数块，而对于残差块，我们对于每一个数据构造
CURVE_FITTING_COST对象，然后调用ADDResiduallock将误差项添加到目标函数中。由于优化需要梯度，我们有若干种选择：
- （1）使用ceres的自动求导（Auto Diff）；
- （2）使用数值求导（Numeric Diff）；
- （3）自行推导解析的导数形式，提供给Ceres。
- 自动求导需要指定误差项和优化变量的纬度。这里的误差是标量，纬度为1；优化的是a、b、c三个量，纬度为3。于是，在自动求导类
AutoDiffCostFunction的模板参数中设定变量纬度为1，3 - 设定好问题之后，调用Solve函数进行求解。你可以在iotions里配置（非常详细的）优化选项。例如，可以选择用Line Search还是Trust Region、迭代次数、步长等。
- 两种方式都可以，可以好好理解下
// 构建最小二乘问题
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
/* 第一个参数 CostFunction* : 描述最小二乘的基本形式即代价函数 例如书上的137页fi(.)的形式
50 * 第二个参数 LossFunction* : 描述核函数的形式 例如书上的ρi（.）
51 * 第三个参数 double* : 待估计参数（用数组存储）
52 * 这里仅仅重载了三个参数的函数，如果上面的double abc[3]改为三个double a=0 ,b=0,c = 0;
53 * 此时AddResidualBlock函数的参数除了前面的CostFunction LossFunction 外后面就必须加上三个参数 分别输入&a,&b,&c
54 * 那么此时下面的 ceres::AutoDiffCostFunction<>模板参数就变为了 后面三个1代表有几类未知参数
55 * 我们修改为了a b c三个变量，所以这里代表了3类，之后需要在自己写的CURVE_FITTING_COST类中的operator()函数中，
56 * 把形式参数变为了 const T* const a, const T* const b, const T* const c ,T* residual
57 * 上面修改的方法与本例程实际上一样，只不过修改的这种方式显得乱，实际上我们在用的时候，一般都是残差种类有几个，那么后面的分类 就分几类
58 * 比如后面讲的重投影误差，此事就分两类 一类是相机9维变量，一类是点的3维变量，然而残差项变为了2维
59 *
60 * （1）: 修改后的写法（当然自己定义的代价函数要对应修改重载函数的形式参数，对应修改内部的残差的计算）：
61 * ceres::CostFunction* cost_function
62 * = new ceres::AutoDiffCostFunction(
63 * new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
64 * problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
65 * 修改后的代价函数的计算模型:
66 * struct CURVE_FITTING_COST
67 * {
68 * CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
69 * // 残差的计算
70 * template
71 * bool operator() (
72 * const T* const a,
73 * const T* const b,
74 * const T* const c,
75 * T* residual ) const // 残差
76 * {
77 * residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
78 * return true;
79 * }
80 * const double _x, _y; // x,y数据
81 * };//代价类结束
82 *
83 *
84 * （2）: 本例程下面的语句通常拆开来写(看起来方便些):
85 * ceres::CostFunction* cost_function
86 * = new ceres::AutoDiffCostFunction(
87 * new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
88 * problem.AddResidualBlock(cost_function,nullptr,abc)
89 * */
problem.AddResidualBlock ( // 向问题中添加误差项
// 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr, // 核函数，这里不使用，为空
abc // 待估计参数
);
}
2.3.1.3 配置求解器
// 配置求解器 ceres::Solver (是一个非线性最小二乘的求解器)
ceres::Solver::Options options;// 这里有很多配置项可以填 Options类嵌入在Solver类中 ，在Options类中可以设置关于求解器的参数
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
options.max_num_iterations = 4;//迭代次数
options.minimizer_progress_to_stdout = true; // 输出到cout 为真时 内部错误输出到cout，我们可以看到错误的地方，默认情况下，会输出到日志文件中保存
2.3.1.4 配置优化信息
ceres::Solver::Summary summary; // 优化信息
2.3.1.5 开始优化
/*下面函数需要3个参数：
110 * 1、 const Solver::Options& options <----> options
111 * 2、 Problem* problem <----> &problem
112 * 3、 Solver::Summary* summary <----> &summart (即使默认的参数也需要定义该变量 )
113 * 这个函数会输出一些迭代的信息。
114 * */
ceres::Solve ( options, &problem, &summary ); // 开始优化
2.5.1.6 允许输出结果
- 输出优化系统信息结果
// 输出结果
cout<<summary.BriefReport() <<endl;
输出为：
generating data:
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.824887e+04 0.00e+00 1.38e+03 0.00e+00 0.00e+00 1.00e+04 0 1.41e-05 5.20e-05
1 2.748700e+39 -2.75e+39 1.38e+03 7.67e+01 -1.52e+35 5.00e+03 1 1.60e-05 8.61e-05
2 2.429783e+39 -2.43e+39 1.38e+03 7.62e+01 -1.35e+35 1.25e+03 1 7.87e-06 9.89e-05
3 1.213227e+39 -1.21e+39 1.38e+03 7.30e+01 -6.73e+34 1.56e+02 1 7.15e-06 1.11e-04
4 1.852387e+37 -1.85e+37 1.38e+03 5.56e+01 -1.03e+33 9.77e+00 1 5.96e-06 1.22e-04
5 6.714689e+31 -6.71e+31 1.38e+03 2.96e+01 -3.85e+27 3.05e-01 1 6.20e-06 1.32e-04
6 9.500531e+12 -9.50e+12 1.38e+03 9.50e+00 -8.39e+08 4.77e-03 1 6.20e-06 1.42e-04
7 1.776982e+04 4.79e+02 1.83e+03 2.58e-01 1.18e+00 1.43e-02 1 1.81e-05 1.64e-04
8 1.599969e+04 1.77e+03 3.45e+03 5.53e-01 1.46e+00 4.29e-02 1 1.50e-05 1.83e-04
9 1.060557e+04 5.39e+03 7.62e+03 7.33e-01 1.68e+00 1.29e-01 1 1.50e-05 2.02e-04
10 3.669783e+03 6.94e+03 9.60e+03 5.25e-01 1.39e+00 3.86e-01 1 1.50e-05 2.21e-04
11 5.397541e+02 3.13e+03 5.00e+03 2.66e-01 1.12e+00 1.16e+00 1 1.50e-05 2.39e-04
12 1.484444e+02 3.91e+02 1.22e+03 8.46e-02 1.02e+00 3.48e+00 1 1.50e-05 2.57e-04
13 1.216815e+02 2.68e+01 3.76e+02 4.17e-02 1.01e+00 1.04e+01 1 1.41e-05 2.74e-04
14 9.290109e+01 2.88e+01 2.42e+02 9.10e-02 1.01e+00 3.13e+01 1 1.50e-05 2.93e-04
15 6.674330e+01 2.62e+01 1.09e+02 1.33e-01 1.00e+00 9.39e+01 1 1.38e-05 3.11e-04
16 5.936574e+01 7.38e+00 2.14e+01 1.08e-01 9.94e-01 2.82e+02 1 1.50e-05 3.30e-04
17 5.653118e+01 2.83e+00 1.36e+01 1.57e-01 9.98e-01 8.45e+02 1 1.41e-05 3.48e-04
18 5.310764e+01 3.42e+00 8.50e+00 2.81e-01 9.89e-01 2.53e+03 1 1.50e-05 3.67e-04
19 5.125939e+01 1.85e+00 2.84e+00 2.98e-01 9.90e-01 7.60e+03 1 1.50e-05 3.85e-04
20 5.097693e+01 2.82e-01 4.34e-01 1.48e-01 9.95e-01 2.28e+04 1 1.48e-05 4.04e-04
21 5.096854e+01 8.39e-03 3.24e-02 2.87e-02 9.96e-01 6.84e+04 1 1.50e-05 4.22e-04
solve time cost = 0.00044111 seconds.
Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e+04, Final cost: 5.096854e+01, Termination: CONVERGENCE
estimated a,b,c = 0.891943 2.17039 0.944142
进程已结束,退出代码0
- 自定义输出结果
cout<<"estimated a,b,c = ";
/*auto a:abc 或者下面的方式都可以*/
for ( auto a:abc ) cout<<a<<" ";
输出为：
estimated a,b,c = 0.891943 2.17039 0.944142
2.3.2 使用ceres优化pnp
- 参考【slam十四讲第二版】【课后习题】【第七讲~视觉里程计Ⅰ】的1.10.1 在Ceres 中实现PnP
- 由于步骤基本和2.3.1一样，所以这里仅仅显示出不一样的部分进行说明
2.3.2.1 代价函数的计算模型
//第一步：定义Cost Function函数
struct cost_function
{
cost_function(Point3f p_3D,Point2f p_p):_p_3D(p_3D),_p_p(p_p) {}//3D-2D：知道n个3D空间点以及其投影位置，然后估计相机位姿
//计算残差
template <typename T>//模板：使得在定义时可以模糊类型
bool operator() (
const T* const r, const T* const t,//r,t为待优化的参数
const T* K,//k为待优化的参数
T* residual ) const //殘差
{
T p_3d[3];
T p_Cam[3];//相机坐标系下空间点的坐标
p_3d[0]=T(_p_3D.x);
p_3d[1]=T(_p_3D.y);
p_3d[2]=T(_p_3D.z);
// 通过tool文件夹中的rotation.h中的AngleAxisRotatePoint()函数
// 计算在相机仅旋转的情况下，新坐标系下的坐标
// 也就是p_Cam=R*p_3d
//cout<<"point_3d: "<
AngleAxisRotatePoint(r,p_3d,p_Cam);
p_Cam[0]=p_Cam[0]+t[0];
p_Cam[1]=p_Cam[1]+t[1];
p_Cam[2]=p_Cam[2]+t[2];
//R,t计算T
//Eigen::Isometry3d T_w_c;
// T_w_c.rotate(r);
const T x=p_Cam[0]/p_Cam[2];
const T y=p_Cam[1]/p_Cam[2];
//3D点投影后的像素坐标
// const T u=x*520.9+325.1;
// const T v=y*521.0+249.7;
const T u=x*K[0]+K[1];
const T v=y*K[2]+K[3];
//观测到的投影位置的像素坐标
T u1=T(_p_p.x);
T v1=T(_p_p.y);
//残差
residual[0]=u-u1;
residual[1]=v-v1;
return true;
}
Point3f _p_3D;
Point2f _p_p;
};
2.3.2.2 输入参数初始化
// cout<<"R = "<
cout<<"start:"<<endl;
double rotation_vector[3],tranf[3];//旋转向量r，平移t
double k[4];
rotation_vector[0]=r.at<double>(0,0);
rotation_vector[1]=r.at<double>(0,1);
rotation_vector[2]=r.at<double>(0,2);
tranf[0]=t.at<double>(0,0);
tranf[1]=t.at<double>(1,0);
tranf[2]=t.at<double>(2,0);
k[0]=520.9;
k[1]=325.1;
k[2]=521.0;
k[3]=249.7;
2.3.2.3 构建最小二乘问题
2.3.2.3.1 自动求导
ceres::Problem problem;
for(int i=0;i<points_3d.size();++i)
{
ceres::CostFunction* costfunction=new ceres::AutoDiffCostFunction<cost_function,2,3,3,4>(new cost_function(points_3d[i],points_2d[i]));
problem.AddResidualBlock(costfunction,NULL,rotation_vector,tranf,k);
}
2.3.2.4 配置求解器
//配置求解器
ceres::Solver::Options option;
option.linear_solver_type=ceres::DENSE_QR;//DENSE_SCHUR
//true:迭代信息输出到屏幕.false:不输出
option.minimizer_progress_to_stdout=true;
ceres::Solver::Summary summary;//优化信息
//可以和g2o优化做对比
2.3.2.5 开始优化
//开始优化
ceres::Solve(option,&problem,&summary);
2.3.3 使用ceres实现BA
- 参考【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】【安装Meshlab】【BAL数据集格式】【】的3 使用ceres实现BA
2.3.3.1 构建ceres最小二乘问题
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i)
{
ceres::CostFunction *cost_function;
// Each Residual block takes a point and a camera as input 每个残差块以一个点和一个相机作为输入
// and outputs a 2 dimensional Residual 输出二维残差
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);//调用SnavelyReprojectionError.h c文件
//等价于下式
//ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction(
// new SnavelyReprojectionError(observed_x, observed_y));
// If enabled use Huber's loss function. 如果启用，请使用Huber的损失函数。
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
// Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
// 每个观测点对应于一对相机和一个点，分别由camera_index()[i]和point_index()[i]标识。
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
//camera_block_size = bal_problem.camera_block_size();
//*camera = cameras + bal_problem.camera_block_size() * bal_problem.camera_index()[i]
double *point = points + point_block_size * bal_problem.point_index()[i];
//point_block_size = bal_problem.point_block_size();
//*point = points + bal_problem.point_block_size() * bal_problem.point_index()[i]
problem.AddResidualBlock(cost_function, loss_function, camera, point); // 向问题中添加误差项
//CostFunction* : 描述最小二乘的基本形式即代价函数
//LossFunction* : 描述核函数的形式
}
2.3.3.1.1 代价函数
// Each Residual block takes a point and a camera as input 每个残差块以一个点和一个相机作为输入
// and outputs a 2 dimensional Residual 输出二维残差
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);//调用SnavelyReprojectionError.h c文件
//等价于下式
//ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction(new SnavelyReprojectionError(observed_x, observed_y));
- 等价于
ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction
2.3.3.1.1.1 残差函数构建
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include 2.3.3.1.2 损失函数
// If enabled use Huber's loss function. 如果启用，请使用Huber的损失函数。
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
2.3.3.1.3 问题定义
camera和point为输入的参数
// Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
// 每个观测点对应于一对相机和一个点，分别由camera_index()[i]和point_index()[i]标识。
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
//camera_block_size = bal_problem.camera_block_size();
//*camera = cameras + bal_problem.camera_block_size() * bal_problem.camera_index()[i]
double *point = points + point_block_size * bal_problem.point_index()[i];//得到的是对应路标点在世界坐标系下的三维坐标
//point_block_size = bal_problem.point_block_size();
//*point = points + bal_problem.point_block_size() * bal_problem.point_index()[i]
problem.AddResidualBlock(cost_function, loss_function, camera, point); // 向问题中添加误差项
//CostFunction* : 描述最小二乘的基本形式即代价函数
//LossFunction* : 描述核函数的形式
2.3.3.2 配置函数设置（稀疏和边缘化）
- 使用
SPARSE_SCHUR会让先对路标部分进行Schur边缘化，以加速的方式求解此问题 - 但是在ceres中我们不能控制哪部分变量被边缘化，这是由Ceres求解器自动寻找并计算的
ceres::Solver::Options options; // 这里有很多配置项可以填Options类嵌入在Solver类中 ，在Options类中可以设置关于求解器的参数
options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
// 增量方程如何求解 这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
//使用Schur消元
默认情况下，最小进度记录到VLOG（1），根据VLOG级别将其发送到STDERR。如果此标志设置为true，并且logging_type不是静默的，则日志输出将发送到STDOUT。
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;// 优化信息
2.3.3.3 进行优化
ceres::Solve(options, &problem, &summary);
3 ceres的说明
- ceres官网说明
3.1 ceres使用说明理解
在某些情况下，无法使用自动求导。例如，以封闭形式计算导数而不是依赖于自动求导代码使用的链式规则可能会更有效。
在这种情况下，可以提供您自己的残差和雅可比计算代码。为此，请定义一个子类， CostFunction或者SizedCostFunction如果您在编译时知道参数和残差的大小。例如，这里是SimpleCostFunction实现f(x) = 10-x.
class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> { //残差1维度；优化的变量1维度
public:
virtual ~QuadraticCostFunction() {}
// 重载虚函数
virtual bool Evaluate(double const* const* parameters, // 各个参数块，一个参数块定义为一个参数数组，好多参数块就是一个数组的数组，也就是这里的双指针
double* residuals, //通过这个参数块和已知的变量如何计算这个残差，残差是一个向量，所以这里是一个一维数组
double** jacobians) const { //雅可比矩阵大小是一个（残差*参数块总数的矩阵），是一个二维数组
const double x = parameters[0][0]; //第1个参数块的第1个参数
residuals[0] = 10 - x; // 计算残差
// Compute the Jacobian if asked for.
if (jacobians != nullptr && jacobians[0] != nullptr) {
jacobians[0][0] = -1;// 雅可比计算 ， 由于残差维度是1，参数块也有1个，所以这里定义1*1即可
}
return true;
}
};