Template Class AutoDiffLocalParameterization

Inheritance Relationships

Base Type

Class Documentation

template<typename PlusFunctor, typename MinusFunctor, int kGlobalSize, int kLocalSize>
class AutoDiffLocalParameterization : public fuse_core::LocalParameterization

Create a local parameterization with the Jacobians computed via automatic differentiation.

To get an auto differentiated local parameterization, you must define two classes with a templated operator() (a.k.a. a functor).

The first functor should compute:

Plus(x, delta) -> x_plus_delta

And the second functor should compute the inverse operation:

Minus(x1, x2) -> delta

Minus() should be defined such that if Plus(x1, delta) -> x2, then Minus(x1, x2) -> delta

The autodiff framework substitutes appropriate “Jet” objects for the template parameter T in order to compute the derivative when necessary, but this is hidden, and you should write the function as if T were a scalar type (e.g. a double-precision floating point number).

Additionally the GlobalSize and LocalSize must be specified as template parameters.

  • GlobalSize is the size of the variables x1 and x2. If this is a quaternion, the GloblaSize would be 4.

  • LocalSize is the size of delta, and may be different from GlobalSize. For quaternions, there are only 3 degrees of freedom, so the LocalSize is 3.

For more information on local parameterizations, see fuse_core::LocalParameterization

Public Functions

FUSE_SMART_PTR_DEFINITIONS (AutoDiffLocalParameterization< PlusFunctor, MinusFunctor, kGlobalSize, kLocalSize >) AutoDiffLocalParameterization()

Constructs new PlusFunctor and MinusFunctor instances.

AutoDiffLocalParameterization(PlusFunctor *plus_functor, MinusFunctor *minus_functor)

Takes ownership of the provided PlusFunctor and MinusFunctor instances.

bool Plus(const double *x, const double *delta, double *x_plus_delta) const override

Generalization of the addition operation, implemented by the provided PlusFunctor.

Parameters:

  • x – [in] The starting variable value, of size GlobalSize()

  • delta – [in] The variable increment to apply, of size LocalSize()

  • x_plus_delta – [out] The final variable value, of size GlobalSize()

Returns:

True if successful, false otherwise

bool ComputeJacobian(const double *x, double *jacobian) const override

The Jacobian of Plus(x, delta) w.r.t delta at delta = 0, computed using automatic differentiation.

Parameters:

  • x – [in] The value used to evaluate the Jacobian, of size GloblaSize()

  • jacobian – [out] The Jacobian in row-major order, of size GlobalSize() x LocalSize()

Returns:

True is successful, false otherwise

virtual bool Minus(const double *x1, const double *x2, double *delta) const override

Generalization of the subtraction operation, implemented by the provided MinusFunctor.

Parameters:

  • x1 – [in] The value of the first variable, of size GlobalSize()

  • x2 – [in] The value of the second variable, of size GlobalSize()

  • delta – [out] The difference between the second variable and the first, of size LocalSize()

Returns:

True if successful, false otherwise

virtual bool ComputeMinusJacobian(const double *x, double *jacobian) const override

The Jacobian of Minus(x1, x2) w.r.t x2 evaluated at x1 = x2 = x, computed using automatic differentiation.

Parameters:
Returns:

True is successful, false otherwise

inline int GlobalSize() const override

The size of the variable parameterization in the nonlinear manifold.

inline int LocalSize() const override

The size of a delta vector in the linear tangent space to the nonlinear manifold.