Lie Groups

SO(2)

template<int A = 0> class InEKF::SO2 : public InEKF::LieGroup<SO2<A>, calcStateDim(2, 0, A), 2, A>

2D rotational states, also known as the 2x2 special orthogonal group, SO(2).

Template Parameters: A – Number of augmented Euclidean states. Can be Eigen::Dynamic if desired. Defaults to 0.

Public Functions

inline SO2(const MatrixState &State = MatrixState::Identity(), const MatrixCov &Cov = MatrixCov::Zero(c, c), const VectorAug &Aug = VectorAug::Zero(a))

Construct SO2 object with all available options.

Parameters

State – A 2x2 Eigen matrix. Defaults to the identity.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline SO2(const SO2 &State)

Copy constructor. Initialize with another SO2 object.

Parameters: State – SO2 object. The matrix, covariance and augmented state will all be copied from it.

inline SO2(double theta, const MatrixCov &Cov = MatrixCov::Zero(c, c), const VectorAug &Aug = VectorAug::Zero(a))

Construct a new SO2 object using an angle.

Parameters

theta – Angle of rotation matrix in radians.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline SO2(const TangentVector &xi, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SO2 object from a tangent vector using the exponential operator.

Parameters

xi – Tangent vector of size (1 + Augmented state size).
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline ~SO2(): Destroy the SO2 object.

inline SO2 R() const

Gets rotational component of the state. In the SO2 case, this is everything except the augmented Euclidean states and covariance.

Returns: SO2<> Rotational component of the state.

void addAug(double x, double sigma = 1)

Adds an element to the augmented Euclidean state. Only usable if A = Eigen::Dynamic.

Parameters

x – Variable to add.
sigma – Covariance of element. Only used if state is uncertain.

SO2<A> inverse() const

Invert state.

Returns: Inverted matrix (transpose). Augmented portion and covariance is dropped.

SO2<A> operator*(const SO2<A> &rhs) const

Combine rotations. Augmented states are summed.

Parameters: rhs – Right hand element of multiplication.
Returns: Combined elements with same augmented size.

Public Static Functions

static MatrixState wedge(const TangentVector &xi)

Move element in R^n to the Lie algebra.

Parameters: xi – Tangent vector
Returns: MatrixState Element of Lie algebra

static SO2 exp(const TangentVector &xi)

Move an element from R^n -> algebra -> group.

Parameters: xi – Tangent vector
Returns: Element of SO2

static TangentVector log(const SO2 &g)

Move an element from group -> algebra -> R^n.

Parameters: g – Group element
Returns: TangentVector

static MatrixCov Ad(const SO2 &g)

Compute the linear map Adjoint.

Parameters: g – Element of SO2
Returns: Matrix of size state dimension x state dimension

Public Static Attributes

static constexpr int rotSize = 2: Size of rotational component of group.

static constexpr int N = calcStateDim(rotSize, 0, A): Dimension of group.

static constexpr int M = calcStateMtxSize(rotSize, 0): State will have matrix of size M x M.

static constexpr int a = A == Eigen::Dynamic ? 0 : A: Handles defaults values of augmented sizes when A is Eigen::Dyanmic.

static constexpr int c = A == Eigen::Dynamic ? 1 : N: Handles defaults values of tangent vector sizes when A is Eigen::Dyanmic.

static constexpr int m = M : Handles defaults values of matrix sizes.

SE(2)

template<int C = 1, int A = 0> class InEKF::SE2 : public InEKF::LieGroup<SE2<C, A>, calcStateDim(2, C, A), calcStateMtxSize(2, C), A>

2D rigid body transformation, also known as the 3x3 special Euclidean group, SE(2).

Template Parameters

C – Number of Euclideans columns to include. Can be Eigen::Dynamic. Defaults to 1.
A – Number of augmented Euclidean states. Can be Eigen::Dynamic if desired. Defaults to 0.

Public Functions

inline SE2(const MatrixState &State = MatrixState::Identity(m, m), const MatrixCov &Cov = MatrixCov::Zero(c, c), const VectorAug &Aug = VectorAug::Zero(a, 1))

Construct SE2 object with all available options.

Parameters

State – An MxM Eigen matrix. Defaults to the identity.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline SE2(const SE2 &State)

Copy constructor. Initialize with another SE2 object.

Parameters: State – SE2 object. The matrix, covariance and augmented state will all be copied from it.

SE2(const TangentVector &xi, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SE2 object from a tangent vector using the exponential operator.

Parameters

xi – Tangent vector of size (1 + 2*Columns + Augmented state size).
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline SE2(double theta, double x, double y, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SE2 object using an theta, x, y values. Only works if C=1.

Parameters

theta – Angle of rotate in radians.
x – X-distance
y – Y-distance
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline ~SE2(): Destroy the SE2 object.

inline SO2 R() const

Gets rotational component of the state.

Returns: SO2<> Rotational component of the state.

inline Eigen::Vector2d operator[](int idx) const

Gets the ith positional column of the group.

Parameters: idx – Index of column to get, from 0 to C-1.
Returns: Eigen::Vector2d

void addCol(const Eigen::Vector2d &x, const Eigen::Matrix2d &sigma = Eigen::Matrix2d::Identity())

Adds a column to the matrix state. Only usable if C = Eigen::Dynamic.

Parameters

x – Column to add in.
sigma – Covariance of element. Only used if state is uncertain.

void addAug(double x, double sigma = 1)

Adds an element to the augmented Euclidean state. Only usable if A = Eigen::Dynamic.

Parameters

x – Variable to add.
sigma – Covariance of element. Only used if state is uncertain.

SE2 inverse() const

Invert state.

Returns: Inverted matrix. Augmented portion and covariance is dropped.

SE2 operator*(const SE2 &rhs) const

Combine transformations. Augmented states are summed.

Parameters: rhs – Right hand element of multiplication.
Returns: Combined elements with same augmented size.

Public Static Functions

static MatrixState wedge(const TangentVector &xi)

Move element in R^n to the Lie algebra.

Parameters: xi – Tangent vector
Returns: MatrixState Element of Lie algebra

static SE2 exp(const TangentVector &xi)

Move an element from R^n -> algebra -> group.

Parameters: xi – Tangent vector
Returns: Element of SE2

static TangentVector log(const SE2 &g)

Move an element from group -> algebra -> R^n.

Parameters: g – Group element
Returns: TangentVector

static MatrixCov Ad(const SE2 &g)

Compute the linear map Adjoint.

Parameters: g – Element of SE2
Returns: Matrix of size state dimension x state dimension

Public Static Attributes

static constexpr int rotSize = 2: Size of rotational component of group.

static constexpr int N = calcStateDim(rotSize, C, A): Dimension of group.

static constexpr int M = calcStateMtxSize(rotSize, C): State will have matrix of size M x M.

static constexpr int a = A == Eigen::Dynamic ? 0 : A: Handles defaults values of augmented sizes when A is Eigen::Dyanmic.

static constexpr int c = N == Eigen::Dynamic ? 3 : N: Handles defaults values of tangent vector sizes when A is Eigen::Dyanmic.

static constexpr int m = M == Eigen::Dynamic ? 3 : M: Handles defaults values of matrix sizes.

SO(3)

template<int A = 0> class InEKF::SO3 : public InEKF::LieGroup<SO3<A>, calcStateDim(3, 0, A), 3, A>

3D rotational states, also known as the 3x3 special orthogonal group, SO(3).

Template Parameters: A – Number of augmented Euclidean states. Can be Eigen::Dynamic if desired. Defaults to 0.

Public Functions

inline SO3(const MatrixState &State = MatrixState::Identity(), const MatrixCov &Cov = MatrixCov::Zero(c, c), const VectorAug &Aug = VectorAug::Zero(a))

Construct SO3 object with all available options.

Parameters

State – A 2x2 Eigen matrix. Defaults to the identity.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline SO3(const SO3 &State)

Copy constructor. Initialize with another SO3 object.

Parameters: State – SO3 object. The matrix, covariance and augmented state will all be copied from it.

SO3(double w1, double w2, double w3, const MatrixCov &Cov = MatrixCov::Zero(c, c), const VectorAug &Aug = VectorAug::Zero(a))

Construct a new SO3 object using angles and the matrix exponential.

Parameters

w1 – Angle 1.
w2 – Angle 2.
w3 – Angle 3.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline SO3(const TangentVector &xi, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SO3 object from a tangent vector using the exponential operator.

Parameters

xi – Tangent vector of size (3 + Augmented state size).
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline ~SO3(): Destroy the SO3 object.

inline SO3 R() const

Gets rotational component of the state. In the SO3 case, this is everything except the augmented Euclidean states and covariance.

Returns: SO3<> Rotational component of the state.

void addAug(double x, double sigma = 1)

Adds an element to the augmented Euclidean state. Only usable if A = Eigen::Dynamic.

Parameters

x – Variable to add.
sigma – Covariance of element. Only used if state is uncertain.

SO3<A> inverse() const

Invert state.

Returns: Inverted matrix (transpose). Augmented portion and covariance is dropped.

SO3<A> operator*(const SO3<A> &rhs) const

Combine rotations. Augmented states are summed.

Parameters: rhs – Right hand element of multiplication.
Returns: Combined elements with same augmented size.

Public Static Functions

static MatrixState wedge(const TangentVector &xi)

Move element in R^n to the Lie algebra.

Parameters: xi – Tangent vector
Returns: MatrixState Element of Lie algebra

static SO3 exp(const TangentVector &xi)

Move an element from R^n -> algebra -> group.

Parameters: xi – Tangent vector
Returns: Element of SO3

static TangentVector log(const SO3 &g)

Move an element from group -> algebra -> R^n.

Parameters: g – Group element
Returns: TangentVector

static MatrixCov Ad(const SO3 &g)

Compute the linear map Adjoint.

Parameters: g – Element of SO3
Returns: Matrix of size state dimension x state dimension

Public Static Attributes

static constexpr int rotSize = 3: Size of rotational component of group.

static constexpr int N = calcStateDim(rotSize, 0, A): Dimension of group.

static constexpr int M = calcStateMtxSize(rotSize, 0): State will have matrix of size M x M.

static constexpr int a = A == Eigen::Dynamic ? 0 : A: Handles defaults values of augmented sizes when A is Eigen::Dyanmic.

static constexpr int c = A == Eigen::Dynamic ? 3 : N: Handles defaults values of tangent vector sizes when A is Eigen::Dyanmic.

static constexpr int m = M : Handles defaults values of matrix sizes.

SE(3)

template<int C = 1, int A = 0> class InEKF::SE3 : public InEKF::LieGroup<SE3<C, A>, calcStateDim(3, C, A), calcStateMtxSize(3, C), A>

3D rigid body transformation, also known as the 4x4 special Euclidean group, SE(3).

Template Parameters

C – Number of Euclideans columns to include. Can be Eigen::Dynamic. Defaults to 1.
A – Number of augmented Euclidean states. Can be Eigen::Dynamic if desired. Defaults to 0.

Public Functions

inline SE3(const MatrixState &State = MatrixState::Identity(m, m), const MatrixCov &Cov = MatrixCov::Zero(c, c), const VectorAug &Aug = VectorAug::Zero(a, 1))

Construct SE2 object with all available options.

Parameters

State – An MxM Eigen matrix. Defaults to the identity.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline SE3(const SE3 &State)

Copy constructor. Initialize with another SE2 object.

Parameters: State – SE2 object. The matrix, covariance and augmented state will all be copied from it.

SE3(const TangentVector &xi, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SE2 object from a tangent vector using the exponential operator.

Parameters

xi – Tangent vector of size (3 + 3*Columns + Augmented state size).
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

SE3(const SO3<> R, const Eigen::Matrix<double, small_xi, 1> &xi, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SE3 object.

Parameters

R – Rotational portion of the SE3 object.
xi – Translational columns to input.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline SE3(double w1, double w2, double w3, double x, double y, double z, const MatrixCov &Cov = MatrixCov::Zero(c, c))

Construct a new SE3 object using exponential ooperator on angles and putting positions directly in.

Parameters

w1 – Rotaional component 1.
w2 – Rotaional component 2.
w3 – Rotaional component 3.
x – X-position.
y – Y-position.
z – Z-position.
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline ~SE3(): Destroy the SE3 object.

inline SO3 R() const

Gets rotational component of the state.

Returns: SO2<> Rotational component of the state.

inline Eigen::Vector3d operator[](int idx) const

Gets the ith positional column of the group.

Parameters: idx – Index of column to get, from 0 to C-1.
Returns: Eigen::Vector2d

void addCol(const Eigen::Vector3d &x, const Eigen::Matrix3d &sigma = Eigen::Matrix3d::Identity())

Adds a column to the matrix state. Only usable if C = Eigen::Dynamic.

Parameters

x – Column to add in.
sigma – Covariance of element. Only used if state is uncertain.

void addAug(double x, double sigma = 1)

Adds an element to the augmented Euclidean state. Only usable if A = Eigen::Dynamic.

Parameters

x – Variable to add.
sigma – Covariance of element. Only used if state is uncertain.

SE3 inverse() const

Invert state.

Returns: Inverted matrix. Augmented portion and covariance is dropped.

SE3 operator*(const SE3 &rhs) const

Combine transformations. Augmented states are summed.

Parameters: rhs – Right hand element of multiplication.
Returns: Combined elements with same augmented size.

Public Static Functions

static MatrixState wedge(const TangentVector &xi)

Move element in R^n to the Lie algebra.

Parameters: xi – Tangent vector
Returns: MatrixState Element of Lie algebra

static SE3 exp(const TangentVector &xi)

Move an element from R^n -> algebra -> group.

Parameters: xi – Tangent vector
Returns: Element of SE2

static TangentVector log(const SE3 &g)

Move an element from group -> algebra -> R^n.

Parameters: g – Group element
Returns: TangentVector

static MatrixCov Ad(const SE3 &g)

Compute the linear map Adjoint.

Parameters: g – Element of SE2
Returns: Matrix of size state dimension x state dimension

Public Static Attributes

static constexpr int rotSize = 3: Size of rotational component of group.

static constexpr int N = calcStateDim(rotSize, C, A): Dimension of group.

static constexpr int M = calcStateMtxSize(rotSize, C): State will have matrix of size M x M.

static constexpr int a = A == Eigen::Dynamic ? 0 : A: Handles defaults values of augmented sizes when A is Eigen::Dyanmic.

static constexpr int c = N == Eigen::Dynamic ? 6 : N: Handles defaults values of tangent vector sizes when A is Eigen::Dyanmic.

static constexpr int m = M == Eigen::Dynamic ? 4 : M: Handles defaults values of matrix sizes.

Lie Group Base

template<class Class, int N, int M, int A> class InEKF::LieGroup

Base Lie Group Class.

Template Parameters

Class – Class that is inheriting from it. Allows for better polymorphism
N – Group dimension
M – Lie Group matrix size
A – Augmented Euclidean state size

Public Types

typedef Eigen::Matrix<double, N, 1> TangentVector: A tangent vector has size Nx1, where N is the state dimension.

typedef Eigen::Matrix<double, N, N> MatrixCov: The covariance matrix has size NxN, where N is the state dimension.

typedef Eigen::Matrix<double, M, M> MatrixState: A group element is a matrix of size MxM.

typedef Eigen::Matrix<double, A, 1> VectorAug: Vector of additional Euclidean states, of size Ax1.

Public Functions

inline LieGroup(): Construct a new Lie Group object. Default Contructor.

inline LieGroup(MatrixState State, MatrixCov Cov, VectorAug Aug)

Construct a new Lie Group object.

Parameters

State – Group element
Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline LieGroup(MatrixCov Cov, VectorAug Aug)

Construct a new Lie Group object.

Parameters

Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.
Aug – Additional euclidean states if A != 0. Defaults to 0s.

inline LieGroup(MatrixCov Cov)

Construct a new Lie Group object.

Parameters: Cov – Covariance of state. If not input, state is set as “certain” and covariance is not tracked.

inline virtual ~LieGroup(): Destroy the Lie Group object.

inline bool uncertain() const

Returns whether object is uncertain, ie if it has a covariance.

Returns: true
Returns: false

inline const MatrixCov &cov() const

Get covariance of group element.

Returns: const MatrixCov&

inline const VectorAug &aug() const

Get additional Euclidean state of object.

Returns: const VectorAug&

inline const MatrixState &mat() const

Get actual group element.

Returns: const MatrixState&

inline const MatrixState &operator()() const

Get actual group element.

Returns: const MatrixState&

inline void setCov(const MatrixCov &Cov)

Set the state covariance.

Parameters: Cov – Covariance matrix.

inline void setAug(const VectorAug &Aug)

Set the additional augmented state.

Parameters: Aug – Augmented state vector.

inline void setMat(const MatrixState &State)

Set the group element.

Parameters: State – matrix Lie group element.

inline const Class &derived() const

Cast LieGroup object to object that is inheriting from it.

Returns: const Class&

inline Class inverse() const

Invert group element.

Returns: Inverted group element. Augmented portion and covariance is dropped.

inline TangentVector log() const

Move this element from group -> algebra -> R^n.

Returns: TangentVector

inline MatrixCov Ad() const

Get adjoint of group element.

Returns: MatrixCov

inline Class compose(const Class &g) const

Multiply group elements.

Parameters: g – Group element.
Returns: Class

inline std::string toString() const

Convert group element to string. If uncertain print covariance as well. If has augmented state, print that as well.

Returns: std::string

Public Static Functions

static inline MatrixState wedge(const TangentVector &xi)

Move element in R^n to the Lie algebra.

Parameters: xi – Tangent vector
Returns: MatrixState Element of Lie algebra

static inline Class exp(const TangentVector &xi)

Move an element from R^n -> algebra -> group.

Parameters: xi – Tangent vector
Returns: Element of SO3

static inline TangentVector log(const Class &g)

Move an element from group -> algebra -> R^n.

Parameters: g – Group element
Returns: TangentVector

static inline MatrixCov Ad(const Class &g)

Compute the linear map Adjoint.

Parameters: g – Element of SO3
Returns: Matrix of size state dimension x state dimension

Helpers

constexpr int InEKF::calcStateDim(int rotMtxSize, int C, int A)

Computes total dimension of the state.

Parameters

rotMtxSize – Matrix size of the rotational component of the group.
C – Number of columns to be included.
A – Number of Euclidean states included.

Returns

Total dimension of state.

constexpr int InEKF::calcStateMtxSize(int rotMtxSize, int C)

Compute group matrix size.

Parameters

rotMtxSize – Matrix size of the rotational component of the group.
C – Number of columns to be included.

Returns

Total dimension of state.