CostFunctionToFunctor

class CostFunctionToFunctor

CostFunctionToFunctor 是一个适配器类，允许用户在用于自动微分的模板函数中使用 CostFunction 对象。这样，用户就可以将分析、数值和自动微分无缝地结合起来。使用自动微分的时候，一般的步骤是我们只需要定义一个类（结构体）然后重载 operator() 运算符计算残差，将这个类作为自动微分的模板参数传入即可。

例如，我们构造一个 CostFunction

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

上述 CostFunction 实现了一个点在其本地（相机）坐标系中对其图像平面的投影，计算不涉及相机外参，并将其观测点和投影点相减，计算残差，同时可以通过解析或者数值微分进行导数的计算。现在我们想要将该 CostFunction 和相机外参结合起来，例如旋转和平移，实现完整的重投影误差的计算，假设我们有如下模板函数

template<typename T>
void RotateAndTranslatePoint(const T* rotation,
                             const T* translation,
                             const T* point,
                             T* result);

如下为相机重投影误差的 struct

struct CameraProjection {
  explicit CameraProjection(double* observation)
  : intrinsic_projection_(std::make_unique<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_;
};

注意，CostFunctionToFunctor 拥有传入构造函数的 CostFunction 的所有权。在上面的示例中，我们假设 IntrinsicProjection 是一个能够评估其残差及其导数的 CostFunction。假设情况并非如此，IntrinsicProjection 的定义如下：

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

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

这里的 ThirdPartyProjectionFunction 是我们无法控制的第三方库函数。因此，该函数可以计算残差，但是无法计算导数，我们希望使用数值微分来计算其导数。在这种情况下，我们可以使用 NumericDiffCostFunction 和 CostFunctionToFunctor 的组合来完成工作。

struct CameraProjection {
  explicit CameraProjection(double* observation)
     : intrinsic_projection_(
           std::make_unique<NumericDiffCostFunction<
         					IntrinsicProjection, 
         					CENTRAL, 2, 5, 3>>()) {}

  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, residuals);
  }

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

至于 CameraProjection 的创建，我们可以使用自动求导即可。