前回，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 = [&](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, &x, 3, z, &J);
かなり適当に実装しましたが一応それなりに動いている風です．実際はこの程度の問題であればUKFを使ってしまえば微分なしでいけるんですが，自動微分モジュールを汎用的に使う方法を試したということで．