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 ε^2 = 0 ε2=0(亦即 ε ε ε是幂零元),每一个二元数 z z z 都有 z = a + b ε z=a+bε z=a+bε 的特性,其中 a a a b b b 是实数, a a a 是实部, b b b 是无穷小部。


求雅可比
Jet 是指可微函数 f f f 的泰勒展开式中前 k k k 项。
假设存在一个Jet,它是一个 n n n维的二元数,由一个实部 a a a n n n 个无穷小的二元数单位( ϵ i , i = 1 , . . . , n ϵ_i, i=1,...,n ϵi,i=1,...,n ∀ i , j : ϵ i ϵ j = 0 ∀i,j :ϵ_iϵ_j=0 i,j:ϵiϵj=0)构成
x = a + ∑ j v j ϵ j x=a+\sum_jv_jϵ_j x=a+jvjϵj
为避免符号冗杂,简化为
x = a + v x=a+\textbf{v} x=a+v
使用泰勒展开,得到:
f ( a + v ) = f ( a ) + D f ( a ) v f(a+\textbf{v})=f(a)+Df(a)\textbf{v} f(a+v)=f(a)+Df(a)v
对多维函数, f : R n → R m f:R^n→R^m f:RnRm,其中, x i = a i + v i x_i=a_i+\textbf{v}_i xi=ai+vi, ∀ i = 1 , . . . , n ∀i=1,...,n i=1,...,n:
f ( x 1 , . . . , x n ) = f ( a 1 , . . . , a n ) + ∑ i D i f ( a 1 , . . . , a n ) v i f(x_1,...,x_n)=f(a_1,...,a_n)+\sum_i D_if(a_1,...,a_n)\textbf{v}_i f(x1,...,xn)=f(a1,...,an)+iDif(a1,...,an)vi
v i = e i \textbf{v}_i=\textbf{e}_i vi=ei 作为 i t h i^{th} ith 个标准误差向量,上面的表达式简化为:
f ( x 1 , . . . , x n ) = f ( a 1 , . . . , a n ) + ∑ i D i f ( a 1 , . . . , a n ) ϵ i f(x_1,...,x_n)=f(a_1,...,a_n)+\sum_i D_if(a_1,...,a_n)ϵ_i f(x1,...,xn)=f(a1,...,an)+iDif(a1,...,an)ϵi
通过提取 ϵ i ϵ_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 n 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


你可能感兴趣的:(g2o)