evalio.stats

Classes:

• Error –

  Dataclass to hold the error between two trajectories.

• Metric –

  Simple dataclass to hold the resulting metrics. Likely output from Error.

• MetricKind –

  Simple enum to define the metric to use for summarizing the error. Used in Error.

• WindowMeters –

  Dataclass to hold the parameters for a distance-based window.

• WindowSeconds –

  Dataclass to hold the parameters for a time-based window.

Functions:

• align –

  Align the trajectories both spatially and temporally.

• align_poses –

  Align the trajectory in place to another trajectory. Operates in place.

• align_stamps –

  Select the closest poses in traj1 and traj2. Operates in place.

• ate –

  Compute the Absolute Trajectory Error (ATE) between two trajectories.

• rte –

  Compute the Relative Trajectory Error (RTE) between two trajectories.

Attributes:

• WindowKind –

  Type alias for either a WindowMeters or a WindowSeconds.

WindowKind module-attribute

WindowKind = WindowMeters | WindowSeconds

Type alias for either a WindowMeters or a WindowSeconds.

Error dataclass

Dataclass to hold the error between two trajectories. Generally output from computing ate or rte.

Contains a (n,) arrays of translation and rotation errors.

Methods:

• mean –

  Compute the mean of the errors.

• median –

  Compute the median of the errors.

• sse –

  Compute the sqrt of sum of squared errors.

• summarize –

  How to summarize the vector of errors.

Attributes:

• rot (NDArray[float64]) –

  rotation error, shape (n,), in degrees

• trans (NDArray[float64]) –

  translation error, shape (n,), in meters

Source code in python/evalio/stats.py

@dataclass(kw_only=True)
class Error:
    """
    Dataclass to hold the error between two trajectories.
    Generally output from computing [ate][evalio.stats.ate] or [rte][evalio.stats.rte].

    Contains a (n,) arrays of translation and rotation errors.
    """

    # Shape: (n,)
    trans: NDArray[np.float64]
    """translation error, shape (n,), in meters"""
    rot: NDArray[np.float64]
    """rotation error, shape (n,), in degrees"""

    def summarize(self, metric: MetricKind) -> Metric:
        """How to summarize the vector of errors.

        Args:
            metric (MetricKind): The metric to use for summarizing the error,
                either mean, median, or sse.

        Returns:
            The summarized error
        """
        match metric:
            case MetricKind.mean:
                return self.mean()
            case MetricKind.median:
                return self.median()
            case MetricKind.sse:
                return self.sse()

    def mean(self) -> Metric:
        """Compute the mean of the errors."""
        return Metric(rot=self.rot.mean(), trans=self.trans.mean())

    def sse(self) -> Metric:
        """Compute the sqrt of sum of squared errors."""
        length = len(self.rot)
        return Metric(
            rot=float(np.sqrt(self.rot @ self.rot / length)),
            trans=float(np.sqrt(self.trans @ self.trans / length)),
        )

    def median(self) -> Metric:
        """Compute the median of the errors."""
        return Metric(
            rot=cast(float, np.median(self.rot)),
            trans=cast(float, np.median(self.trans)),
        )

rot instance-attribute

rot: NDArray[float64]

rotation error, shape (n,), in degrees

trans instance-attribute

trans: NDArray[float64]

translation error, shape (n,), in meters

mean

mean() -> Metric

Compute the mean of the errors.

Source code in python/evalio/stats.py

def mean(self) -> Metric:
    """Compute the mean of the errors."""
    return Metric(rot=self.rot.mean(), trans=self.trans.mean())

median

median() -> Metric

Compute the median of the errors.

Source code in python/evalio/stats.py

def median(self) -> Metric:
    """Compute the median of the errors."""
    return Metric(
        rot=cast(float, np.median(self.rot)),
        trans=cast(float, np.median(self.trans)),
    )

sse

sse() -> Metric

Compute the sqrt of sum of squared errors.

Source code in python/evalio/stats.py

def sse(self) -> Metric:
    """Compute the sqrt of sum of squared errors."""
    length = len(self.rot)
    return Metric(
        rot=float(np.sqrt(self.rot @ self.rot / length)),
        trans=float(np.sqrt(self.trans @ self.trans / length)),
    )

summarize

summarize(metric: MetricKind) -> Metric

How to summarize the vector of errors.

Parameters:

• metric (MetricKind) –

  The metric to use for summarizing the error, either mean, median, or sse.

Returns:

• Metric –

  The summarized error

Source code in python/evalio/stats.py

def summarize(self, metric: MetricKind) -> Metric:
    """How to summarize the vector of errors.

    Args:
        metric (MetricKind): The metric to use for summarizing the error,
            either mean, median, or sse.

    Returns:
        The summarized error
    """
    match metric:
        case MetricKind.mean:
            return self.mean()
        case MetricKind.median:
            return self.median()
        case MetricKind.sse:
            return self.sse()

Metric dataclass

Simple dataclass to hold the resulting metrics. Likely output from Error.

Attributes:

• rot (float) –

  rotation error in degrees

• trans (float) –

  translation error in meters

Source code in python/evalio/stats.py

@dataclass(kw_only=True)
class Metric:
    """Simple dataclass to hold the resulting metrics. Likely output from [Error][evalio.stats.Error]."""

    trans: float
    """translation error in meters"""
    rot: float
    """rotation error in degrees"""

rot instance-attribute

rot: float

rotation error in degrees

trans instance-attribute

trans: float

translation error in meters

MetricKind

Bases: StrEnum

Simple enum to define the metric to use for summarizing the error. Used in Error.

Attributes:

• mean –

  Mean

• median –

  Median

• sse –

  Sqrt of Sum of squared errors

Source code in python/evalio/stats.py

class MetricKind(StrEnum):
    """Simple enum to define the metric to use for summarizing the error. Used in [Error][evalio.stats.Error.summarize]."""

    mean = auto()
    """Mean"""
    median = auto()
    """Median"""
    sse = auto()
    """Sqrt of Sum of squared errors"""

mean class-attribute instance-attribute

mean = auto()

Mean

median class-attribute instance-attribute

median = auto()

Median

sse class-attribute instance-attribute

sse = auto()

Sqrt of Sum of squared errors

WindowMeters dataclass

Dataclass to hold the parameters for a distance-based window.

Methods:

• name –

  Get a string representation of the window.

Attributes:

• value (float) –

  Distance in meters

Source code in python/evalio/stats.py

@dataclass
class WindowMeters:
    """Dataclass to hold the parameters for a distance-based window."""

    value: float
    """Distance in meters"""

    def name(self) -> str:
        """Get a string representation of the window."""
        return f"{self.value:.1f}m"

value instance-attribute

value: float

Distance in meters

name

name() -> str

Get a string representation of the window.

Source code in python/evalio/stats.py

def name(self) -> str:
    """Get a string representation of the window."""
    return f"{self.value:.1f}m"

WindowSeconds dataclass

Dataclass to hold the parameters for a time-based window.

Methods:

• name –

  Get a string representation of the window.

Attributes:

• value (float) –

  Duration of the window in seconds

Source code in python/evalio/stats.py

@dataclass
class WindowSeconds:
    """Dataclass to hold the parameters for a time-based window."""

    value: float
    """Duration of the window in seconds"""

    def name(self) -> str:
        """Get a string representation of the window."""
        return f"{self.value}s"

value instance-attribute

value: float

Duration of the window in seconds

name

name() -> str

Get a string representation of the window.

Source code in python/evalio/stats.py

def name(self) -> str:
    """Get a string representation of the window."""
    return f"{self.value}s"

align

align(
    traj: Trajectory[M1],
    gt: Trajectory[M2],
    in_place: bool = False,
) -> tuple[Trajectory[M1], Trajectory[M2]]

Align the trajectories both spatially and temporally.

The resulting trajectories will be have the same origin as the second ("gt") trajectory. See align_poses and align_stamps for more details.

Parameters:

• traj (Trajectory) –

  One of the trajectories to align.

• gt (Trajectory) –

  The other trajectory to align to.

• in_place (bool, default: False ) –

  If true, the original trajectory will be modified. Defaults to False.

Source code in python/evalio/stats.py

def align(
    traj: ty.Trajectory[M1], gt: ty.Trajectory[M2], in_place: bool = False
) -> tuple[ty.Trajectory[M1], ty.Trajectory[M2]]:
    """Align the trajectories both spatially and temporally.

    The resulting trajectories will be have the same origin as the second ("gt") trajectory.
    See [align_poses][evalio.stats.align_poses] and [align_stamps][evalio.stats.align_stamps] for more details.

    Args:
        traj (Trajectory): One of the trajectories to align.
        gt (Trajectory): The other trajectory to align to.
        in_place (bool, optional): If true, the original trajectory will be modified. Defaults to False.
    """
    if not in_place:
        traj = deepcopy(traj)
        gt = deepcopy(gt)

    align_stamps(traj, gt)
    align_poses(traj, gt)

    return traj, gt

align_poses

align_poses(traj: Trajectory[M1], other: Trajectory[M2])

Align the trajectory in place to another trajectory. Operates in place.

This results in the current trajectory having an identical first pose to the other trajectory. Assumes the first pose of both trajectories have the same stamp.

Parameters:

• traj (Trajectory) –

  The trajectory that will be modified

• other (Trajectory) –

  The trajectory to align to.

Source code in python/evalio/stats.py

def align_poses(traj: ty.Trajectory[M1], other: ty.Trajectory[M2]):
    """Align the trajectory in place to another trajectory. Operates in place.

    This results in the current trajectory having an identical first pose to the other trajectory.
    Assumes the first pose of both trajectories have the same stamp.

    Args:
        traj (Trajectory): The trajectory that will be modified
        other (Trajectory): The trajectory to align to.
    """
    this = traj.poses[0]
    oth = other.poses[0]
    delta = oth * this.inverse()

    for i in range(len(traj.poses)):
        traj.poses[i] = delta * traj.poses[i]

align_stamps

align_stamps(traj1: Trajectory[M1], traj2: Trajectory[M2])

Select the closest poses in traj1 and traj2. Operates in place.

Does this by finding the higher frame rate trajectory and subsampling it to the closest poses of the other one. Additionally it checks the beginning of the trajectories to make sure they start at about the same stamp.

Parameters:

• traj1 (Trajectory) –

  One trajectory

• traj2 (Trajectory) –

  Other trajectory

Source code in python/evalio/stats.py

def align_stamps(traj1: ty.Trajectory[M1], traj2: ty.Trajectory[M2]):
    """Select the closest poses in traj1 and traj2. Operates in place.

    Does this by finding the higher frame rate trajectory and subsampling it to the closest poses of the other one.
    Additionally it checks the beginning of the trajectories to make sure they start at about the same stamp.

    Args:
        traj1 (Trajectory): One trajectory
        traj2 (Trajectory): Other trajectory
    """
    # Check if we need to skip poses in traj1
    first_pose_idx = 0
    while traj1.stamps[first_pose_idx] < traj2.stamps[0]:
        first_pose_idx += 1
    if not closest(
        traj2.stamps[0],
        traj1.stamps[first_pose_idx - 1],
        traj1.stamps[first_pose_idx],
    ):
        first_pose_idx -= 1
    traj1.stamps = traj1.stamps[first_pose_idx:]
    traj1.poses = traj1.poses[first_pose_idx:]

    # Check if we need to skip poses in traj2
    first_pose_idx = 0
    while traj2.stamps[first_pose_idx] < traj1.stamps[0]:
        first_pose_idx += 1
    if not closest(
        traj1.stamps[0],
        traj2.stamps[first_pose_idx - 1],
        traj2.stamps[first_pose_idx],
    ):
        first_pose_idx -= 1
    traj2.stamps = traj2.stamps[first_pose_idx:]
    traj2.poses = traj2.poses[first_pose_idx:]

    # Find the one that is at a higher frame rate
    # Leaves us with traj1 being the one with the higher frame rate
    swapped = False
    traj_1_dt = (traj1.stamps[-1] - traj1.stamps[0]).to_sec() / len(traj1.stamps)
    traj_2_dt = (traj2.stamps[-1] - traj2.stamps[0]).to_sec() / len(traj2.stamps)
    if traj_1_dt > traj_2_dt:
        traj1, traj2 = traj2, traj1  # type: ignore
        swapped = True

    # cache this value
    len_traj1 = len(traj1)

    # Align the two trajectories by subsampling keeping traj1 stamps
    traj1_idx = 0
    traj1_stamps: list[ty.Stamp] = []
    traj1_poses: list[ty.SE3] = []
    for i, stamp in enumerate(traj2.stamps):
        while traj1_idx < len_traj1 - 1 and traj1.stamps[traj1_idx] < stamp:
            traj1_idx += 1

        # go back one if we overshot
        if not closest(stamp, traj1.stamps[traj1_idx - 1], traj1.stamps[traj1_idx]):
            traj1_idx -= 1

        traj1_stamps.append(traj1.stamps[traj1_idx])
        traj1_poses.append(traj1.poses[traj1_idx])

        if traj1_idx >= len_traj1 - 1:
            traj2.stamps = traj2.stamps[: i + 1]
            traj2.poses = traj2.poses[: i + 1]
            break

    traj1.stamps = traj1_stamps
    traj1.poses = traj1_poses

    if swapped:
        traj1, traj2 = traj2, traj1  # type: ignore

ate

ate(traj: Trajectory[M1], gt: Trajectory[M2]) -> Error

Compute the Absolute Trajectory Error (ATE) between two trajectories.

Will check if the two trajectories are aligned and if not, will align them. Will not modify the original trajectories.

Parameters:

• traj (Trajectory) –

  One of the trajectories

• gt (Trajectory) –

  The other trajectory

Returns:

• Error –

  The computed error

Source code in python/evalio/stats.py

def ate(traj: ty.Trajectory[M1], gt: ty.Trajectory[M2]) -> Error:
    """Compute the Absolute Trajectory Error (ATE) between two trajectories.

    Will check if the two trajectories are aligned and if not, will align them.
    Will not modify the original trajectories.

    Args:
        traj (Trajectory): One of the trajectories
        gt (Trajectory): The other trajectory

    Returns:
        The computed error
    """
    if not _check_aligned(traj, gt):
        traj, gt = align(traj, gt)

    # Compute the ATE
    return _compute_metric(gt.poses, traj.poses)

rte

rte(
    traj: Trajectory[M1],
    gt: Trajectory[M2],
    window: WindowKind = WindowMeters(30),
) -> Error

Compute the Relative Trajectory Error (RTE) between two trajectories.

Will check if the two trajectories are aligned and if not, will align them. Will not modify the original trajectories.

Parameters:

• traj (Trajectory) –

  One of the trajectories

• gt (Trajectory) –

  The other trajectory

• window (WindowKind, default: WindowMeters(30) ) –

  The window to use for computing the RTE. Either a WindowMeters or a WindowSeconds. Defaults to WindowMeters(30), which is a 30 meter window.

Returns: The computed error

Source code in python/evalio/stats.py

def rte(
    traj: ty.Trajectory[M1],
    gt: ty.Trajectory[M2],
    window: WindowKind = WindowMeters(30),
) -> Error:
    """Compute the Relative Trajectory Error (RTE) between two trajectories.

    Will check if the two trajectories are aligned and if not, will align them.
    Will not modify the original trajectories.

    Args:
        traj (Trajectory): One of the trajectories
        gt (Trajectory): The other trajectory
        window (WindowKind, optional): The window to use for computing the RTE.
            Either a [WindowMeters][evalio.stats.WindowMeters] or a [WindowSeconds][evalio.stats.WindowSeconds].
            Defaults to WindowMeters(30), which is a 30 meter window.
    Returns:
        The computed error
    """
    if not _check_aligned(traj, gt):
        traj, gt = align(traj, gt)

    if window.value <= 0:
        raise ValueError("Window size must be positive")

    window_deltas_poses: list[ty.SE3] = []
    window_deltas_gts: list[ty.SE3] = []

    # cache this value
    len_gt = len(gt)

    if isinstance(window, WindowSeconds):
        # Find our pairs for computation
        end_idx = 1
        duration = ty.Duration.from_sec(window.value)

        for i in range(len_gt):
            while end_idx < len_gt and gt.stamps[end_idx] - gt.stamps[i] < duration:
                end_idx += 1

            if end_idx >= len_gt:
                break

            window_deltas_poses.append(traj.poses[i].inverse() * traj.poses[end_idx])
            window_deltas_gts.append(gt.poses[i].inverse() * gt.poses[end_idx])


    elif isinstance(window, WindowMeters):
        # Compute deltas for all of ground truth poses
        dist = np.zeros(len_gt)
        for i in range(1, len_gt):
            dist[i] = ty.SE3.distance(gt.poses[i], gt.poses[i - 1])

        cum_dist = np.cumsum(dist)
        end_idx = 1
        end_idx_prev = 0

        # Find our pairs for computation
        for i in range(len_gt):
            while end_idx < len_gt and cum_dist[end_idx] - cum_dist[i] < window.value:
                end_idx += 1

            if end_idx >= len_gt:
                break
            elif end_idx == end_idx_prev:
                continue

            window_deltas_poses.append(traj.poses[i].inverse() * traj.poses[end_idx])
            window_deltas_gts.append(gt.poses[i].inverse() * gt.poses[end_idx])

            end_idx_prev = end_idx

    if len(window_deltas_poses) == 0:
        if isinstance(traj.metadata, ty.Experiment):
            print_warning(
                f"No {window} windows found for '{traj.metadata.name}' on '{traj.metadata.sequence}'"
            )
        else:
            print_warning(f"No {window} windows found")
        return Error(rot=np.array([np.nan]), trans=np.array([np.nan]))

    # Compute the RTE
    return _compute_metric(window_deltas_gts, window_deltas_poses)