Unit3 - GTSAM Docs

Unit3 represents a direction in three dimensions while enforcing unit length. Its state has two degrees of freedom, making it the natural type for bearings, surface normals, and translation directions.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import gtsam
import numpy as np

Initialization and normalization¶

Construct from a three-vector or three scalars. The input need not be normalized; Unit3 normalizes it and retains only direction.

direction = gtsam.Unit3(np.array([1.0, 2.0, 3.0]))
print("unit vector:", direction.unitVector())
assert np.isclose(np.linalg.norm(direction.unitVector()), 1.0)

Direction operations¶

dot(other) computes cosine similarity, skew() returns the cross-product matrix, and errorVector(other) gives a two-dimensional tangent-space discrepancy.

other = gtsam.Unit3(0.0, 0.0, 1.0)
print("dot product:", direction.dot(other))
print("direction error:", direction.errorVector(other))
np.testing.assert_allclose(
    direction.skew() @ other.unitVector(),
    np.cross(direction.unitVector(), other.unitVector()),
)

Tangent basis and manifold operations¶

basis() returns a 3×2 orthonormal basis for the tangent plane. retract(delta) moves along that plane and localCoordinates() recovers a small two-vector displacement.

basis = direction.basis()
np.testing.assert_allclose(basis.T @ basis, np.eye(2), atol=1e-12)

delta = np.array([0.02, -0.01])
perturbed = direction.retract(delta)
np.testing.assert_allclose(direction.localCoordinates(perturbed), delta, atol=1e-8)

Source¶

Unit3.h

AI assistance caveat¶

AI was used to help draft this documentation, and inaccuracies could be present.