Ceres学习笔记建模篇001_代价函数基类CostFunction及其派生类SizedCostFunction介绍

Ceres主要由两大部分组成：

• 建模API：Ceres提供了一组丰富的工具来构造（最小二乘）优化问题；
• 求解器API：Ceres提供最小化算法来求解优化问题；

本篇主要介绍如何使用Ceres构造非线性优化问题，即建模。

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

$\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中的地图生成等问题都可以描述成以上形式。

本文主要详细介绍Ceres中的代价函数基类CostFunction，及其派生类SizedCostFunction。

1 CostFunction

CostFunction类的作用有两个：

1. 根据输入变量，即参数块，实现残差（代价）项计算，即输出，残差项是一维向量，可以包含有多个残差，也可以只有1个残差，也就是说，CostFunction是用户向Ceres描述最小二乘问题的接口，即建模；
2. 计算雅可比矩阵；

如果用公式来表示，如下：
参数块，由个子参数块构成，第个子参数块中的参数数量为;
是关于参数块的函数，用于计算残差；
是残差关于第个子参数块的雅可比向量，构成雅可比矩阵。

CostFunction是一个纯虚类，其声明如下：

class CERES_EXPORT CostFunction 
{
 public:
  // 构造函数
  CostFunction() : num_residuals_(0) {}

  // 禁用复制构造函数
  CostFunction(const CostFunction&) = delete;

  // 禁用重载赋值运算符
  void operator=(const CostFunction&) = delete;

  // 析构函数
  virtual ~CostFunction() {}

  // 用于计算残差和雅可比矩阵的主功能接口函数
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;

  // 获取参数块的大小及每个子参数块的大小
  const std::vector<int32_t>& parameter_block_sizes() const 
  {
    return parameter_block_sizes_;
  }

  // 获取残差数量
  int num_residuals() const { return num_residuals_; }

 protected:
  // 获取存储参数块及各子参数块大小的容器，一般在获取后会设置每个子参数块的大小
  std::vector<int32_t>* mutable_parameter_block_sizes() 
  {
    return &parameter_block_sizes_;
  }

  // 设置残差数量
  void set_num_residuals(int num_residuals) { num_residuals_ = num_residuals; }

 private:
  // 类成员变量
  std::vector<int32_t> parameter_block_sizes_;		// 参数块的大小及每个子参数块的大小
  int num_residuals_;		// 残差数量
};

CostFunction类声明中，只有两个类成员变量：

1. parameter_block_sizes_用于存储每个子参数块中的参数数量，parameter_block_sizes_.size()表示子参数块数量，parameter_block_sizes_[i]表示第个子参数块中的参数数量；
2. num_residuals_用于存储残差数量；

CostFunction类主要通过成员函数Evaluate()来实现残差和雅可比矩阵计算：
bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const，各参数说明如下：

1. parameters表示所有参数块的所有参数，是一个指针数组，数组的大小表示参数块的数量，数组中的每个元素又是一个指针，指向另一个数组（子参数块），例如，parameters[i]表示第个子参数块，parameters[i][c]表示第个子参数块中的第个参数；

为了便于理解，可以把parameters理解成一个二维数组，但不严谨，因为二维数组中，每一行的列数是一致的，而实际上，每个子参数块中的参数数量通常是不一致的；

parameters不可能是nullptr，因为不可能不存在待优化的参数；
parameter/parameters[i]的维度必须与parameter_block_sizes_/parameter_block_sizes_[i]的维度保持一致，包括每个子参数块的次序；

2. residuals表示残差向量，是一个一维数组；

residuals不可能是nullptr，因为不可能不存在目标函数，只要有目标函数，就有残差；
residuals的维度必须与num_residuals_值保持一致；

3. jacobians表示残差向量对参数块中所有参数的雅可比矩阵，与parameters一样，也是一个指针数组，数组的大小表示参数块的数量，数组中的每个元素又是一个指针，指向另一个数组（残差向量对子参数块中所有参数的偏导数）；

雅可比矩阵中，每个分量表示如下：

jacobians[i][r * parameters_block_size_[i] + c]= $\frac{\partial \text{residuals}[r]}{\partial \text{parameters}[i][c]}$ （3）
式中， $i$表示参数块索引， $c$表示某个参数块中的参数索引， $r$表示残差的索引；
jacobians的维度必须与 parameter_block_sizes_的维度保持一致， jacobians[i]的维度必须与 num_residuals_*parameter_block_sizes_[i]的维度保持一致；

• 若jacobians为nullptr，表示只需要计算残差，不需要计算雅可比矩阵；
• 若jacobians不为nullptr，且jacobians[i]为nullptr，表示不需要计算该参数块对应的雅可比向量，也就是说，不需要对该参数块中的参数进行优化，例如，当该参数块被标记为constant时；

Evaluate()函数返回值表示是否成功计算残差/雅可比矩阵；
可以对参数施加约束，如果参数块的初值满足约束条件，那么，每当不满足约束时，函数将返回false，从而避免优化器向着不合理的方向进行优化，这并不是一个非常复杂的强制约束机制，但这往往是一个好的机制；另外，参数块的初始值必须是合理的，否则，第0次迭代时，优化器就会报错；

CostFunction类是一个纯虚类，必须由用户派生出一个类，用户在派生类时，必须：

1. 通过mutable_parameter_block_sizes() + pushback()来指定所有参数块的维度，有几个参数块就pushback()几次；
2. 通过set_num_residuals()来指定残差维度；
3. override（重写）Evaluate()函数；

另外，在向Problem中AddResidualBlock()时，系统也会验证子参数块的数量及残差数量是否与parameter_block_sizes_和num_residuals_一致；

2 SizedCostFunction

如果参数块的维度以及残差向量的维度能够在编译时确定，可以使用SizedCostFunction类；
SizedCostFunction类是由CostFunction类派生得到的模板类，与CostFunction类相比，SizedCostFunction类通过模板参数确定子参数块的数量、每个子参数块中的参数数量、残差数量，因此，用户只需要override（重写）Evaluate()函数即可，不需要通过调用其他成员函数来指定各参数维度。

// 模板参数即残差数量、依次指定每个参数块中的参数数量，有几个子参数块就指定几个数
template <int kNumResiduals, int... Ns>
class SizedCostFunction : public CostFunction 
{
 public:
  static_assert(kNumResiduals > 0 || kNumResiduals == DYNAMIC,
                "Cost functions must have at least one residual block.");
  static_assert(internal::StaticParameterDims<Ns...>::kIsValid,
                "Invalid parameter block dimension detected. Each parameter "
                "block dimension must be bigger than zero.");

  using ParameterDims = internal::StaticParameterDims<Ns...>;

  // 构造函数，通过传入的模板参数，调用相关成员函数，指定各维度
  SizedCostFunction() 
  {
    // 指定残差数量
    set_num_residuals(kNumResiduals);
    // 指定子参数块数量，以及每个子参数块中的参数数量
    *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
  }

  virtual ~SizedCostFunction() {}

  // 用户仅仅但必须重写Evaluate()函数
  // Subclasses must implement Evaluate().
};

注意：残差数量在编译时可以不确定，即残差数量是动态变化的，并不是常数，此时，SizedCostFunction的派生类可以将表示残差数量的模板参数设置为ceres::DimentionType::DYNAMIC，最终的残差数量可以通用调用基类成员函数set_num_residuals(kNumResiduals)来确定，kNumResiduals可以是变量；

3 示例程序

根据前述内容可以知道，SizedCostFunction类是CostFunction类的派生类，功能非常相似，唯一的区别在于，CostFunction类的派生类需要通过调用成员函数 set_num_residuals()和mutable_parameter_block_sizes()来指定维度，而SizedCostFunction类的派生类通过模板参数来指定；
下面通过一个示例程序说明CostFunction类与SizedCostFunction类的区别；
自定义CostFunction的派生类，用于计算残差及雅可比矩阵，如下：

class MyCostFunction1 : public ceres::CostFunction 
{
public:
	// 此处，通过构造函数传入相关参数的维度
    MyCostFunction1(const int num_residuals, const std::vector<int>& vec_block_sizes, 
        double x, double y) : x_(x), y_(y) 
    {
        set_num_residuals(num_residuals);
        *mutable_parameter_block_sizes() = vec_block_sizes;
    }
    virtual ~MyCostFunction1() {}

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

        // 本例中输出残差维度为1
        double y0 = exp(m * x_ + c);
        residuals[0] = y_ - y0;

        if (jacobians == NULL) {
            return true;
        }
        // 残差对第1个参数块中的参数依次求偏导，即对m求偏导
        if (jacobians[0] != NULL) {
            jacobians[0][0] = -y0 * x_;
        }
        // 残差对第2个参数块中的参数依次求偏导，即对c求偏导
        if (jacobians[1] != NULL) {
            jacobians[1][0] = -y0;
        }
        return true;
    }

private:
    const double x_;
    const double y_;
};

自定义SizedCostFunction的派生类，用于计算残差及雅可比矩阵，如下：

// 通过模板参数传入相关参数的维度
class MyCostFunction2 : public ceres::SizedCostFunction<1, /*输出（resudual）维度大小*/\
    1, /*第1个输入参数块维度大小*/\
    1 /*第2个输入参数块维度大小*/>
{
public:
    MyCostFunction2(double x, double y) : x_(x), y_(y) {}
    virtual ~MyCostFunction2() {}

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

        // 本例中输出残差维度为1
        double y0 = exp(m * x_ + c);
        residuals[0] = y_ - y0;

        if (jacobians == NULL) {
            return true;
        }
        // 残差对第1个参数块中的参数依次求偏导，即对m求偏导
        if (jacobians[0] != NULL) {
            jacobians[0][0] = -y0 * x_;
        }
        // 残差对第2个参数块中的参数依次求偏导，即对c求偏导
        if (jacobians[1] != NULL) {
            jacobians[1][0] = -y0;
        }
        return true;
    }

private:
    const double x_;
    const double y_;
};