Namespace inertialsim::geometry¶

Namespace List > inertialsim > geometry

Coordinates, rotations, rigid transforms, and geometric utilities. More...

Namespaces¶

Type	Name
namespace	axis_angle Axis-angle rotation representation conversions (internal API).
namespace	cartesian Conversions to cartesian coordinates (internal API).
namespace	euler_angles Euler angle rotation representation conversions (internal API).
namespace	legendre Associated Legendre polynomials (internal API).
namespace	matrix Rotation matrix representation conversions (internal API).
namespace	power_series Power series approximations for common functions (internal API).
namespace	quaternion Quaternion rotation representation conversions (internal API).
namespace	random Random rotation generation utilities (internal API).
namespace	rotation_vector Rotation vector representation conversions (internal API).
namespace	spherical Conversions to spherical coordinates (internal API).

Classes¶

Type	Name
class	CoordinateRotation 3-dimensional coordinate frame rotations (passive convention).
class	CoordinateTransform 3-dimensional coordinate frame transforms (passive convention).
class	CoordinateTranslation 3-dimensional coordinate frame translations (passive convention).
class	ExtendedPose Pose plus velocity.
class	FloatingPointTolerances Singleton class for managing floating point tolerance settings.
class	MatrixLieGroup <typename Derived, GroupDimension, AlgebraDimension> Abstract base class for matrix Lie groups.
class	Pose Rigid body pose (position and orientation).
class	RigidTransform 3-dimensional rigid body transforms, SE(3) .
class	RigidTransformBase <typename Derived> Base class for 3-dimensional rigid body transform types.
class	Rotation 3-dimensional rotations of rigid bodies, SO(3) .
class	RotationBase <typename Derived> Base class for 3-dimensional rotation types.
class	Translation 3-dimensional translations.
class	Vector 3-dimensional vectors.
class	VectorBase <typename Derived> Base class for 3-dimensional vector types.

Public Types¶

Type	Name
enum	Integrator Integration methods.
enum	Interpolator

Public Attributes¶

Type	Name
const std::map< char, int >	kAxisIndex Map from axis character ( `X` ,`Y` ,`Z` ) to index.
const std::map< char, Vector3D >	kCartesianBasis Map from axis character ( `X` ,`Y` ,`Z` ) to basis vector.
constexpr double	kDefaultFpTolerance = `100.0 \* std::numeric\_limits<[double](namespaceinertialsim.md#typedef-matrix)>[::epsilon](namespaceinertialsim.md#typedef-matrix)()` Standard tolerance for floating point equivalence checks.
constexpr double	kMetersPerInternationalFoot = `0.3048` Common unit conversions.
constexpr int	kNumCartesianAxes = `3` Standard number of Cartesian axes ( `3` ).
constexpr double	kPi = `3.14159265358979323846` Common value of pi.
const Vector3D	kXBasis Standard Cartesian basis `[1, 0, 0]` .
const Vector3D	kYBasis Standard Cartesian basis `[0, 1, 0]` .
const Vector3D	kZBasis Standard Cartesian basis `[0, 0, 1]` .

Public Functions¶

Type	Name
Vector3D	CrossProduct (const Vector3D & a, const Vector3D & b) Compute the column-wise cross product of 3D vectors.
Array	CumulativeIntegral (const Array & array, const Timestamps & time, Integrator method=Integrator::TRAPEZOID, const std::optional< Array > initial_value=std::nullopt) Compute column-wise cumulative integral of an array.
Array	CumulativeSum (const Array & array) Compute column-wise cumulative sum of an array.
double	Deg2Rad (double degrees) Converts degrees to radians (scalar version).
Array	Deg2Rad (const Array & degrees) Converts degrees to radians (array version).
Scalar1D	DotProduct (const Vector3D & a, const Vector3D & b) Compute the column-wise dot (inner) product of 3D vectors.
std::vector< Scalar1D >	FiniteDifferenceWeights (const Timestamps & time, int order, int accuracy) Compute finite difference weights.
bool	IsOrthonormal (const RotationMatrix & matrix, std::optional< double > orthogonal_threshold=std::nullopt, std::optional< double > normal_threshold=std::nullopt) Check if a 3x3 matrix is orthonormal.
BooleanArray	IsOrthonormal (const RotationMatrixArray & matrices, std::optional< double > orthogonal_threshold=std::nullopt, std::optional< double > normal_threshold=std::nullopt) Check if an array of 3x3 matrices are orthonormal.
bool	IsSorted (const Scalar1D & array) Checks if an array is sorted.
bool	IsStrictlySorted (const Scalar1D & array) Checks if an array is strictly sorted.
bool	IsUnit (double value) Check if input is close to one.
BooleanArray	IsUnit (const Array & array) Check if elements of an array are close to one.
bool	IsZero (double value) Check if input is close to zero.
BooleanArray	IsZero (const Array & array) Check if elements of an array are close to zero.
int	LeviCivita (int i, int j, int k) Levi-Civita symbol.
std::pair< Matrix, Scalar1D >	NormalizeVector (const Matrix & vectors) Normalize an array of vectors.
double	Rad2Deg (double radians) Converts radians to degrees (scalar version).
Array	Rad2Deg (const Array & radians) Converts radians to degrees (array version).
int	Sign (double value) Element-wise sign function.
RotationMatrix	SkewMatrix (const Eigen::Vector3d & vector) Create a skew-symmetric matrix from a single 3D vector.
RotationMatrixArray	SkewMatrix (const Vector3D & vector) Create skew-symmetric matrices from multiple 3D vectors.
std::vector< Array >	SlidingWindow (const Array & array, int window_size) Column-wise sliding windows over an array.
IntegerArray	VectorArgmax (const Matrix & vectors) Find the argmax for each vector in an array.
Array	WrapTo2Pi (const Array & angle) Wraps the input angles such that `0 <= angle < 2*pi` .
Array	WrapToPi (const Array & angle) Wraps the input angles such that `-pi < angle <= pi` .

Detailed Description¶

The geometry namespace provides:

methods to convert and validate user supplied inputs (arrays, vectors, matrices)
classes to represent and manipulate geometric quantities (vectors, translations, rotations, rigid transforms, poses, etc.) based on Lie group theory
classes to represent passive coordinate transforms

Input conventions

All inputs are expected to be in SI units.

By inertialsim convention, rotations and translations represent an active transformation (they act to rotate and translate an object relative to a fixed coordinate system). This is a common source of bugs, particularly in rotation logic. Reference [02], Reference [05], and Reference [11] discuss active vs. passive transforms in greater detail.

Because these conventions cannot be automatically validated, users must be aware of them and manage inputs and outputs.

Open-source packages

This module is tested for consistency against the following open source packages:

SciPy Spatial Transforms module: https://docs.scipy.org/doc/scipy/reference/spatial.transform.html
DFKI-RIC pytransform3d package: https://dfki-ric.github.io/pytransform3d/index.html

Public Types Documentation¶

enum Integrator¶

Integration methods.

enum inertialsim::geometry::Integrator {
    EULER,
    TRAPEZOID,
    RK4
};

Numerical integration methods for sampled data:

EULER: Forward integration using a single sample.
TRAPEZOID: Trapezoidal rule using average of samples at start and end.
RK4: Runge-Kutta 4^th order algorithm using 3 samples (start, mid, end). Equivalent to Simpson's rule for sampled data.

enum Interpolator¶

enum inertialsim::geometry::Interpolator {
    LINEAR,
    CUBIC
};

Public Attributes Documentation¶

variable kAxisIndex¶

Map from axis character ( X ,Y ,Z ) to index.

const std::map<char, int> inertialsim::geometry::kAxisIndex;

variable kCartesianBasis¶

Map from axis character ( X ,Y ,Z ) to basis vector.

const std::map<char, Vector3D> inertialsim::geometry::kCartesianBasis;

variable kDefaultFpTolerance¶

Standard tolerance for floating point equivalence checks.

constexpr double inertialsim::geometry::kDefaultFpTolerance;

Value: 100 * std::numeric_limits<double>:: epsilon()

variable kMetersPerInternationalFoot¶

Common unit conversions.

constexpr double inertialsim::geometry::kMetersPerInternationalFoot;

Conversion factor from international feet to meters.

Note

This is the international foot and not the U.S. survey foot. Some older inertial navigation references use the latter. As of 1/1/2023, NIST recommends avoiding any use of the survey foot. The value of the international foot is defined in Standard [01]. See: https://www.nist.gov/pml/us-surveyfoot

variable kNumCartesianAxes¶

Standard number of Cartesian axes ( 3 ).

constexpr int inertialsim::geometry::kNumCartesianAxes;

variable kPi¶

Common value of pi.

constexpr double inertialsim::geometry::kPi;

variable kXBasis¶

Standard Cartesian basis [1, 0, 0] .

const Vector3D inertialsim::geometry::kXBasis;

variable kYBasis¶

Standard Cartesian basis [0, 1, 0] .

const Vector3D inertialsim::geometry::kYBasis;

variable kZBasis¶

Standard Cartesian basis [0, 0, 1] .

const Vector3D inertialsim::geometry::kZBasis;

Public Functions Documentation¶

function CrossProduct¶

Compute the column-wise cross product of 3D vectors.

Vector3D inertialsim::geometry::CrossProduct (
    const  Vector3D & a,
    const  Vector3D & b
)

Computes the cross product \(a \times b\) for each pair of column vectors.

Parameters:

a First vector array (3xN).
b Second vector array (3xN).

Returns:

Cross product result \(a \times b\) (3xN).

function CumulativeIntegral¶

Compute column-wise cumulative integral of an array.

Array inertialsim::geometry::CumulativeIntegral (
    const  Array & array,
    const  Timestamps & time,
    Integrator method=Integrator::TRAPEZOID,
    const std::optional< Array > initial_value=std::nullopt
)

Computes the cumulative integral of the input array over time using the specified integration method. Each row (axis) is integrated independently across columns (time samples).

Parameters:

array The array to integrate (MxN where rows are independent axes, columns are time samples).
time The time array (1xN).
method Integration method (default: TRAPEZOID).
initial_value Optional initial value (Mx1) for the integral (defaults to zeros if not provided).

Returns:

The cumulative integral (same dimensions as input).

function CumulativeSum¶

Compute column-wise cumulative sum of an array.

Array inertialsim::geometry::CumulativeSum (
    const  Array & array
)

Computes the cumulative sum of the input array. Each row (axis) is summed independently along columns (time samples).

Parameters:

array The array to sum (MxN where rows are independent axes, columns are time samples).

Returns:

The cumulative sum (same dimensions as input).

function Deg2Rad¶

Converts degrees to radians (scalar version).

inline double inertialsim::geometry::Deg2Rad (
    double degrees
)

Parameters:

degrees Angle in degrees.

Returns:

Angle in radians.

function Deg2Rad¶

Converts degrees to radians (array version).

inline Array inertialsim::geometry::Deg2Rad (
    const  Array & degrees
)

Parameters:

degrees Angle(s) in degrees.

Returns:

Angle(s) in radians.

function DotProduct¶

Compute the column-wise dot (inner) product of 3D vectors.

Scalar1D inertialsim::geometry::DotProduct (
    const  Vector3D & a,
    const  Vector3D & b
)

Computes the dot product \(a \cdot b\) for each pair of column vectors.

Parameters:

a First vector array (3xN).
b Second vector array (3xN).

Returns:

Dot product result \(a \cdot b\) (1xN).

function FiniteDifferenceWeights¶

Compute finite difference weights.

std::vector< Scalar1D > inertialsim::geometry::FiniteDifferenceWeights (
    const  Timestamps & time,
    int order,
    int accuracy
)

Calculates the weights for a Fornberg-style finite difference approximation of a function sampled at the input times. The order input defines the difference order (e.g., 1 for first derivative, 2 for second derivative). The accuracy input defines the approximation order of the result and should be a positive even integer (2, 4, 6, etc.). An accuracy of n requires n+1 neighboring samples.

If there are fewer samples than required for the requested accuracy, the accuracy will be reduced to the maximum possible. Central points will use a central difference scheme while edge points will use a full or partial forward or backward scheme with the requested accuracy.

See Reference [34] for details.

Parameters:

time Sample times.
order Order of the finite difference approximation (0, 1, 2, ...).
accuracy Accuracy of the finite difference approximation (positive even integer: 2, 4, 6, ...).

Returns:

Finite difference weights for each timestamp.

Exception:

std::invalid_argument if order < 0 or insufficient samples.

function IsOrthonormal¶

Check if a 3x3 matrix is orthonormal.

bool inertialsim::geometry::IsOrthonormal (
    const  RotationMatrix & matrix,
    std::optional< double > orthogonal_threshold=std::nullopt,
    std::optional< double > normal_threshold=std::nullopt
)

A matrix is orthonormal if its columns (or rows) are orthogonal unit vectors. An orthonormal matrix has determinant = 1.0 and rows/columns are orthogonal to each other. This is equivalent to checking that \(M^T M = I\) and \(\det(M) = 1\) within specified tolerances. All rotation matrices are orthonormal.

Parameters:

matrix The 3x3 matrix to check.
orthogonal_threshold Threshold for orthogonality check (nullopt uses default from FloatingPointTolerances singleton). Columns must be orthogonal within this tolerance.
normal_threshold Threshold for normality check (nullopt uses default from FloatingPointTolerances). Column norms must be 1.0 within this tolerance.

Returns:

True if the matrix is orthonormal within the specified thresholds.

function IsOrthonormal¶

Check if an array of 3x3 matrices are orthonormal.

BooleanArray inertialsim::geometry::IsOrthonormal (
    const  RotationMatrixArray & matrices,
    std::optional< double > orthogonal_threshold=std::nullopt,
    std::optional< double > normal_threshold=std::nullopt
)

A matrix is orthonormal if its columns (or rows) are orthogonal unit vectors. An orthonormal matrix has determinant = 1.0 and rows/columns are orthogonal to each other. This is equivalent to checking that \(M^T M = I\) and \(\det(M) = 1\) within specified tolerances. All rotation matrices are orthonormal.

Parameters:

matrices Vector of 3x3 matrices to check.
orthogonal_threshold Threshold for orthogonality check (nullopt uses default from FloatingPointTolerances singleton). Columns must be orthogonal within this tolerance.
normal_threshold Threshold for normality check (nullopt uses default from FloatingPointTolerances). Column norms must be 1.0 within this tolerance.

Returns:

Boolean array indicating orthonormality of each matrix.

function IsSorted¶

Checks if an array is sorted.

bool inertialsim::geometry::IsSorted (
    const  Scalar1D & array
)

Checks if the input array is sorted in non-decreasing order. Duplicates are permitted. See IsStrictlySorted() to exclude duplicates.

Example

Scalar1D a = {-2.2, 0.0, 1.2, 1.2, 5.5};
IsSorted(a);  // returns true

Parameters:

array Scalar array of values (1xN).

Returns:

True if sorted (allowing duplicates), false otherwise.

function IsStrictlySorted¶

Checks if an array is strictly sorted.

bool inertialsim::geometry::IsStrictlySorted (
    const  Scalar1D & array
)

Checks if the input array is sorted in strictly increasing order. Duplicates are not permitted. See IsSorted() to include duplicates.

Example

Scalar1D a = {-2.2, 0.0, 1.2, 5.5};
IsStrictlySorted(a);  // returns true
Scalar1D b = {-2.2, 0.0, 1.2, 1.2, 5.5};
IsStrictlySorted(b);  // returns false (duplicate 1.2)

Parameters:

array Scalar array of values (1xN).

Returns:

True if strictly sorted (no duplicates), false otherwise.

function IsUnit¶

Check if input is close to one.

bool inertialsim::geometry::IsUnit (
    double value
)

Checks if the value is close to one. The global tolerance currently set in the FloatingPointTolerances singleton is used for the comparison.

Parameters:

value The value to check.

Returns:

True if the value is "close" to one.

function IsUnit¶

Check if elements of an array are close to one.

BooleanArray inertialsim::geometry::IsUnit (
    const  Array & array
)

Checks if the elements of the array are close to one. The global tolerance currently set in the FloatingPointTolerances singleton is used for the comparison.

Parameters:

array The array to check.

Returns:

Elements close to one (boolean array).

function IsZero¶

Check if input is close to zero.

bool inertialsim::geometry::IsZero (
    double value
)

Checks if the value is close to zero. The global tolerance currently set in the FloatingPointTolerances singleton is used for the comparison.

The primary purpose of this function is to avoid numerical precision issues in expressions with small divisors. For example: mathematically sin(0.5x)/x has a well-behaved limit and tends to 0.5 as x tends to 0. However, this expression experiences numerical issues when x is extremely small.

Parameters:

value The value to check.

Returns:

True if the value is "close" to zero within tolerance.

function IsZero¶

Check if elements of an array are close to zero.

BooleanArray inertialsim::geometry::IsZero (
    const  Array & array
)

Checks if the elements of the array are close to zero. The global tolerance currently set in the FloatingPointTolerances singleton is used for the comparison.

Parameters:

array The array to check.

Returns:

Elements close to zero (boolean array).

function LeviCivita¶

Levi-Civita symbol.

int inertialsim::geometry::LeviCivita (
    int i,
    int j,
    int k
)

The Levi-Civita symbol is defined as:

+1 if the sequence is an even permutation of (0,1,2) (cyclical)
-1 if the sequence is an odd permutation of (0,1,2) (anti-cyclical)
0 otherwise (if any two indices are equal)

Parameters:

i First index in 3-dimensional sequence (must be 0, 1, or 2).
j Second index in 3-dimensional sequence (must be 0, 1, or 2).
k Third index in 3-dimensional sequence (must be 0, 1, or 2).

Returns:

+1 for even permutations, -1 for odd permutations, 0 otherwise.

Exception:

std::invalid_argument if any index is not in {0, 1, 2}.

function NormalizeVector¶

Normalize an array of vectors.

std::pair< Matrix , Scalar1D > inertialsim::geometry::NormalizeVector (
    const  Matrix & vectors
)

Normalizes vectors to unit length: u = v / norm(v), so that norm(u) = 1. Handles divide-by-zero gracefully by detecting zero-length vectors and returning them unchanged. The length (norm) of each vector is also returned.

Parameters:

vectors Matrix where each column is a vector to normalize (MxN).

Returns:

Pair containing:

Unit vectors (same dimensions as input)
Array of norms corresponding to each unit vector (1xN)

function Rad2Deg¶

Converts radians to degrees (scalar version).

inline double inertialsim::geometry::Rad2Deg (
    double radians
)

Parameters:

radians Angle in radians.

Returns:

Angle in degrees.

function Rad2Deg¶

Converts radians to degrees (array version).

inline Array inertialsim::geometry::Rad2Deg (
    const  Array & radians
)

Parameters:

radians Angle(s) in radians.

Returns:

Angle(s) in degrees.

function Sign¶

Element-wise sign function.

int inertialsim::geometry::Sign (
    double value
)

Unlike the standard sign function, 0 is treated as positive (+1).

Example

For input [-2, -1, 0, 1, 2], returns [-1, -1, 1, 1, 1]

Parameters:

value The input value.

Returns:

+1 if value >= 0, -1 if value < 0.

function SkewMatrix¶

Create a skew-symmetric matrix from a single 3D vector.

RotationMatrix inertialsim::geometry::SkewMatrix (
    const Eigen::Vector3d & vector
)

Returns the skew-symmetric matrix equivalent to the input vector. This matrix is also known as the cross product matrix or anti-symmetric matrix. It is the matrix equivalent of the cross product operator such that: \(a = b \times c\) is equivalent to \(a = [b]_{\times} c\), where \([b]_{\times}\) is the skew-symmetric matrix.

Parameters:

vector A 3D vector.

Returns:

The 3x3 skew-symmetric (anti-symmetric) matrix.

function SkewMatrix¶

Create skew-symmetric matrices from multiple 3D vectors.

RotationMatrixArray inertialsim::geometry::SkewMatrix (
    const  Vector3D & vector
)

Returns the skew-symmetric matrix equivalent to each input vector. This matrix is also known as the cross product matrix or anti-symmetric matrix. It is the matrix equivalent of the cross product operator such that: \(a = b \times c\) is equivalent to \(a = [b]_{\times} c\), where \([b]_{\times}\) is the skew-symmetric matrix.

Parameters:

vectors Matrix where each column is a 3D vector (3xN).

Returns:

Array of 3x3 skew-symmetric (anti-symmetric) matrices (one per input vector).

function SlidingWindow¶

Column-wise sliding windows over an array.

std::vector< Array > inertialsim::geometry::SlidingWindow (
    const  Array & array,
    int window_size
)

Creates an array of sliding windows (column-wise) over the input. window_size must be a positive integer.

The resulting view is centered on each input element. For edge points where the view cannot be centered, it returns the first window_size elements or the last window_size elements. For even window_size, the window is biased left of the input point.

The purpose of this function is to support windowed algorithms like interpolation and numerical differentiation.

Parameters:

array The input array.
window_size Size of each window (must be positive and <= array size).

Returns:

Vector of windowed arrays.

Exception:

std::invalid_argument if window_size > array size or window_size < 1.

function VectorArgmax¶

Find the argmax for each vector in an array.

IntegerArray inertialsim::geometry::VectorArgmax (
    const  Matrix & vectors
)

For each column vector, finds the index of the maximum component (0 <= index < M for Mx1 vectors). In the case of multiple occurrences of the same value, returns the index of the first occurrence.

Example

For vector [[0.1], [0.5], [0.3]], returns 1 (the row index of 0.5).

Parameters:

vectors Matrix where each column is a vector (MxN).

Returns:

Indices of the maximum component of each column (1xN).

function WrapTo2Pi¶

Wraps the input angles such that 0 <= angle < 2*pi .

Array inertialsim::geometry::WrapTo2Pi (
    const  Array & angle
)

Parameters:

angle Array of angles. The operation is element-wise.

Returns:

Array of angles wrapped between [0, 2*pi).

function WrapToPi¶

Wraps the input angles such that -pi < angle <= pi .

Array inertialsim::geometry::WrapToPi (
    const  Array & angle
)

Parameters:

angle Array of angles. The operation is element-wise.

Returns:

Array of angles wrapped between (-pi, pi].

The documentation for this class was generated from the following file cpp/include/inertialsim/geometry/angle.h