Source code for sym.unit3

# -----------------------------------------------------------------------------
# This file was autogenerated by symforce from template:
#     geo_package/CLASS.py.jinja
# Do NOT modify by hand.
# -----------------------------------------------------------------------------


# ruff: noqa: PLR0915, F401, PLW0211, PLR0914

import math
import random
import typing as T

import numpy

# isort: split
from .ops import unit3 as ops


[docs]class Unit3(object): """ Autogenerated Python implementation of :py:class:`symforce.geo.unit3.Unit3`. Direction in R^3 represented as a unit vector on the S^2 sphere manifold. Storage is three dimensional, and tangent space is two dimensional. Due to the nature of the manifold, the unit X vector is handled as a singularity. The implementation of the retract and local_coordinates functions are based on Appendix B.2 : [Hertzberg 2013] Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds The retract operation performs a perturbation to the desired unit X vector, which is then rotated to desired position along the actual stored unit vector through a Householder-reflection + relection across the XZ plane. x.retract(delta) = x [+] delta = Rx * Exp(delta), where Exp(delta) = [cos(||delta||), sinc(||delta||) * delta], and Rx = (I - 2 vv^T / (v^Tv))X, v = x - e_x != 0, X is a matrix negating 2nd vector component = diag(1, -1, -1) , x = e_x See: `unit3_visualization.ipynb` for a visualization of the Unit3 manifold. """ __slots__ = ["data"] def __repr__(self): # type: () -> str return "<{} {}>".format(self.__class__.__name__, self.data) # -------------------------------------------------------------------------- # Handwritten methods included from "custom_methods/unit3.py.jinja" # -------------------------------------------------------------------------- def __init__(self, vec): # type: (T.Union[T.Sequence[float], numpy.ndarray]) -> None if isinstance(vec, numpy.ndarray): if vec.shape in {(3, 1), (1, 3)}: vec = vec.flatten() elif vec.shape != (3,): raise IndexError( "Expected vec to be a vector of length 3; instead had shape {}".format( vec.shape ) ) elif len(vec) != 3: raise IndexError( "Expected vec to be a sequence of length 3, was instead length {}.".format(len(vec)) ) self.data = list(vec) # type: T.List[float]
[docs] @classmethod def random(cls, epsilon=1e-8): # type: (float) -> Unit3 """ Return a random :class:`Unit3` object """ u1 = random.uniform(0, 1) u2 = random.uniform(0, 1) return Unit3.random_from_uniform_samples(u1, u2, epsilon=epsilon)
# -------------------------------------------------------------------------- # Custom generated methods # --------------------------------------------------------------------------
[docs] def basis(self, epsilon): # type: (Unit3, float) -> numpy.ndarray """ Returns a :class:`Matrix32` with the basis vectors of the tangent space (in R^3) at the current Unit3 direction. """ # Total ops: 50 # Input arrays _self = self.data # Intermediate terms (12) _tmp0 = _self[1] ** 2 _tmp1 = max( 0, -( 0.0 if _self[2] ** 2 + _tmp0 - 10 * epsilon * math.copysign(1, _self[0]) == 0 else math.copysign( 1, _self[2] ** 2 + _tmp0 - 10 * epsilon * math.copysign(1, _self[0]) ) ), ) _tmp2 = 1 - _tmp1 _tmp3 = _self[0] - 1 _tmp4 = _self[2] + epsilon * math.copysign(1, _self[2]) _tmp5 = _tmp4**2 _tmp6 = _tmp0 + _tmp5 _tmp7 = 2 / (_tmp3**2 + _tmp6) _tmp8 = 2 / _tmp6 _tmp9 = _tmp2 * _tmp4 * _tmp7 _tmp10 = _self[1] * _tmp9 _tmp11 = _self[1] * _tmp1 * _tmp4 * _tmp8 # Output terms _res = numpy.zeros((3, 2)) _res[0, 0] = _self[1] * _tmp2 * _tmp3 * _tmp7 _res[1, 0] = -_tmp1 * (-_tmp0 * _tmp8 + 1) - _tmp2 * (-_tmp0 * _tmp7 + 1) _res[2, 0] = _tmp10 + _tmp11 _res[0, 1] = -_tmp3 * _tmp9 _res[1, 1] = -_tmp10 - _tmp11 _res[2, 1] = _tmp1 * (-_tmp5 * _tmp8 + 1) + _tmp2 * (-_tmp5 * _tmp7 + 1) return _res
[docs] def to_unit_vector(self): # type: (Unit3) -> numpy.ndarray """ Returns a :class:`Vector3` version of the unit direction. """ # Total ops: 0 # Input arrays _self = self.data # Intermediate terms (0) # Output terms _res = numpy.zeros(3) _res[0] = _self[0] _res[1] = _self[1] _res[2] = _self[2] return _res
[docs] @staticmethod def random_from_uniform_samples(u1, u2, epsilon): # type: (float, float, float) -> Unit3 """ Generate a random :class:`Unit3` direction from two variables uniformly sampled in [0, 1]. """ # Total ops: 23 # Input arrays # Intermediate terms (8) _tmp0 = 2 * math.pi * u1 _tmp1 = math.cos(_tmp0) _tmp2 = 2 * u2 - 1 _tmp3 = math.sqrt(1 - _tmp2**2) _tmp4 = _tmp3**2 _tmp5 = math.sin(_tmp0) _tmp6 = 1 / math.sqrt(_tmp1**2 * _tmp4 + _tmp2**2 + _tmp4 * _tmp5**2 + epsilon) _tmp7 = _tmp3 * _tmp6 # Output terms _res = [0.0] * 3 _res[0] = _tmp1 * _tmp7 _res[1] = _tmp5 * _tmp7 _res[2] = _tmp2 * _tmp6 return Unit3.from_storage(_res)
[docs] @staticmethod def from_vector(a, epsilon): # type: (numpy.ndarray, float) -> Unit3 """ Return a :class:`Unit3` that points along the direction of vector ``a`` ``a`` will be normalized. """ # Total ops: 10 # Input arrays if a.shape == (3,): a = a.reshape((3, 1)) elif a.shape != (3, 1): raise IndexError( "a is expected to have shape (3, 1) or (3,); instead had shape {}".format(a.shape) ) # Intermediate terms (1) _tmp0 = 1 / math.sqrt(a[0, 0] ** 2 + a[1, 0] ** 2 + a[2, 0] ** 2 + epsilon) # Output terms _res = [0.0] * 3 _res[0] = _tmp0 * a[0, 0] _res[1] = _tmp0 * a[1, 0] _res[2] = _tmp0 * a[2, 0] return Unit3.from_storage(_res)
[docs] @staticmethod def from_unit_vector(a): # type: (numpy.ndarray) -> Unit3 """ Return a :class:`Unit3` that points along the direction of vector ``a`` ``a`` is expected to be a unit vector. """ # Total ops: 0 # Input arrays if a.shape == (3,): a = a.reshape((3, 1)) elif a.shape != (3, 1): raise IndexError( "a is expected to have shape (3, 1) or (3,); instead had shape {}".format(a.shape) ) # Intermediate terms (0) # Output terms _res = [0.0] * 3 _res[0] = a[0, 0] _res[1] = a[1, 0] _res[2] = a[2, 0] return Unit3.from_storage(_res)
# -------------------------------------------------------------------------- # StorageOps concept # --------------------------------------------------------------------------
[docs] @staticmethod def storage_dim(): # type: () -> int return 3
[docs] def to_storage(self): # type: () -> T.List[float] return list(self.data)
[docs] @classmethod def from_storage(cls, vec): # type: (T.Sequence[float]) -> Unit3 instance = cls.__new__(cls) if isinstance(vec, list): instance.data = vec else: instance.data = list(vec) if len(vec) != cls.storage_dim(): raise ValueError( "{} has storage dim {}, got {}.".format(cls.__name__, cls.storage_dim(), len(vec)) ) return instance
# -------------------------------------------------------------------------- # LieGroupOps concept # --------------------------------------------------------------------------
[docs] @staticmethod def tangent_dim(): # type: () -> int return 2
[docs] def retract(self, vec, epsilon=1e-8): # type: (numpy.ndarray, float) -> Unit3 if len(vec) != self.tangent_dim(): raise ValueError( "Vector dimension ({}) not equal to tangent space dimension ({}).".format( len(vec), self.tangent_dim() ) ) return ops.LieGroupOps.retract(self, vec, epsilon)
[docs] def local_coordinates(self, b, epsilon=1e-8): # type: (Unit3, float) -> numpy.ndarray return ops.LieGroupOps.local_coordinates(self, b, epsilon)
[docs] def interpolate(self, b, alpha, epsilon=1e-8): # type: (Unit3, float, float) -> Unit3 return ops.LieGroupOps.interpolate(self, b, alpha, epsilon)
# -------------------------------------------------------------------------- # General Helpers # -------------------------------------------------------------------------- def __eq__(self, other): # type: (T.Any) -> bool if isinstance(other, Unit3): return self.data == other.data else: return False