this class represent the state of the MSCEqF. This includes the state of the lifted system (element of the symmetry group) and the covariance. More...

#include <state.hpp>

Public Types
using	MSCEqFStateKey = std::variant<MSCEqFStateElementName, uint>
	Key to access the msceqf state map.

using	MSCEqFKey = std::variant<MSCEqFStateKey, fp>
	Key to access the msceqf state and clones map.

using	MSCEqFStateMap = std::unordered_map<MSCEqFStateKey, MSCEqFStateElementSharedPtr>
	MSCEqF state map.

using	MSCEqFClonesMap = std::map<fp, MSCEqFStateElementSharedPtr>
	MSCEqF clones map.

Public Member Functions
	MSCEqFState ()=delete
	Deleted default constructor.

	MSCEqFState (const StateOptions &opts, const SystemState &xi0)
	Construct the state of the MSCEqF given the options.

	MSCEqFState (const MSCEqFState &other)
	Rule of Five.

	MSCEqFState (MSCEqFState &&other) noexcept

MSCEqFState &	operator= (const MSCEqFState &other)

MSCEqFState &	operator= (MSCEqFState &&other) noexcept

const SE23 &	D () const
	Get a reference to the SE23 component of the Semi Direct Bias Group element of the MSCEqF state.

const SE3	B () const
	Get a copy of the SE3 component of the Semi Direct Bias Group element of the MSCEqF state that includes the rotational component (R) and the first isometry (v)

const SE3	C () const
	Get a copy of to the SE3 component of the Semi Direct Bias Group element of the MSCEqF state that includes the rotational component (R) and the second isometry (p)

const Vector6 &	delta () const
	Get a reference to the R6 component of the Semi Direct Bias Group element of the MSCEqF state.

const SE3 &	E () const
	Get a reference to the SE3 element of the MSCEqF state.

const In &	L () const
	Get a reference to the In element of the MSCEqF state.

const SOT3 &	Q (const uint &feat_id) const
	Get a reference to the SOT3 element of the MSCEqF state that correspond to the given feature id.

const SE3 &	clone (const fp &timestamp) const
	Get a reference to the SE3 element of the MSCEqF clones that correspond to the given timestamp.

const uint &	index (const MSCEqFKey &key) const
	Get a reference to the index of the state element or the clone element corresponding to the given key.

const uint &	dof (const MSCEqFKey &key) const
	Get a reference to the dof of the state element or the clone element corresponding to the given key.

size_t	clonesSize () const
	Get the amount of clones.

const fp &	cloneTimestampToMarginalize () const
	Get the timestamp of the clone to marginalize. We implement our keyframing strategy here. So far we simply marginalize the oldest clone.

const MatrixX &	cov () const
	Get a reference to the covariance matrix.

const MatrixX	covBlock (const MSCEqFKey &key) const
	get a constant copy of the covariance block relative to the elements (states or clones) corresponding to the given keys.

const MatrixX	subCov (const std::vector< MSCEqFKey > &keys) const
	Get a constant copy of the the covariance submatrix (including cross-correlations) constructed with covariance blocks relative to the elements (states or clones) corresponding to the given keys. The ordering of the covariance returned follows the ordering of the given keys.

const MatrixX	subCovCols (const std::vector< MSCEqFKey > &keys) const
	Get a constant copy of the the covariance submatrix (including cross-correlations) constructed with covariance columns relative to the elements (states or clones) corresponding to the given keys. The ordering of the covariance returned follows the ordering of the given keys.

const StateOptions &	opts () const
	Get the state options.

void	initializeStateElement (const MSCEqFStateKey &key, const MatrixX &cov_block)
	Initialize MSCEqF state element into the state map, and the relative covariance block.

void	stochasticCloning (const fp &timestamp)
	Augment the MSCEqF clones map with a new clone of the actual E element of the MSCEqF state. The new clone is mapped via the given timestamp.

void	marginalizeCloneAt (const fp &timestamp)
	Marginalize out clone at a given timestamp.

const MSCEqFState	Random () const
	Return a random MSCEqF state without changing this. This method WILL NOT* change the actual values of the state. This method WILL NOT initialize the covariance or the clones map for the returned state. This method will only initialize the state map with random values for the returned state.

const MSCEqFState	operator* (const MSCEqFState &other) const
	operator* overloading for MSCEqFState. This function will perform the composition this * other for each element of the state map. This method will NOT perform any composition for the covariance matrix, and for the clones map.

Static Public Member Functions
static std::string	toString (const MSCEqFStateKey &key)
	Get a string describing the given MSCEqFStateKey.

Friends
class	Symmetry
	Symmetry can access private members of MSCEqFState.

class	Propagator
	Propagator can access private members of MSCEqFState.

class	Updater
	Updater can access private members of MSCEqFState.

class	ZeroVelocityUpdater
	Zero velocity updater can access private members of MSCEqFState.

Detailed Description

this class represent the state of the MSCEqF. This includes the state of the lifted system (element of the symmetry group) and the covariance.

Constructor & Destructor Documentation

◆ MSCEqFState()

msceqf::MSCEqFState::MSCEqFState	(	const StateOptions &	opts,
		const SystemState &	xi0 )

Construct the state of the MSCEqF given the options.

Parameters

opts	Options of the MSCEqF state
xi0	Origin

Note: The MSCEqF state has not to be confused with the system state. The former is the state of the lifted system, in which the EqF is based on, while latter is the state of the system posed on the homogenous space. The given origin is needed to initialize cross-correlation whenever the origin is not identity.

Member Function Documentation

◆ B()

const SE3 msceqf::MSCEqFState::B ( ) const

nodiscard

Get a copy of the SE3 component of the Semi Direct Bias Group element of the MSCEqF state that includes the rotational component (R) and the first isometry (v)

Returns: SE3 group element representing the rotational component and the first isometry of the MSCEqF state

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ C()

const SE3 msceqf::MSCEqFState::C ( ) const

nodiscard

Get a copy of to the SE3 component of the Semi Direct Bias Group element of the MSCEqF state that includes the rotational component (R) and the second isometry (p)

Returns: SE3 group element representing the rotational component and the second isometry of the MSCEqF state

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ clone()

const SE3 & msceqf::MSCEqFState::clone ( const fp & timestamp ) const

nodiscard

Get a reference to the SE3 element of the MSCEqF clones that correspond to the given timestamp.

Parameters

timestamp Timestamp

Returns: SE3 group element of the MSCEqF clones representing the element acting on the extrinsic calibration

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ clonesSize()

size_t msceqf::MSCEqFState::clonesSize ( ) const

inlinenodiscard

Get the amount of clones.

Returns: Number of clones

◆ cloneTimestampToMarginalize()

const fp & msceqf::MSCEqFState::cloneTimestampToMarginalize ( ) const

nodiscard

Get the timestamp of the clone to marginalize. We implement our keyframing strategy here. So far we simply marginalize the oldest clone.

Returns: Timestamp of the clone to marginalize

◆ cov()

const MatrixX & msceqf::MSCEqFState::cov ( ) const

nodiscard

Get a reference to the covariance matrix.

Returns: Covariance matrix

◆ covBlock()

const MatrixX msceqf::MSCEqFState::covBlock ( const MSCEqFKey & key ) const

nodiscard

get a constant copy of the covariance block relative to the elements (states or clones) corresponding to the given keys.

Parameters

key	State element name, feature id or timestamp of clone

Returns: Block of the covariance matrix corresponding to the given key

◆ D()

const SE23 & msceqf::MSCEqFState::D ( ) const

nodiscard

Get a reference to the SE23 component of the Semi Direct Bias Group element of the MSCEqF state.

Returns: SE23 group element of the MSCEqF state

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ delta()

const Vector6 & msceqf::MSCEqFState::delta ( ) const

nodiscard

Get a reference to the R6 component of the Semi Direct Bias Group element of the MSCEqF state.

Returns: R6 group element of the MSCEqF state

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ dof()

const uint & msceqf::MSCEqFState::dof ( const MSCEqFKey & key ) const

nodiscard

Get a reference to the dof of the state element or the clone element corresponding to the given key.

Parameters

key	State element name, feature id or timestamp of clone

Returns: Degree of freedom of the state element or the clone element corresponding to the given key

◆ E()

const SE3 & msceqf::MSCEqFState::E ( ) const

nodiscard

Get a reference to the SE3 element of the MSCEqF state.

Returns: SE3 group element of the MSCEqF state representing the element acting on the extrinsic calibration

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ index()

const uint & msceqf::MSCEqFState::index ( const MSCEqFKey & key ) const

nodiscard

Get a reference to the index of the state element or the clone element corresponding to the given key.

Parameters

key	State element name, feature id or timestamp of clone

Returns: Index of the state element or the clone element corresponding to the given key

◆ initializeStateElement()

void msceqf::MSCEqFState::initializeStateElement	(	const MSCEqFStateKey &	key,
		const MatrixX &	cov_block )

Initialize MSCEqF state element into the state map, and the relative covariance block.

Parameters

key	State element name or feature id
cov_block	Corresponding blcok of the covariance matrix

Note: Note that the MSCEqF states are always initialized at the identity. This is correct since is xi0 that "controls" how close to ground truth these are.

◆ L()

const In & msceqf::MSCEqFState::L ( ) const

nodiscard

Get a reference to the In element of the MSCEqF state.

Returns: In group element of the MSCEqF state representing the element acting on the intrinsic calibration

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ marginalizeCloneAt()

void msceqf::MSCEqFState::marginalizeCloneAt ( const fp & timestamp )

Marginalize out clone at a given timestamp.

Parameters

timestamp Timestamp of the clone to marginalize

◆ operator*()

const MSCEqFState msceqf::MSCEqFState::operator* ( const MSCEqFState & other ) const

nodiscard

operator* overloading for MSCEqFState. This function will perform the composition this * other for each element of the state map. This method will NOT perform any composition for the covariance matrix, and for the clones map.

Parameters

other MSCEqF state

Returns: MSCEqF state

Note: THIS IS MEANT TO BE AN HELPER FUNCTION FOR DEBUG/TESTING

◆ opts()

const StateOptions & msceqf::MSCEqFState::opts ( ) const

inlinenodiscard

Get the state options.

Returns: State options

◆ Q()

const SOT3 & msceqf::MSCEqFState::Q ( const uint & feat_id ) const

nodiscard

Get a reference to the SOT3 element of the MSCEqF state that correspond to the given feature id.

Parameters

feat_id Feature id

Returns: SOT3 group element of the MSCEqF state representing the element acting on the feature

Note: This function does not introduce any runtime overhead due to casting, because it uses static_pointer_cast

◆ Random()

const MSCEqFState msceqf::MSCEqFState::Random ( ) const

nodiscard

Return a random MSCEqF state without changing this. This method *WILL NOT change the actual values of the state. This method WILL NOT initialize the covariance or the clones map for the returned state. This method will only initialize the state map with random values for the returned state.

Returns: MSCEqF state

Note: THIS IS MEANT TO BE AN HELPER FUNCTION FOR DEBUG/TESTING

◆ stochasticCloning()

void msceqf::MSCEqFState::stochasticCloning ( const fp & timestamp )

Augment the MSCEqF clones map with a new clone of the actual E element of the MSCEqF state. The new clone is mapped via the given timestamp.

Parameters

timestamp Timestamp of the new clone

◆ subCov()

const MatrixX msceqf::MSCEqFState::subCov ( const std::vector< MSCEqFKey > & keys ) const

nodiscard

Get a constant copy of the the covariance submatrix (including cross-correlations) constructed with covariance blocks relative to the elements (states or clones) corresponding to the given keys. The ordering of the covariance returned follows the ordering of the given keys.

Parameters

keys	Vector of state elements name, feature id or timestamp of clone

Returns: Stacked blocks of the covariance matrix corresponding to the given keys including cross-correlations

◆ subCovCols()

const MatrixX msceqf::MSCEqFState::subCovCols ( const std::vector< MSCEqFKey > & keys ) const

nodiscard

Get a constant copy of the the covariance submatrix (including cross-correlations) constructed with covariance columns relative to the elements (states or clones) corresponding to the given keys. The ordering of the covariance returned follows the ordering of the given keys.

Parameters

keys	Vector of state elements name, feature id or timestamp of clone

Returns: Stacked columns of the covariance matrix corresponding to the given keys including cross-correlations

◆ toString()

static std::string msceqf::MSCEqFState::toString ( const MSCEqFStateKey & key )

static

Get a string describing the given MSCEqFStateKey.

Parameters

key	State element name or feature id

Returns: Key as string

The documentation for this class was generated from the following file:

include/msceqf/state/state.hpp

Public Types

Public Member Functions

Static Public Member Functions

Friends

Detailed Description

Constructor & Destructor Documentation

◆ MSCEqFState()

Member Function Documentation

◆ B()

◆ C()

◆ clone()

◆ clonesSize()

◆ cloneTimestampToMarginalize()

◆ cov()

◆ covBlock()

◆ D()

◆ delta()

◆ dof()

◆ E()

◆ index()

◆ initializeStateElement()

◆ L()

◆ marginalizeCloneAt()

◆ operator*()

◆ opts()

◆ Q()

◆ Random()

◆ stochasticCloning()

◆ subCov()

◆ subCovCols()

◆ toString()