Vector and rotation arithmetic on pose vectors.
Mirrors tbp/monty/frameworks/utils/spatial_arithmetics.py. Everything here
answers one of two questions: how far apart are these directions, or what
does this rotation do to these directions.
What a pose is
A pose vector set is three row vectors — the surface normal and the two
principal curvature directions — held as a 3×3 list of lists, in Monty's
[normal, curvature_1, curvature_2] order. Features carry the same thing
flattened to nine numbers, so the batch helpers take rows of nine.
Two divergences from the Python, both deliberate
- Quaternions are rotations. Python returns
[x, y, z, w]arrays from scipy'sas_quat(), while Monty's ownQuaternionWXYZand its configs writewfirst. Rather than pass an array whose meaning depends on which library produced it, the converters return aMonty.Geometry.Rotation, which has exactly one spelling. - An undefined angle is
:infinity. Python returnsnp.inf; there is no float infinity in the BEAM, and:infinitysorts above every number, soget_angle_beefed_up/2 < tolerancestill reads as false — the answer callers want. Trying to do arithmetic on it raises, which is better than quietly propagating a NaN.
Summary
Functions
Adds two 3-vectors.
The matrix that turns each of frames into target, one per frame.
The rotation that turns the frame m1 into the frame m2, and how badly it
fails to.
The rotation matrix that turns the frame m1 into the frame m2, with no
rotation object in the way.
Transforms observed points into the model's reference frame.
Applies each of rotations to the same pose vector set.
Whether a matrix is orthonormal: mutually perpendicular unit rows.
The cross product of two 3-vectors.
The dot product of two 3-vectors.
Converts a list of Euler angles, in degrees, to rotations.
The angle between two unit vectors, in radians.
The angle between two vectors, in radians, or :infinity when either is
undefined (not a vector) or has no direction (the zero vector).
The angle between two vectors of any length, in radians.
The angles between each hypothesis' neighbours and its query vector.
Unit vectors evenly spaced in the plane orthogonal to frame's first vector.
The angle from v1 to v2, in radians, signed by surface_normal.
The distinct rotations among poses, as {euler_poses, rotations}.
The inverse of a 3×3 matrix, or :singular when it has none.
The gap between 1.0 and the next float, as sys.float_info.epsilon.
The length of a 3-vector.
Whether a matrix can be inverted.
The 1-norm of a matrix: its largest absolute column sum.
A unit vector in the direction of v.
Whether two unit vectors point along the same line, in either direction.
Whether new_pose differs from every pose in all_poses by at least
similarity_th radians.
The part of v lying in the plane with normal n.
Rotates rows of flattened pose vectors — nine numbers, three vectors each.
Rotates a pose vector set by one rotation.
Converts a list of rotation matrices to rotations.
Converts rotations to Monty.Geometry.Rotations.
Multiplies a 3-vector by a scalar.
Subtracts two 3-vectors.
Types
Functions
Adds two 3-vectors.
The matrix that turns each of frames into target, one per frame.
Monty asks for these rather than for rotations (as_scipy=False in
align_multiple_orthonormal_vectors), and that is the form a learning module
keeps: a stored node frame is only approximately orthonormal — YCB's surface
normals and curvature directions are unit to a few parts in a thousand and
perpendicular to a few hundredths — so the product is only approximately a
rotation. Rounding it to the nearest rotation instead, as a rotation object
does, moves the pose by hundredths of a radian: the displacement it rotates
drifts by millimetres a step, and the pose evidence drifts with it.
@spec align_orthonormal_vectors(matrix(), matrix()) :: {Monty.Geometry.Rotation.t(), float()}
The rotation that turns the frame m1 into the frame m2, and how badly it
fails to.
Rows are vectors, so the alignment rotation is m1ᵀ · m2 — m2's rows
expressed in m1's basis. The error is how far applying that rotation's
inverse to m1 lands from m2: ~0 for a right-handed pair, and large for a
mirrored one, which is why Monty kept the number around.
The rotation matrix that turns the frame m1 into the frame m2, with no
rotation object in the way.
A mirrored pair has no matrix that turns one into the other; the matrix here
is the reflection itself, which is why align_orthonormal_vectors/2 measures
its error against the nearest proper rotation instead.
@spec apply_rf_transform_to_points( [vector()], %{required(String.t()) => term()}, vector(), vector(), Monty.Geometry.Rotation.t(), number() ) :: {[vector()], %{required(String.t()) => term()}}
Transforms observed points into the model's reference frame.
locations are sensor positions in the body frame, features the features
observed there. The two reference points — where that spot sits in the model
and where it sits in the body — are the same physical point seen twice, so
centring on one, rotating, and shifting to the other is the whole transform.
object_rotation expresses how the model must be rotated to match the
observations, hence its inverse here.
object_scale is accepted and ignored, as in Monty.
@spec batch_rotate_pose_vectors(Monty.Geometry.SurfacePoseVectors.t(), [matrix()]) :: [matrix()]
Applies each of rotations to the same pose vector set.
Returns one rotated 3×3 set per rotation, in the rotations' order.
Whether a matrix is orthonormal: mutually perpendicular unit rows.
Both halves come from transpose(m) · m being the identity — off-diagonal
entries measure non-perpendicularity, diagonal ones measure row length. Monty
allows a mean absolute error of 0.01, generous enough to accept a curvature
estimate that is only roughly orthogonal.
The cross product of two 3-vectors.
The dot product of two 3-vectors.
@spec euler_to_rotations([vector()], keyword()) :: [Monty.Geometry.Rotation.t()]
Converts a list of Euler angles, in degrees, to rotations.
The angle between two unit vectors, in radians.
Assumes unit input for speed — the surface normals and curvature directions
this is used on already are. Use get_angle_between/2 for vectors of any
length, or get_angle_beefed_up/2 when either side may be undefined.
The angle between two vectors, in radians, or :infinity when either is
undefined (not a vector) or has no direction (the zero vector).
An undefined direction is similar to nothing, which is the point: a hypothesis with no curvature direction should be excluded from a comparison, not win it by looking maximally unlike its competitors along one particular axis.
The angle between two vectors of any length, in radians.
The magnitudes cancel inside atan2, so no normalisation is needed. A zero
vector contains no direction, and 0.0 is the answer Monty documents for it.
The angles between each hypothesis' neighbours and its query vector.
hypothesis_features is one list of neighbour vectors per hypothesis,
query_features one vector per hypothesis; the result is one list of angles
per hypothesis, in the neighbours' order.
@spec get_more_directions_in_plane(matrix(), pos_integer()) :: [matrix()]
Unit vectors evenly spaced in the plane orthogonal to frame's first vector.
frame holds the normal and the two curvature directions; count directions
are sampled by turning the first curvature direction towards the second. This
is how a flat surface, whose curvature directions are arbitrary, still offers
Monty a spread of candidate poses.
The angle from v1 to v2, in radians, signed by surface_normal.
Both directions live in the plane, so the sign is what "clockwise seen from
the normal" means — the sign is the whole reason to prefer this over
get_angle/2, which cannot tell a turn from its mirror image.
@spec get_unique_rotations([[Monty.Geometry.Rotation.t()]], float(), keyword()) :: {[vector()], [Monty.Geometry.Rotation.t()]}
The distinct rotations among poses, as {euler_poses, rotations}.
poses is a list of paths, each a list of rotations; a rotation is kept the
first time it appears within similarity_th radians of everything kept so
far. The Euler angles come back rounded to 3 decimals and wrapped into
[0, 360), because they are used as human-readable labels.
:reverse? (default true) returns the rotations that undo each kept pose,
matching Monty's default.
The inverse of a 3×3 matrix, or :singular when it has none.
Cofactor expansion, which in three dimensions is both exact and shorter than any iterative method.
@spec machine_epsilon() :: float()
The gap between 1.0 and the next float, as sys.float_info.epsilon.
The length of a 3-vector.
Whether a matrix can be inverted.
Monty asks numpy for the condition number — how much an input error can be
amplified by inverting — and accepts anything below 1/ε. Without LAPACK the
estimate here is the 1-norm condition number ‖a‖₁ · ‖a⁻¹‖₁, a small factor
away from numpy's 2-norm figure and sharing the property that matters: a
singular matrix has no inverse, so both are infinite.
The 1-norm of a matrix: its largest absolute column sum.
A unit vector in the direction of v.
Raises when v is shorter than epsilon: there is no direction to return for
the zero vector, and returning zero would put a non-unit vector into a pose
that later arithmetic assumes is unit.
Whether two unit vectors point along the same line, in either direction.
tolerance bounds 1 - |cos θ|, so it reads as "the angle is within
sqrt(2 · tolerance) of 0 or π" — and, unlike comparing a dot product to 1,
it treats both ends of the line alike.
Monty calls this is_parallel; Elixir reserves the is_ prefix for guards,
so the name is the one thing here that changed shape.
@spec pose_is_new([Monty.Geometry.Rotation.t()], Monty.Geometry.Rotation.t(), float()) :: boolean()
Whether new_pose differs from every pose in all_poses by at least
similarity_th radians.
The part of v lying in the plane with normal n.
v minus its projection onto n, so a vector parallel to the normal collapses
to zero and a vector already in the plane is unchanged.
@spec rotate_multiple_pose_vectors([vector()], Monty.Geometry.Rotation.t()) :: [ vector() ]
Rotates rows of flattened pose vectors — nine numbers, three vectors each.
The rows keep their order and their length; only the directions change.
@spec rotate_pose_vectors( Monty.Geometry.SurfacePoseVectors.t() | Monty.Geometry.GazeDirection.t(), Monty.Geometry.Rotation.t() ) :: Monty.Geometry.SurfacePoseVectors.t()
Rotates a pose vector set by one rotation.
A gaze direction is where to look, not a surface frame, so rotating it is meaningless; it raises rather than returning something that looks like a pose.
@spec rotation_matrices_to_rotations([matrix()], keyword()) :: [ Monty.Geometry.Rotation.t() ]
Converts a list of rotation matrices to rotations.
@spec rotations_to_rotations([vector() | matrix()], keyword()) :: [ Monty.Geometry.Rotation.t() ]
Converts rotations to Monty.Geometry.Rotations.
Euler angles (a flat three-list) are what the feature LM produces, rotation
matrices (a list of three rows) what the evidence LM produces; the shape says
which, exactly as ndim does in Python. invert: true returns the rotations
that undo the given ones.
Multiplies a 3-vector by a scalar.
Subtracts two 3-vectors.