Ceresの自動微分とEKF SLAM

前回,Ceresの自動微分モジュールの抜き出し方を調べたので,せっかくなので実際にこれを使ってEKF SLAMの微分計算を自動化してみました.

基本的には前と同じでAutoDiffを使って微分計算するんですが,今回はラムダ関数にauto引数を渡すことでコードを簡潔にしてみました.C++14以降なら,だいたい以下のような感じで関数fの微分が求められます.

Eigen::Vector2d vel = ...;  // linear & angular velocity
Eigen::Vector3d pose = ...; // current robot pose
Eigen::Vector3d new_pose;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> jacobian;

auto f = [&amp;](const auto* x, auto* z) {
  z[0] = x[0] + vel[0] * cos(x[2]);
  z[1] = x[1] + vel[0] * sin(x[2]);
  z[2] = x[2] + vel[1];
  return true;
};

double* x = pose.data();
double* z = new_pose.data();
double* J = jacobian.data();

ceres::internal::AutoDiff<decltype(f), double, 3> ad;
ad.Differentiate(f, &amp;x, 3, z, &amp;J);

かなり適当に実装しましたが一応それなりに動いている風です.実際はこの程度の問題であればUKFを使ってしまえば微分なしでいけるんですが,自動微分モジュールを汎用的に使う方法を試したということで.

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.