本文详细介绍了Ceres优化库中的核心组件CostFunction及其派生类，包括SizedCostFunction、AutoDiffCostFunction、NumericDiffCostFunction等。这些组件用于计算残差及雅可比矩阵，适用于不同的应用场景。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

CostFunction
SizedCostFunction
AutoDiffCostFunction
DynamicAutoDiffCostFunction
NumericDiffCostFunction
DynamicNumericDiffCostFunction
CostFunctionToFunctor
DynamicCostFunctionToFunctor
ConditionedCostFunction

CostFunction

CostFunction 用来计算残差，并且计算雅可比矩阵。

如对于给定的parameter blocks $\left[x_{i_1}, ... ,x_{i_k}\right]$ ，计算residuals $f_i\left(x_{i_1},...,x_{i_k}\right)$

以及jacobians
$J_{ij} = \frac{\partial}{\partial x_{i_j}}f_i\left(x_{i_1},...,x_{i_k}\right),\quad \forall j \in \{1, \ldots, k\}$

注意 $x_{i_j}$ 的维度不一定相同。


class CostFunction {

 public:

  virtual bool Evaluate(double const* const* parameters,

                        double* residuals,

                        double** jacobians) = 0;

  const vector<int32>& parameter_block_sizes();

  int num_residuals() const;


 protected:

  vector<int32>* mutable_parameter_block_sizes();

  void set_num_residuals(int num_residuals);


 private:
  // Cost function signature metadata: number of inputs & their sizes, number of outputs (residuals).
  std::vector<int32> parameter_block_sizes_;
  int num_residuals_;

};


bool CostFunction::Evaluate(double const **parameters, double *residuals, double **jacobians)

parameters 是指向parameter blocks的指针数组，并且和CostFunction::parameter_block_sizes_的大小和顺序相对应。

residuals是长度为num_residuals_的数组。

jacobians存储了对应parameter blocks的雅可比矩阵。jacobians[i] 是长度为CostFunction::num_residuals_ * CostFunction::parameter_block_sizes_ [i]个元素的数组，每个雅可比矩阵以row major的形式存储：

jacobians[i][r * parameter_block_size_[i] + c] = $\frac{\partial residual[r]}{\partial parameters[i][c]}$

这里parameters[i][c]表示parameter_block_sizes_[i]的第c个参数。

这里jacobians[i][r * parameter_block_size_[i] + c]表示第r个残差对parameters[i][c]的偏导数

如果jacobians为空，则不需要求雅可比矩阵，只需要计算残差。当jacobians[i]为空，则不需要计算对第i个参数的雅可比矩阵，这在第i个参数设置为不需要优化时可以使用。

如BA问题中，计算重投影误差时 $r = f(camera,point)$ ，camera和point为两个参数块，维数分别为9（R、T、f、k1、k2），3（X、Y、Z）。

parameter_block_sizes_[0]=9

parameter_block_sizes_[1]=3

num_residuals_ = 2

size( jacobians[0] ) = 2 * 9 = 18 前9个是r1（u）对这9这个参数的偏导，后9个是r2（v）对这9这个参数的偏导

size( jacobians[1] ) = 2 * 3 = 6

SizedCostFunction

在知道残差向量、参数的维度时，可以使用SizeCostFunction来指定其维度。用户只要实现CostFunction::Evaluate()。

template<int kNumResiduals,
         int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
         int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
class SizedCostFunction : public CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;
};

AutoDiffCostFunction

编写CostFunction或SizedCostFunction是一个繁琐和容易的过程，特别是在计算雅可比矩阵时。因此ceres提供了自动微分。

template <typename CostFunctor,
       int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
       int N0,       // Number of parameters in block 0.
       int N1 = 0,   // Number of parameters in block 1.
       int N2 = 0,   // Number of parameters in block 2.
       int N3 = 0,   // Number of parameters in block 3.
       int N4 = 0,   // Number of parameters in block 4.
       int N5 = 0,   // Number of parameters in block 5.
       int N6 = 0,   // Number of parameters in block 6.
       int N7 = 0,   // Number of parameters in block 7.
       int N8 = 0,   // Number of parameters in block 8.
       int N9 = 0>   // Number of parameters in block 9.
class AutoDiffCostFunction : public
SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
 public:
  explicit AutoDiffCostFunction(CostFunctor* functor);
  // Ignore the template parameter kNumResiduals and use
  // num_residuals instead.
  AutoDiffCostFunction(CostFunctor* functor, int num_residuals);
}

为了得到一个自动区分微分的cost function，必须定义一个模板函数template <typename T> bool operator()(const T* const x, ..., T* residual)。在内部实现时，ceres会以结构体Jet替换T。

operator()的最后个参数（唯一非const的参数）为计算的残差，函数返回true表示成功。

例如，考虑一个误差 $e=k-x^Ty$ ，其中 $x$ 、 $y$ 为2维度向量， $k$ 为常数。在实际最小二乘问题中，最小化的是 $e^2$ ，这个平方在ceres的优化框架中已经隐含了。

class MyScalarCostFunctor {
  MyScalarCostFunctor(double k): k_(k) {}

  template <typename T>
  bool operator()(const T* const x , const T* const y, T* e) const {
    e[0] = T(k_) - x[0] * y[0] - x[1] * y[1];
    return true;
  }

 private:
  double k_;
};

注意上面的定义中，输入参数首先是x、y，它们是指向T类型数组的常量指针，如果有其他输入参数，可以放在y的后面。残差是最后一个参数，是一个指向数组的指针，在此例中 $e$ 是一个常量，因此只要设置e[0]即可。

之后需要定义自动微分的cost function：

CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
        new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                    |  |  |
                        Dimension of residual ------+  |  |
                        Dimension of x ----------------+  |
                        Dimension of y -------------------+

AutoDiffCostFunction 也支持在运行时定义残差的数量：

CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
        new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
        runtime_number_of_residuals); <----+           |     |  |
                                           |           |     |  |
                                           |           |     |  |
          Actual number of residuals ------+           |     |  |
          Indicate dynamic number of residuals --------+     |  |
          Dimension of x ------------------------------------+  |
          Dimension of y ---------------------------------------+

该框架最多支持10个独立的变量，每个变量的维度没有限制。

DynamicAutoDiffCostFunction

AutoDiffCostFunction要求参数数目及每个参数的维度在编译时已知的，同时只支持10个参数块。在许多应用中，这是不够的。例如，例如贝塞尔曲线拟合，神经网络训练等。这个时候可以使用DynamicAutoDiffCostFunction。

template <typename CostFunctor, int Stride = 4>
class DynamicAutoDiffCostFunction : public CostFunction {
};

首先定义一个cost functor：

struct MyCostFunctor {
  template<typename T>
  bool operator()(T const* const* parameters, T* residuals) const {
  }
}

在创建DynamicAutoDiffCostFunction后需要指定参数的维度：

DynamicAutoDiffCostFunction<MyCostFunctor, 4>* cost_function =
  new DynamicAutoDiffCostFunction<MyCostFunctor, 4>(
    new MyCostFunctor());
cost_function->AddParameterBlock(5);
cost_function->AddParameterBlock(10);
cost_function->SetNumResiduals(21);

NumericDiffCostFunction

在某些情况下，很难定义一个模板类的cost functor，例如调用一个不可控的第三方函数计算残差时。这种情况下可以使用NumericDiffCostFunction。

重新考虑误差 $e=k-x^Ty$ ，假定其类型为doube，重写functor：

class MyScalarCostFunctor {
  MyScalarCostFunctor(double k): k_(k) {}

  bool operator()(const double* const x,
                  const double* const y,
                  double* residuals) const {
    residuals[0] = k_ - x[0] * y[0] + x[1] * y[1];
    return true;
  }

 private:
  double k_;
};

接着定义cost function：

CostFunction* cost_function
    = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
        new MyScalarCostFunctor(1.0));                    ^     ^  ^  ^
                                                          |     |  |  |
                              Finite Differencing Scheme -+     |  |  |
                              Dimension of residual ------------+  |  |
                              Dimension of x ----------------------+  |
                              Dimension of y -------------------------+

NumericDiffCostFunction也支持运行时决定残差的维度，具体参考手册。

ceres提供了三种方法来求解数值微分：
FORWARD： $f′(x) = \frac{f(x+h)−f(x)}{h}$ 计算速度最快，精度最低
CENTRAL： $f′(x) = \frac{f(x+h)−f(x−h)}{2h}$ 计算速度一般，精度一般。
RIDDERS：使用CENTRAL的方法计算微分，在计算过程多次减小h。计算速度最慢，精度最高。

注意ceres在实现数值微分时，假设参数是存在于欧式空间，添加的扰动也存在于欧式空间。如果参数为表示旋转的单位四元数，添加的扰动会使得四元数不在具有模为1的约束。这种问题叫做LocalParameterization。

DynamicNumericDiffCostFunction

NumericDiffCostFunction要求参数数目及每个参数的维度在编译时已知的，同时只支持10个参数块。在许多应用中，这是不够的。这个时候可以使用DynamicNumericDiffCostFunction。

template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
class DynamicNumericDiffCostFunction : public CostFunction {
};

functor：

struct MyCostFunctor {
  bool operator()(double const* const* parameters, double* residuals) const {
  }
}

cost function：

DynamicNumericDiffCostFunction<MyCostFunctor>* cost_function =
  new DynamicNumericDiffCostFunction<MyCostFunctor>(new MyCostFunctor);
cost_function->AddParameterBlock(5);
cost_function->AddParameterBlock(10);
cost_function->SetNumResiduals(21);

CostFunctionToFunctor

CostFunctionToFunctor 是一个adapter class，可以在编写functor时使用CostFunction对象。（编写functor是为了构建自动微分的cost function），因此可以对多种微分方式进行混合。

例如编写了一个通过内参投影到图像坐标系的cost function，我们可以直接推导出雅可比矩阵。

class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {
  public:
    IntrinsicProjection(const double* observation);
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const;
};

下面编写一个通过外参、内参投影到图像坐标系的functor，我们希望通过自动微分的方式求解其雅可比矩阵。在编写完functor后，就可以利用之前提到的方式构建自动微分的cost function。

struct CameraProjection {
  CameraProjection(double* observation)
  : intrinsic_projection_(new IntrinsicProjection(observation)) {}

  template <typename T>
  bool operator()(const T* rotation,
                  const T* translation,
                  const T* intrinsics,
                  const T* point,
                  T* residual) const {
    T transformed_point[3];
    RotateAndTranslatePoint(rotation, translation, point, transformed_point);

    // Note that we call intrinsic_projection_, just like it was
    // any other templated functor.
    return intrinsic_projection_(intrinsics, transformed_point, residual);
  }

 private:
  CostFunctionToFunctor<2,5,3> intrinsic_projection_;
};

但是如果IntrinsicProjection中调用了第三方函数并且不能获得对应的雅可比矩阵，则需要使用数值微分的形式求解。这种情况下可以使用NumericDiffCostFunction和CostFunctionToFunctor完成这项工作。

struct IntrinsicProjection
  IntrinsicProjection(const double* observation) {
    observation_[0] = observation[0];
    observation_[1] = observation[1];
  }

  bool operator()(const double* calibration,
                  const double* point,
                  double* residuals) {
    double projection[2];
    ThirdPartyProjectionFunction(calibration, point, projection);
    residuals[0] = observation_[0] - projection[0];
    residuals[1] = observation_[1] - projection[1];
    return true;
  }
 double observation_[2];
};

truct CameraProjection {
  CameraProjection(double* observation)
    intrinsic_projection_(
      new NumericDiffCostFunction<IntrinsicProjection, CENTRAL, 2, 5, 3>(
        new IntrinsicProjection(observation)) {
  }

  template <typename T>
  bool operator()(const T* rotation,
                  const T* translation,
                  const T* intrinsics,
                  const T* point,
                  T* residuals) const {
    T transformed_point[3];
    RotateAndTranslatePoint(rotation, translation, point, transformed_point);
    return intrinsic_projection_(intrinsics, transformed_point, residual);
  }

 private:
  CostFunctionToFunctor<2,5,3> intrinsic_projection_;
};

DynamicCostFunctionToFunctor

class IntrinsicProjection : public CostFunction {
  public:
    IntrinsicProjection(const double* observation);
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const;
};

struct CameraProjection {
  CameraProjection(double* observation)
      : intrinsic_projection_(new IntrinsicProjection(observation)) {
  }

  template <typename T>
  bool operator()(T const* const* parameters,
                  T* residual) const {
    const T* rotation = parameters[0];
    const T* translation = parameters[1];
    const T* intrinsics = parameters[2];
    const T* point = parameters[3];

    T transformed_point[3];
    RotateAndTranslatePoint(rotation, translation, point, transformed_point);

    const T* projection_parameters[2];
    projection_parameters[0] = intrinsics;
    projection_parameters[1] = transformed_point;
    return intrinsic_projection_(projection_parameters, residual);
  }

 private:
  DynamicCostFunctionToFunctor intrinsic_projection_;
};