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中经典的重投影误差为例，分析仿函数的典型定义方法。

这里由于我们只是举一个简单的例子，因此忽略相机坐标系向像素坐标系的转换过程，将重投影误差定义为归一化图像坐标系下的平面位置差：
$$ϵ=\mathbf{u}-\mathbf{T}\mathbf{P}$$
该误差函数对应的仿函数如下（这里采用的是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()统一添加和管理，而已知参数则在仿函数创建时通过构造函数传入，若优化问题没有已知参数，则不需要编写构造函数。在本例中，已知参数是相机坐标系下的特征点二维坐标$\mathbf{p}={\left[u,v\right]}^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对象指针。