Ceres自动求导（AutomaticDiff）初探

g2o中如果没有定义这个边的linearizeOplus()，就会调用数值求导。但是g2o的数值求导比较慢，效果较差。所以下面探讨在g2o中嵌入ceres的自动求导，避免复杂的雅可比矩阵的推导。

参考：http://ceres-solver.org/automatic_derivatives.html
先看一下数值求导与自动求导的区别：

We will now consider automatic differentiation. It is a technique that can compute exact derivatives, fast, while requiring about the same effort from the user as is needed to use numerical differentiation.

关于自动求导、数值求导、解析求导三者的详细区别，参考：https://blog.csdn.net/yizhou2010/article/details/52712202

---

二元数
二元数（Dual number）是实数的推广，二元数引入了一个无穷小的二元数单位：$\epsilon$，它的平方${\epsilon }^2=0$（亦即$\epsilon$是幂零元），每一个二元数 $z$ 都有 $z=a+b\epsilon$ 的特性，其中 $a$ 和 $b$ 是实数， $a$ 是实部， $b$ 是无穷小部。

---

求雅可比
Jet 是指可微函数 $f$ 的泰勒展开式中前 $k$ 项。
假设存在一个Jet，它是一个$n$维的二元数，由一个实部 $a$ 和 $n$ 个无穷小的二元数单位（${ϵ}_i,i=1,...,n$， $\forall i,j:{ϵ}_i{ϵ}_j=0$）构成
$$x=a+\sum _j{v}_j{ϵ}_j$$
为避免符号冗杂，简化为
$$x=a+\text{v}$$
使用泰勒展开，得到：
$$f(a+\text{v})=f(a)+Df(a)\text{v}$$
对多维函数，$f:R^n\to R^m$，其中，$x_i=a_i+{\text{v}}_i$, $\forall i=1,...,n$:
$$f(x_1,...,x_n)=f(a_1,...,a_n)+\sum _i{D}_{i}f(a_1,...,a_n){\text{v}}_i$$
令${\text{v}}_i={\text{e}}_i$ 作为 $i^{th}$ 个标准误差向量，上面的表达式简化为：
$$f(x_1,...,x_n)=f(a_1,...,a_n)+\sum _i{D}_{i}f(a_1,...,a_n){ϵ}_i$$
通过提取 ${ϵ}_i$ 的系数，我们能得到Jacobian矩阵

---

举例说明：
使用Rat43的例子说明，代价函数的计算模型如下：

struct Rat43CostFunctor {
  Rat43CostFunctor(const double x, const double y) : x_(x), y_(y) {}

  template 
  bool operator()(const T* parameters, T* residuals) const {
    const T b1 = parameters[0];
    const T b2 = parameters[1];
    const T b3 = parameters[2];
    const T b4 = parameters[3];
    residuals[0] = b1 * pow(1.0 + exp(b2 -  b3 * x_), -1.0 / b4) - y_;
    return true;
  }

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


CostFunction* cost_function =
      new AutoDiffCostFunction(
        new Rat43CostFunctor(x, y));

其中，这里模板参数T通常为double，在需要求residual的雅克比时，T=Jet
$n$维的二元数Jet满足下面的运算法则：

template struct Jet {
  double a;
  Eigen::Matrix v;
};

template Jet operator+(const Jet& f, const Jet& g) {
  return Jet(f.a + g.a, f.v + g.v);
}

template Jet operator-(const Jet& f, const Jet& g) {
  return Jet(f.a - g.a, f.v - g.v);
}

template Jet operator*(const Jet& f, const Jet& g) {
  return Jet(f.a * g.a, f.a * g.v + f.v * g.a);
}

template Jet operator/(const Jet& f, const Jet& g) {
  return Jet(f.a / g.a, f.v / g.a - f.a * g.v / (g.a * g.a));
}

template  Jet exp(const Jet& f) {
  return Jet(exp(f.a), exp(f.a) * f.v);
}

// This is a simple implementation for illustration purposes, the
// actual implementation of pow requires careful handling of a number
// of corner cases.
template   Jet pow(const Jet& f, const Jet& g) {
  return Jet(pow(f.a, g.a),
                g.a * pow(f.a, g.a - 1.0) * f.v +
                pow(f.a, g.a) * log(f.a); * g.v);
}

使用上面的重载函数，使用Jets矩阵调用Rat43CostFunctor，而不是原来doubles(parameters的类型时double)，这样就可以计算得到雅可比矩阵了。

class Rat43Automatic : public ceres::SizedCostFunction<1,4> {
 public:
  Rat43Automatic(const Rat43CostFunctor* functor) : functor_(functor) {}
  virtual ~Rat43Automatic() {}
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    // Just evaluate the residuals if Jacobians are not required.
    //【1】如果不需要雅可比，直接使用double类型的parameters调用Rat43CostFunctor即可
    if (!jacobians) return (*functor_)(parameters[0], residuals);

    // Initialize the Jets
    //【2】初始化Jets
    ceres::Jet<4> jets[4];
    for (int i = 0; i < 4; ++i) {
      jets[i].a = parameters[0][i];
      jets[i].v.setZero();
      jets[i].v[i] = 1.0;
    }

    ceres::Jet<4> result;
    (*functor_)(jets, &result);

    // Copy the values out of the Jet.
    //【3】提取残差
    residuals[0] = result.a;      
    //【4】提取雅可比矩阵  
    for (int i = 0; i < 4; ++i) {
      jacobians[0][i] = result.v[i]; 
    }
    return true;
  }

 private:
  std::unique_ptr functor_;
};

<完>

---

@leatherwang

---