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ε ε ε멱 0 원), 각 이원 수 z z z 는 z = a + b 가 있 습 니 다.ε z=a+bε z=a+bε 의 특성, 그 중에서 a a a 와 b b 는 실수 이 고 a a a 는 실 부 이 며 b b 는 무한 소부 이다.
구 아 커 비 제 트 는 마이크로 함수 f f 의 테일러 전개 식 중 전 k k k 항 을 말한다.Jet 가 존재 한다 고 가정 하면 n n n 차원 의 이원 수 이 고 실제 a a a a 와 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+j∑​vj​ϵ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: Rn → Rm, 그 중에서 x i = a + v i xi=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) + i ∑ Di f (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​)+i∑​Di​f(a1​,...,an​)ϵi 추출 을 통 해ϵ i ϵ_i ϵi 의 계수, 우 리 는 Jacobian 행렬 을 얻 을 수 있다.
예 를 들 어 Rat 43 의 예 를 들 어 대가 함수 의 계산 모델 은 다음 과 같다.

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 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