Class sym::Unit3#

template<typename ScalarType> class Unit3#

Autogenerated C++ implementation of symforce.geo.unit3.Unit3.

Direction in R^3, represented as a :class:Rot3 <symforce.geo.rot3.Rot3> that transforms [0, 0, 1] to the desired direction.

The storage is therefore a quaternion and the tangent space is 2 dimensional. Most operations are implemented using operations from :class:Rot3 <symforce.geo.rot3.Rot3>.

Note: an alternative implementation could directly store a unit vector and define its boxplus manifold as described in Appendix B.2 of [Hertzberg 2013]. This can be done by finding the Householder reflector of x and use it to transform the exponential map of delta, which is a small perturbation in the tangent space (R^2). Namely::

x.retract(delta) = x [+] delta = Rx * Exp(delta), where
Exp(delta) = [sinc(||delta||) * delta, cos(||delta||)], and
Rx = (I - 2 vv^T / (v^Tv))X, v = x - e_z != 0, X is a matrix negating 2nd vector component
= I                     , x = e_z

[Hertzberg 2013] Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds

Public Types

using Scalar = ScalarType #

using Self = Unit3<Scalar>#

using DataVec = Eigen::Matrix<Scalar, 4, 1>#

using TangentVec = Eigen::Matrix<Scalar, 2, 1>#

using SelfJacobian = Eigen::Matrix<Scalar, 2, 2>#

using Vector3 = Eigen::Matrix<Scalar, 3, 1>#

Public Functions

inline explicit Unit3(const DataVec &data, const bool normalize = true)#

Construct from data vec

Parameters:: normalize – Project to the manifold on construction. This ensures numerical stability as this constructor is called after each codegen operation. Constructing from a normalized vector may be faster, e.g. with FromStorage.

inline Unit3()#

inline const DataVec &Data() const#

inline Unit3(const Rot3<Scalar> &rotation)#

const Vector3 ToUnitVector() const#

This function was autogenerated from a symbolic function. Do not modify by hand.

Symbolic function: to_unit_vector

Args:

Outputs: res: Matrix31

const sym::Rot3<Scalar> ToRotation() const#

This function was autogenerated from a symbolic function. Do not modify by hand.

Symbolic function: to_rotation

Args:

Outputs: res: Rot3

inline void ToStorage(Scalar *const vec) const#

inline Self Inverse() const#

inline Self Compose(const Self &b) const#

inline Self Between(const Self &b) const#

inline Self InverseWithJacobian(SelfJacobian *const res_D_a = nullptr) const#

inline Self ComposeWithJacobians(const Self &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr) const#

inline Self BetweenWithJacobians(const Self &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr) const#

template<typename Other> inline auto operator*(const Other &b) const -> decltype(Compose(b))#

inline TangentVec ToTangent(const Scalar epsilon = kDefaultEpsilon<Scalar>) const#

inline Self Retract(const TangentVec &vec, const Scalar epsilon = kDefaultEpsilon<Scalar>) const#

inline TangentVec LocalCoordinates(const Self &b, const Scalar epsilon = kDefaultEpsilon<Scalar>) const#

inline Self Interpolate(const Self b, const Scalar alpha, const Scalar epsilon = kDefaultEpsilon<Scalar>) const#

inline bool IsApprox(const Self &b, const Scalar tol) const#

template<typename ToScalar> inline Unit3<ToScalar> Cast() const#

inline bool operator==(const Unit3 &rhs) const#

Public Static Functions

template<typename Generator> static inline Unit3 Random(Generator &gen)#

static const sym::Unit3<Scalar> FromVector(const Vector3 &a, const Scalar epsilon)#

Return a :class:Unit3 that points along the direction of vector a

a does not have to be a unit vector.

static inline constexpr int32_t StorageDim()#

static inline Unit3 FromStorage(const Scalar *const vec)#

static inline Self Identity()#

static inline constexpr int32_t TangentDim()#

static inline Self FromTangent(const TangentVec &vec, const Scalar epsilon = kDefaultEpsilon<Scalar>)#