Ceres学习笔记应用篇001_Ceres Solver介绍、基本使用步骤及一个简单例子

1 Ceres Solver介绍

Ceres Solver是由Google开发的开源C++库，用于解决具有边界约束的非线性最小二乘优化和一般无约束优化问题，成熟、功能丰富、高性能。

与一般优化问题不同的是，非线性最小二乘优化问题的目标函数具有明确的物理意义——残差。

具有边界约束的非线性最小二乘鲁棒优化问题形式如下：

$\underset{x}{\min }\frac{1}{2}{\sum }_i{\rho }_i(\parallel f_i(x_{i1},\cdots ,x_{ik}){\parallel }^2),l_j\le x_j\le u_j$ （1）

• $(x_{i1},\cdots ,x_{ik})$在Ceres中被称为参数块（ParameterBlock），通常是几组标量的集合，例如，相机的位姿可以定义成是一组包含3个参数的平移向量（用于描述相机的位置），和包含4个参数的四元数（用于描述相机姿态），当然，参数块也可以只有一个参数，$l_j$和$u_j$是参数块中对应每个参数的边界；
• $f_i(\cdot )$在Ceres中被称为代价函数（CostFuntion），是关于参数块的函数，在一个优化问题中，可能会存在多个代价函数；
• ${\rho }_i(\cdot )$在Ceres中被称为损失函数（LossFuntion），是一个标量函数，将代价函数计算出的值映射到另一个区间中的值，用于减少异常值或外点（outliers）对非线性最小二乘优化问题的影响，作用有点类似于机器学习中的激活函数，例如，直线拟合时，对于距离直线非常远的点，应当减少它的权重，损失函数并非是必须的，可以为空（NULL），此时，损失函数值等同于代价函数计算值，即${\rho }_i(t)=t$；

当损失函数为空，且参数没有边界时，就是我们熟悉的非线性最小二乘问题，如下：

$\underset{x}{\min }\frac{1}{2}{\sum }_i\parallel f_i(x_{i1},\cdots ,x_{ik}){\parallel }^2,l_j=-\infty ,u_j=\infty$ （2）
一般情况下，最小二乘问题与鲁棒最小二乘问题的区别在于鲁棒最小二乘会指定损失函数，具体效果在后续的有关曲线拟合的学习笔记中会有所体现。

• ${\rho }_i(\parallel f_i(x_{i1},\cdots ,x_{ik}){\parallel }^2)$在Ceres中被称为残差块（ResidualBlock），残差块中包含了参数块、代价函数、损失函数，因此，在添加残差块时，必须指定参数集合、代价函数，视具体情况是否指定损失函数。

统计学中的曲线拟合、计算机视觉中的相机标定、视觉SLAM中的地图生成等问题都可以描述成以上形式。

2 Ceres使用基本步骤

Ceres求解过程主要有两大步骤，构建最小二乘问题和求解最小二乘问题，具体步骤如下：
一、 构建最小二乘问题

1. 用户自定义残差计算模型，可能存在多个；
2. 构建Ceres代价函数（CostFuntion），将用户自定义残差计算模型添加至CostFuntion，可能存在多个CostFuntion，为每个CostFuntion添加用户自定义残差计算模型，并指定用户自定义残差计算模型的导数计算方法；
3. 构建Ceres问题（Problem），并在Problem中添加残差块（ResidualBlock），可能存在多个ResidualBlock，为每个ResidualBlock指定CostFuntion，LossFuntion以及参数块（ParameterBlock）；

二、 求解最小二乘问题

1. 配置求解器参数Options，即设置Problem求解方法及参数。例如迭代次数、步长等等；
2. 输出日志内容Summary；
3. 优化求解Solve。
   

3 一个简单的例子HelloWorld

3.1 HelloWorld_auto_diff

以求解如下函数的最小值为例：

$\frac{1}{2}(10-x{)}^2$ （3）
对于求解该函数的最小值问题，可以构建成一个非常简单的优化问题，虽然一眼就能看出 x = 10时函数能够获取最小值，但以此为例，可以说明使用 Ceres解决一般优化问题或者非线性最小二乘问题的基本步骤。

3.1.1 用户自定义残差计算模型

// 用户自定义残差计算模型
struct MyCostFunctorAutoDiff 
{
	// 模板函数
	template<typename Type>
	bool operator()(const Type* const x, Type* residual) const
	{
		// 输入参数x和输出参数residual都只有1维
		residual[0] = 10.0 - x[0];
		return true;
	}
};

注意，operator()是一个模板函数，输入和输出的参数类型都是Type类型，当仅需要获得残差值作为输出时，Ceres在调用MyCostFunctorAutoDiff::operator()时可以指定Type的类型为double，当需要获得Jacobians值（微分或导数）作为输出时，Ceres在调用MyCostFunctorAutoDiff::operator()时可以指定Type的类型为Jet，后续会有更详细的介绍。

3.1.2 构建Ceres代价函数CostFuntion

// 构建Ceres代价函数CostFuntion，用来计算残差，残差计算方法为用户自定义残差计算模型MyCostFunctorAutoDiff
// 本例中使用自动微分方法AutoDiffCostFunction来计算导数
// AutoDiffCostFunction模板参数中，需要依次指定
// 用户自定义残差计算模型MyCostFunctorAutoDiff、输出（resudual）维度大小、输入（参数x）维度大小
// 这两个维度大小需要与残差计算模型中输入、输出参数的维度一致，本例中对应residual[0]和x[0]
// 本例中只存在一个代价函数
ceres::CostFunction* cost_function =
    new ceres:: AutoDiffCostFunction<MyCostFunctorAutoDiff, /* 用户自定义残差计算模型 */\
    1, /* 输出（resudual）维度大小 */\
    1 /* 输入（参数x）维度大小 */>(new MyCostFunctorAutoDiff);

说明如下：

• 本例中只存在一个代价函数；
• 本例中使用自动微分方法来计算导数；
• AutoDiffCostFunction模板参数中，需要依次指定用户自定义残差计算模型MyCostFunctorAutoDiff、输出（resudual）维度大小、输入（参数x）维度大小，这两个维度大小需要与残差计算模型中输入、输出参数的维度一致，本例中对应residual[0]和x[0]。
  

3.1.3 构建Ceres问题Problem

// 构建非线性最小二乘问题
ceres::Problem problem;
// 添加残差块，需要依次指定代价函数，损失函数，参数块
// 本例中损失函数为单位函数
problem.AddResidualBlock(cost_function, nullptr, &x);

说明如下：

• 添加残差块ResidualBlock时，需要依次指定代价函数CostFunction，损失函数LossFunction（本例中损失函数为单位函数），参数块ParameterBlock;
• 本例中只添加一项残差块。
  

3.1.4 配置求解器参数Options

// 配置求解器参数
ceres::Solver::Options options;
// 指定线性求解器来求解问题
options.linear_solver_type = ceres::DENSE_QR;
// 输出每次迭代的信息
options.minimizer_progress_to_stdout = true;

3.1.5 输出日志内容Summary

// 输出日志内容
ceres::Solver::Summary summary;

3.1.6 优化求解

// 开始优化求解
ceres::Solve(options, &problem, &summary);

3.1.7 完整过程及结果

#include "ceres/ceres.h"
#include "glog/logging.h"
#include 

// 用户自定义残差计算模型
struct MyCostFunctorAutoDiff 
{
	// 模板函数
	template<typename Type>
	bool operator()(const Type* const x, Type* residual) const
	{
		// 输入参数x和输出参数residual都只有1维
		residual[0] = 10.0 - x[0];
		return true;
	}
};

int main(int argc, char** argv)
{
	google::InitGoogleLogging(argv[0]);
	// 设置参数初始值
	const double initial_x = 0.5;
	double x = initial_x;

	// 构建Ceres代价函数CostFuntion，用来计算残差，残差计算方法为用户自定义残差计算模型MyCostFunctorAutoDiff
	// 本例中使用自动微分方法AutoDiffCostFunction来计算导数
	// AutoDiffCostFunction模板参数中，需要依次指定
	// 用户自定义残差计算模型MyCostFunctorAutoDiff、输出（resudual）维度大小、输入（参数x）维度大小
	// 这两个维度大小需要与残差计算模型中输入、输出参数的维度一致，本例中对应residual[0]和x[0]
	// 本例中只存在一个代价函数
    ceres::CostFunction* cost_function =
        new ceres:: AutoDiffCostFunction<MyCostFunctorAutoDiff, /* 用户自定义残差计算模型 */\
        1, /* 输出（resudual）维度大小 */\
        1 /* 输入（参数x）维度大小 */>(new MyCostFunctorAutoDiff);

	// 构建非线性最小二乘问题
	ceres::Problem problem;
	// 添加残差块，需要依次指定代价函数，损失函数，参数块
	// 本例中损失函数为单位函数
	problem.AddResidualBlock(cost_function, nullptr, &x);

	// 配置求解器参数
	ceres::Solver::Options options;
	// 指定线性求解器来求解问题
	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";
	std::cout << "x : " << initial_x << " -> " << x << "\n";

	std::system("pause");
	return 0;
}

优化过程及结果如下：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.512500e+01    0.00e+00    9.50e+00   0.00e+00   0.00e+00  1.00e+04        0    4.88e-04    8.55e-04
   1  4.511598e-07    4.51e+01    9.50e-04   9.50e+00   1.00e+00  3.00e+04        1    8.34e-04    2.41e-03
   2  5.012552e-16    4.51e-07    3.17e-08   9.50e-04   1.00e+00  9.00e+04        1    2.11e-04    2.74e-03
Ceres Solver Report: Iterations: 3, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
x : 0.5 -> 10

与大多数优化软件包一样，Ceres Solver需要能够在任意参数值下，正确计算目标函数中每一参数的值和导数，这样才能取得好的优化结果。
Ceres Solver提供了多种导数计算方法，3.1节使用的是自动微分来计算导数，还有两种导数计算方法：数值法和解析法。

3.2 HelloWorld_numeric_diff

在某些情况下，很难在用户自定义残差计算模型中，定义一个模板函数来计算残差，例如，当求解残差的过程涉及到调用第三方库函数时，无法对库函数进行求导。在这种情况下，可以使用数值微分法，用户可以在自定义的误差模型中通过任意手段定义一个普通函数来计算残差，并使用它构造Ceres损失函数NumericDiffCostFunction，例如：

3.2.1 用户自定义残差计算模型

// 第三方库函数
double fun(double x)
{
	return 10.0 - x;
}

// 用户自定义残差计算模型
struct MyCostFunctorNumericDiff
{
	bool operator()(const double* const x, double* residual) const
	{
		// 残差计算方法中调用了第三方库函数
		residual[0] = fun(x[0]);
		return true;
	}
};

3.2.2 构建Ceres代价函数CostFuntion

// 构建Ceres代价函数CostFuntion，用来计算残差，残差计算方法为用户自定义残差计算模型MyCostFunctorNumericDiff
// 本例中使用数值微分方法NumericDiffCostFunction来计算导数
// NumericDiffCostFunction模板参数中，需要依次指定
// 用户自定义残差计算模型MyCostFunctorAutoDiff、数值计算导数方法、输出（resudual）维度大小、输入（参数x）维度大小
// 这两个维度大小需要与残差计算模型中输入、输出参数的维度一致，本例中对应residual[0]和x[0]
// 本例中只存在一个代价函数
ceres::CostFunction* cost_function =
    new ceres::NumericDiffCostFunction<MyCostFunctorNumericDiff, /*用户自定义残差计算模型*/\
	ceres::CENTRAL, /*数值计算导数方法*/\
	1, /*输出（resudual）维度大小*/\
	1 /*输入（参数x）维度大小*/>(new MyCostFunctorNumericDiff);

与3.1节自动微分法计算导数相比，区别如下：

• 本例使用数值法计算导数；
• 数值法计算导数时，需要指定具体方法，本例中使用的是ceres::CENTRAL方法，导数计算过程如下：
  
  $f^{\prime }(x)\approx \frac{f(x+h)-f(x-h)}{2\ast h}$ （4）
  式中，$h\to 0$。
  通常更推荐使用自动微分法，因为C++模板使自动微分更高效，而数值微分计算更复杂，容易出现数值错误，导致收敛更慢。
  

3.2.3 完整代码及结果

#include "ceres/ceres.h"
#include "glog/logging.h"
#include 

// 第三方库函数
double fun(double x)
{
	return 10.0 - x;
}

// 用户自定义残差计算模型
struct MyCostFunctorNumericDiff
{
	bool operator()(const double* const x, double* residual) const
	{
		// 残差计算方法中调用了第三方库函数
		residual[0] = fun(x[0]);
		return true;
	}
};

int main(int argc, char** argv)
{
	google::InitGoogleLogging(argv[0]);
	// 设置参数初始值
	const double initial_x = 0.5;
	double x = initial_x;

	// 构建Ceres代价函数CostFuntion，用来计算残差，残差计算方法为用户自定义残差计算模型MyCostFunctorNumericDiff
	// 本例中使用数值微分方法NumericDiffCostFunction来计算导数
	// NumericDiffCostFunction模板参数中，需要依次指定
	// 用户自定义残差计算模型MyCostFunctorAutoDiff、数值计算导数方法、输出（resudual）维度大小、输入（参数x）维度大小
	// 这两个维度大小需要与残差计算模型中输入、输出参数的维度一致，本例中对应residual[0]和x[0]
	// 本例中只存在一个代价函数
    ceres::CostFunction* cost_function =
        new ceres::NumericDiffCostFunction<MyCostFunctorNumericDiff, /*用户自定义残差计算模型*/\
        ceres::CENTRAL, /*数值计算导数方法*/\
        1, /*输出（resudual）维度大小*/\
        1 /*输入（参数x）维度大小*/>(new MyCostFunctorNumericDiff);

	// 构建非线性最小二乘问题
	ceres::Problem problem;
	// 添加残差块，需要依次指定代价函数，损失函数，参数块
	// 本例中损失函数为单位函数
	problem.AddResidualBlock(cost_function, nullptr, &x);

	// 配置求解器参数
	ceres::Solver::Options options;
	// 指定线性求解器来求解问题
	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";
	std::cout << "x : " << initial_x << " -> " << x << "\n";

	std::system("pause");
	return 0;
}

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.512500e+01    0.00e+00    9.50e+00   0.00e+00   0.00e+00  1.00e+04        0    9.78e-04    1.81e-03
   1  4.511690e-07    4.51e+01    9.50e-04   9.50e+00   1.00e+00  3.00e+04        1    1.29e-03    3.93e-03
   2  5.012669e-16    4.51e-07    3.17e-08   9.50e-04   1.00e+00  9.00e+04        1    4.84e-04    4.69e-03
Ceres Solver Report: Iterations: 3, Initial cost: 4.512500e+01, Final cost: 5.012669e-16, Termination: CONVERGENCE
x : 0.5 -> 10

3.3 HelloWorld_Analytic_diff

在某些情况下，不太可能使用自动微分方法，例如，直接给出闭式解（解析解），即直接给出严格的导数计算公式（多项式、三角函数、指数、分数等基本函数的形式），将参数代入公式就能计算出导数的方式，会比使用链式法则自动微分求解更有效率，速度更快。
通过解析解计算导数的方式通常会比较繁琐，需要人工计算雅克比矩阵，因此，除非用户能够确保雅克比矩阵计算正确，否则，仍然建议使用自动微分法AutoDiffCostFunction或数值微分法NumericDiffFunction来构建残差模块。

3.3.1 用户自定义代价计算函数及导数计算方法

// 构建用户自定义代价计算函数MyCostFunction，用来计算残差，继承自SizedCostFunction
// 本例中使用用户给出的微分计算公式来计算导数
// SizedCostFunction<1, 1>模板参数中，需要依次指定
// 输出（resudual）维度大小、输入（参数块x）维度大小
// 本例中只有1个输入参数块，且该参数块的维度大小为1，输出（resudual）维度大小为1
// 本例中只存在一个代价函数
class MyCostFunction
    : public ceres::SizedCostFunction<1, /*输出（resudual）维度大小*/\
    1 /*输入参数块维度大小*/> {
public:
    virtual ~MyCostFunction() {}

    // 用户自定义残差计算方法
    virtual bool Evaluate(double const* const* parameters, /*输入参数块*/\
        double* residuals, /*输出残差*/\
        double** jacobians /*输出雅克比矩阵*/) const 
    {
        // 本例中只有1个输入参数
        double x = parameters[0][0];

        // 本例中只有1个输出参数
        residuals[0] = 10 - x;

        // 由于本例中输入和输出参数的维度都是1，因此雅克比矩阵的大小为1*1
        // Evaluate函数在雅克比矩阵为NULL的情况也能被调用，因此要验证是否需要计算雅克比矩阵
        // 本例中其实没必要验证jacobians[0]是否为空，因为本例只有1个参数块，总是要计算其导数，
        // 但通常当残差计算过程比较复杂时，有可能只需要针对指定参数块进行计算，
        // 那么，未指定的参数块，也即不需要进行优化的参数，其对应的雅克比是不需要计算的
        if (jacobians != NULL && jacobians[0] != NULL) 
        {
            jacobians[0][0] = -1;
        }
        return true;
    }
};

3.3.2 完整代码及结果

#include "ceres/ceres.h"
#include "glog/logging.h"

// 构建用户自定义代价计算函数MyCostFunction，用来计算残差，继承自SizedCostFunction
// 本例中使用用户给出的微分计算公式来计算导数
// SizedCostFunction<1, 1>模板参数中，需要依次指定
// 输出（resudual）维度大小、输入（参数块x）维度大小
// 本例中只有1个输入参数块，且该参数块的维度大小为1，输出（resudual）维度大小为1
// 本例中只存在一个代价函数
class MyCostFunction
    : public ceres::SizedCostFunction<1, /*输出（resudual）维度大小*/\
    1 /*输入参数块维度大小*/> {
public:
    virtual ~MyCostFunction() {}

    // 用户自定义残差计算方法
    virtual bool Evaluate(double const* const* parameters, /*输入参数块*/\
        double* residuals, /*输出残差*/\
        double** jacobians /*输出雅克比矩阵*/) const 
    {
        // 本例中只有1个输入参数
        double x = parameters[0][0];

        // 本例中只有1个输出参数
        residuals[0] = 10 - x;

        // 由于本例中输入和输出参数的维度都是1，因此雅克比矩阵的大小为1*1
        // Evaluate函数在雅克比矩阵为NULL的情况也能被调用，因此要验证是否需要计算雅克比矩阵
        // 本例中其实没必要验证jacobians[0]是否为空，因为本例只有1个参数块，总是要计算其导数，
        // 但通常当残差计算过程比较复杂时，有可能只需要针对指定参数块进行计算，
        // 那么，未指定的参数块，也即不需要进行优化的参数，其对应的雅克比是不需要计算的
        if (jacobians != NULL && jacobians[0] != NULL) 
        {
            jacobians[0][0] = -1;
        }
        return true;
    }
};

int main(int argc, char** argv) 
{
    google::InitGoogleLogging(argv[0]);

    // 设置参数初始值
    const double initial_x = 0.5;
    double x = initial_x;

    // 构建Ceres代价函数CostFuntion，用来计算残差
    // 残差计算方法为用户自定义代价计算函数MyCostFunction
    ceres::CostFunction* cost_function = new MyCostFunction;

    // 构建非线性最小二乘问题
    ceres::Problem problem;
    // 添加残差块，需要依次指定代价函数，损失函数，参数块
    // 本例中损失函数为单位函数
    problem.AddResidualBlock(cost_function, NULL, &x);

    // 配置求解器参数
    ceres::Solver::Options options;
    // 指定线性求解器来求解问题
    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";
    std::cout << "x : " << initial_x << " -> " << x << "\n";

    std::system("pause");
    return 0;
}

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.512500e+01    0.00e+00    9.50e+00   0.00e+00   0.00e+00  1.00e+04        0    4.75e-04    9.64e-04
   1  4.511598e-07    4.51e+01    9.50e-04   9.50e+00   1.00e+00  3.00e+04        1    8.46e-04    2.44e-03
   2  5.012552e-16    4.51e-07    3.17e-08   9.50e-04   1.00e+00  9.00e+04        1    2.69e-04    2.91e-03
Ceres Solver Report: Iterations: 3, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
x : 0.5 -> 10

使用 Ceres过程中，目前最复杂的部分就是计算导数，本节只是涉及到Ceres中导数计算中的皮毛，有时候用户可能需要更复杂的导数计算方法。一旦能够熟练使用NumericDiffCostFunction和AutoDiffCostFunction之后，建议再看看DynamicAutoDiffCostFunction， CostFunctionToFunctor，NumericDiffFunctor以及 ConditionedCostFunction等构建和计算代价函数的高级方法。