Covariance6x6

Represents a 6×6 covariance matrix, e.g. for a 6-DOF pose (position and orientation).

Parameters

Field	Type	Description
data	np.ndarray | list | tuple	Array-like input of shape (6, 6), converted to a contiguous float32 array.

Raises

Exception	Condition
TypeError	data can't be converted to float32 (e.g. non-numeric elements)
ValueError	data is not rank-2, or its shape isn't (6, 6)
ValueError	data contains a non-finite value (NaN/Inf)
ValueError	data isn't symmetric within covariance_atol (max |C - Cᵗ| > covariance_atol)
ValueError	data isn't positive semi-definite (smallest eigenvalue < -covariance_atol)

Attributes

Attribute	Type	Description
data	np.ndarray	Defensive copy of the underlying (6, 6) float32 matrix. Assigning a new value re-validates it (shape, finiteness, symmetry, PSD) the same way as construction.
shape	tuple[int, ...]	Always (6, 6).
ndim	int	Always 2.
dtype	np.dtype	Always float32.
size	int	Always 36.
covariance_atol	float	Class-level absolute tolerance (1e-6) used when checking symmetry and positive semi-definiteness.

Methods

Method	Description
to_numpy(copy=True)	Returns the matrix as np.ndarray. Pass copy=False for a reference to the internal array instead — faster, but mutating it mutates the Covariance6x6 too.
copy()	Returns a new Covariance6x6 with an independent copy of the data.
Covariance6x6.coerce(value)	Returns value unchanged if it's already a Covariance6x6; otherwise wraps an array-like into one (running full validation). Raises TypeError for any other input.

Operators

Operation	Behavior
c == other	True only if other is also a Covariance6x6 with element-equal data. False for anything else.
len(c)	Always 6 (length of the first axis).
np.asarray(c)	Works directly via __array__. Always returns a copy; use to_numpy(copy=False) for a zero-copy view.
hash(c)	Not supported — mutable via the data setter.

Visualization

datatypes.visualize(covariance, entity_path=...) logs a 1-sigma position-uncertainty ellipsoid (rr.Ellipsoids3D), computed from the eigendecomposition of the position sub-block data[:3, :3]: semi-axes are sqrt(eigenvalue), oriented along the eigenvectors. Only the position sub-block is rendered — there's no equally direct geometric picture for the orientation sub-block. The ellipsoid is centered at the world origin, since a bare Covariance6x6 carries no position of its own; pass a Position3D/Pose3D to the same visualize() call to overlay it at a real pose. No label support is registered for this type.

Example

"""Demonstrates the Telekinesis Covariance6x6 datatype."""

import time

import numpy as np
from loguru import logger
import rerun as rr

from telekinesis import datatypes

def covariance6x6_example():
    """Demonstrate creation, inspection, visualization, update, NumPy interop, and serialization."""

    # ======================= Create ============================================
    matrix = np.diag([1.0, 2.0, 3.0, 0.5, 0.5, 0.5]).astype(np.float32)
    covariance = datatypes.Covariance6x6(matrix)
    logger.info(f"Input matrix:\n{matrix}")
    logger.info(f"Original Covariance6x6: {covariance}")

    # ======================= Inspect ===========================================
    data = covariance.data
    shape = covariance.shape
    size = covariance.size
    dtype = covariance.dtype
    ndim = covariance.ndim
    numpy_array = covariance.to_numpy()
    covariance_copy = covariance.copy()

    logger.info(
        f"shape={shape}, "
        f"size={size}, "
        f"ndim={ndim}, "
        f"dtype={dtype}"
    )
    logger.info(f"Data:\n{data}")
    logger.info(f"NumPy array:\n{numpy_array}")
    logger.info(f"Copied Covariance6x6: {covariance_copy}")

    # ======================= Visualize =========================================
    rr.init("covariance6x6_example", spawn=True)
    datatypes.visualize(covariance, entity_path="/Covariance6x6")

    # ======================= Update ============================================
    covariance.data = np.eye(6, dtype=np.float32) * 2.0
    logger.info(f"Updated Covariance6x6:\n{covariance.data}")
    datatypes.visualize(covariance, entity_path="/Covariance6x6/updated")

    # ======================= NumPy Interop =====================================
    is_symmetric = np.allclose(covariance.data, covariance.data.T)
    eigenvalues = np.linalg.eigvalsh(covariance.data)
    variances = np.diag(covariance.data)

    logger.info(f"Is symmetric (np.allclose with transpose): {is_symmetric}")
    logger.info(f"Eigenvalues (np.linalg.eigvalsh): {eigenvalues}")
    logger.info(f"Per-axis variances (np.diag): {variances}")

    # ======================= Serialize / Deserialize ===========================
    start = time.perf_counter()
    serialized = datatypes.serialize(covariance)
    serialization_ms = (time.perf_counter() - start) * 1000

    start = time.perf_counter()
    deserialized = datatypes.deserialize(serialized)["param_0"]
    deserialization_ms = (time.perf_counter() - start) * 1000

    logger.info(f"Deserialized Covariance6x6:\n{deserialized.data}")
    logger.info(f"Round-trip successful: {covariance == deserialized}")
    logger.info(f"Serialization time: {serialization_ms:.3f} ms")
    logger.info(f"Deserialization time: {deserialization_ms:.3f} ms")


if __name__ == "__main__":
    covariance6x6_example()