12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

SFINAE to restrict template matching

release/4.3a0
Frank Dellaert 2025-04-27 13:35:56 -04:00
parent 1ced4d0470
commit a0c6902c8e
1 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/navigation/LIEKF.h
View File

@ -29,7 +29,7 @@
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <type_traits>
namespace gtsam { namespace gtsam {
@ -92,7 +92,7 @@ class LIEKF {
* @note Use this if your dynamics does not depend on the state, e.g. * @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 * 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); predict(G::Expmap(u * dt), Q);
} }
@ -108,12 +108,18 @@ class LIEKF {
* @param dt time step * @param dt time step
* @param Q process noise covariance * @param Q process noise covariance
*/ */
template <typename Dynamics> template <typename Dynamics,
typename = std::enable_if_t<
// not a plain vector…
!std::is_convertible_v<Dynamics, typename G::TangentVector>
// …and is callable as a true functor
&& std::is_invocable_r_v<typename G::TangentVector, Dynamics,
const G&, OptionalJacobian<n, n>&> > >
void predict(Dynamics&& f, double dt, const Matrix& Q) { void predict(Dynamics&& f, double dt, const Matrix& Q) {
typename G::Jacobian F; typename G::Jacobian F;
auto xi = f(X_, F); typename G::TangentVector xi = f(X_, F);
G U = G::Expmap(xi * dt); G U = G::Expmap(xi * dt);
auto A = U.inverse().AdjointMap() * F; typename G::Jacobian A = U.inverse().AdjointMap() * F;
X_ = X_.compose(U); X_ = X_.compose(U);
P_ = A * P_ * A.transpose() + Q; P_ = A * P_ * A.transpose() + Q;
} }
@ -133,12 +139,19 @@ class LIEKF {
* @param dt time step * @param dt time step
* @param Q process noise covariance * @param Q process noise covariance
*/ */
template <typename Control, typename Dynamics> template <typename Control, typename Dynamics,
typename = std::enable_if_t<
std::is_invocable_r_v<typename G::TangentVector, // return type
Dynamics, // functor
const G&, // X
const Control&, // u
OptionalJacobian<n, n>& // H
> > >
void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
typename G::Jacobian F; typename G::Jacobian F;
auto xi = f(X_, u, F); typename G::TangentVector xi = f(X_, u, F);
G U = G::Expmap(xi * dt); G U = G::Expmap(xi * dt);
auto A = U.inverse().AdjointMap() * F; typename G::Jacobian A = U.inverse().AdjointMap() * F;
X_ = X_.compose(U); X_ = X_.compose(U);
P_ = A * P_ * A.transpose() + Q; P_ = A * P_ * A.transpose() + Q;
} }