Ceres详解(二) CostFunction
代价函数CostFunction
与其他非线性优化工具包一样,ceres的性能很大程度上依赖于导数计算的精度和效率。这部分工作在ceres中称为CostFunction
,ceres提供了许多种CostFunction
模板,较为常用的包括以下三种:
- 自动导数(
AutoDiffCostFunction
):由ceres自行决定导数的计算方式,最常用的求导方式。 - 数值导数(
NumericDiffCostFunction
):由用户手动编写导数的数值求解形式,通常在残差函数的计算使用无法直接调用的库函数,导致调用AutoDiffCostFunction
类构建时使用;但手动编写的精度和计算效率不如模板类,因此不到不得已,官方并不建议使用该方法。 - 解析导数(Analytic Derivatives):当导数存在闭合解析形式时使用,用于可基于
CostFunciton
基类自行编写;但由于需要自行管理残差和雅克比矩阵,除非闭合解具有具有明显的精度和效率优势,否则同样不建议使用。
可以看出,ceres官方极力推荐用户使用自动求导方式AutoDiffCostFunction
,这里也主要以AutoDiffCostFunction
为例说明。AutoDiffCostFunction
为模板类,构造函数如下:
ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);
模板参数依次为仿函数(functor)类型CostFunctor
,残差维数residualDim
和参数维数paramDim
,接受参数类型为仿函数指针CostFunctor*
。
仿函数CostFunctor
仿函数的本质为结构体struct
或者类class
,由于重载了()
运算符,使得其能够具有和函数一样的调用行为,因此被称为仿函数。ceres中采用仿函数来表示残差的计算过程,这里我们以SLAM中经典的重投影误差为例,分析仿函数的典型定义方法。
这里由于我们只是举一个简单的例子,因此忽略相机坐标系向像素坐标系的转换过程,将重投影误差定义为归一化图像坐标系下的平面位置差:
ϵ
=
u
−
T
P
\epsilon=\bm{u}-\bm{T}\bm{P}
ϵ=u−TP
该误差函数对应的仿函数如下(这里采用的是struct
定义方式,采用class
只需要按对应格式重写即可。),主要包括构造函数、重载操作符()
和工厂函数三部分:
struct Reprojection{
// 构造函数,传递用于计算误差的测量值
Reprojection(double observed_u, double observed_v):
observed_u_(observed_u), observed_v_(observed_v){}
// 重载()操作符,用于计算误差
template <typename T>
bool operator()(const T* const camera, const T* const pt3_w, T* residual)const
{
T pt3_c[3];//3d coordinates in camera frame
// conversion from world to camera coordinates
T rvec[3] = {camera[0], camera[1], camera[2]};
ceres::AngleAxisRotatePoint(rvec, pt3_w, pt3_c);
pt3_c[0] += camera[3];
pt3_c[1] += camera[4];
pt3_c[2] += camera[5];
// perspective division (normalization)
T x_normalized = -pt3_c[0] / pt3_c[2];
T y_normalized = -pt3_c[1] / pt3_c[2];
residual[0]=observed_u - x_normalized;
residual[1]=observed_v - y_normalized;
return true;
}
// 工厂模式函数
static ceres::CostFunction* create(const double observed_u, const double observed_v){
return(new ceres::AutoDiffCostFunction<ReprojectionError3D, 2, 9, 3>(
new Reprojection(observed_u,observed_v)));
}
// 用于存储测量值的成员变量
double observed_u_;
double observed_v_;
};
构造函数(可选)
误差函数中的参数包括已知参数和待优化参数两部分,其中待优化参数由Problem::AddResidualBlock()
统一添加和管理,而已知参数则在仿函数创建时通过构造函数传入,若优化问题没有已知参数,则不需要编写构造函数。在本例中,已知参数是相机坐标系下的特征点二维坐标
p
=
[
u
,
v
]
T
\bm{p}=\left[u,v\right]^T
p=[u,v]T,因此构造函数接受两个double
型数据,并将其存入结构体的成员变量observed_u
和observed_v
中。
重载操作符()
(必有)
操作符()
是一个模板方法,返回值为bool
型,接受参数为待优化变量和残差变量。待优化变量的传入方式应和Probelm::AddResidualBlock()
一致,即若Probelm::AddResidualBlock()
中一次性传入变量数组指针,此处亦应该一次性传入变量数组指针;若Probelm::AddResidualBlock()
变量是依次传入,此处亦应该依次传入,且保证变量传入顺序一致。同时需要注意的是,该操作符的输入和输出变量统一为模板类型T
,由于在编程过程中我们会使用大量的开源算法库,而这些算法库具有自己独有的数据类型且各不相同(例如Eigen的矢量为Vector
类型,矩阵为Matrix
类型;OpenCV矢量为Point
类型,矩阵为Mat
类型),在传入矢量或矩阵时需要尤其注意这一点(一般统一转化为double
型数组)。
由于在优化过程中,我们不希望因为程序的误操作导致操作符()
重载的内容被修改,因此需要为函数体加上const
关键字修饰。同理,在残差的计算过程中,为了避免除ceres优化之外的误操作引起待优化变量的改变,需要同时使用const
关键字修饰参数类型和参数名保证类型和内容均不变;而residual
只需要保证类型不变,参数每次都是可变的,因此只需要使用const
修饰类型T
即可。
工厂函数(可选)
对于每一个新的量测来说,CostFunction的构造方式是完全一致的,为了避免每次重复创建实例和析构实例,可以采用工厂模式,即该类提供一个静态的成员函数用于创建CostFunction对象指针。本例中create()
函数接受量测信息observed_u
和ovserved_v
并返回一个AutoDiffCostFunction
对象指针。