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）是实数的推广，二元数引入了一个无穷小的二元数单位：$ε$，它的平方$ε^2 = 0$（亦即$ε$是幂零元），每一个二元数 $z$ 都有 $z=a+bε$ 的特性，其中 $a$ 和 $b$ 是实数， $a$ 是实部， $b$ 是无穷小部。

---

求雅可比
Jet 是指可微函数 $f$ 的泰勒展开式中前 $k$ 项。
假设存在一个Jet，它是一个$n$维的二元数，由一个实部 $a$ 和 $n$ 个无穷小的二元数单位（$ϵ_i, i=1,...,n$， $∀i,j :ϵ_iϵ_j=0$）构成

$$x=a+\sum_jv_jϵ_j$$
为避免符号冗杂，简化为
$$x=a+\textbf{v}$$
使用泰勒展开，得到：
$$f(a+\textbf{v})=f(a)+Df(a)\textbf{v}$$
对多维函数， $f:R^n→R^m$，其中， $x_i=a_i+\textbf{v}_i$, $∀i=1,...,n$:
$$f(x_1,...,x_n)=f(a_1,...,a_n)+\sum_i D_if(a_1,...,a_n)\textbf{v}_i$$
令 $\textbf{v}_i=\textbf{e}_i$ 作为 $i^{th}$ 个标准误差向量，上面的表达式简化为：
$$f(x_1,...,x_n)=f(a_1,...,a_n)+\sum_i D_if(a_1,...,a_n)ϵ_i$$
通过提取 $ϵ_i$ 的系数，我们能得到Jacobian矩阵

---

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

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

  template <typename T>
  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<Rat43CostFunctor, 1, 4>(
        new Rat43CostFunctor(x, y));

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

template<int N> struct Jet {
  double a;
  Eigen::Matrix<double, 1, N> v;
};

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

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

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

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

template <int N> Jet<N> exp(const Jet<N>& f) {
  return Jet<T, N>(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 <int N>  Jet<N> pow(const Jet<N>& f, const Jet<N>& g) {
  return Jet<N>(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<const Rat43CostFunctor> functor_;
};

<完>

---

@leatherwang

---