diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index 843365c6d..a8a136094 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -29,7 +29,7 @@ #include #include - +#include namespace gtsam { @@ -92,7 +92,7 @@ class LIEKF { * @note Use this if your dynamics does not depend on the state, e.g. * predict(f(u), dt, q) where u is a control input */ - void predict(const Vector& u, double dt, const Matrix& Q) { + void predict(const typename G::TangentVector& u, double dt, const Matrix& Q) { predict(G::Expmap(u * dt), Q); } @@ -108,12 +108,18 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template + template + // …and is callable as a true functor + && std::is_invocable_r_v&> > > void predict(Dynamics&& f, double dt, const Matrix& Q) { typename G::Jacobian F; - auto xi = f(X_, F); + typename G::TangentVector xi = f(X_, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + typename G::Jacobian A = U.inverse().AdjointMap() * F; X_ = X_.compose(U); P_ = A * P_ * A.transpose() + Q; } @@ -133,12 +139,19 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template + template & // H + > > > void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; - auto xi = f(X_, u, F); + typename G::TangentVector xi = f(X_, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; + typename G::Jacobian A = U.inverse().AdjointMap() * F; X_ = X_.compose(U); P_ = A * P_ * A.transpose() + Q; }