Skip to content

Matching

matching

match_non_overlapping_timestamps 

match_non_overlapping_timestamps(
    trajectory: Trajectory,
    other: Trajectory,
    max_distance: float = 0.0,
) -> float

Roughly matches two trajectories temporally.

Parameters:

  • trajectory (Trajectory) –

    Trajectory to match.

  • other (Trajectory) –

    Other trajectory to match against.

  • max_distance (float, default: 0.0 ) –

    Maximum distance for spatial matching. Defaults to 0.0.

Returns:

  • float ( float ) –

    Mean time offset.

Source code in trajectopy\processing\matching.py 
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
def match_non_overlapping_timestamps(trajectory: Trajectory, other: Trajectory, max_distance: float = 0.0) -> float:
    """Roughly matches two trajectories temporally.

    Args:
        trajectory (Trajectory): Trajectory to match.
        other (Trajectory): Other trajectory to match against.
        max_distance (float, optional): Maximum distance for spatial matching. Defaults to 0.0.

    Returns:
        float: Mean time offset.
    """
    other, trajectory = _match_trajectories_spatial(
        trajectory=other.copy(), other=trajectory.copy(), max_distance=max_distance
    )
    mean_time_offset = np.median(trajectory.timestamps - other.timestamps)
    logger.info("Median time offset: %.3f s", mean_time_offset)
    return mean_time_offset

match_timestamps 

match_timestamps(
    trajectory: Trajectory,
    timestamps: ndarray,
    inplace: bool = True,
) -> Trajectory

Truncates trajectory to only those poses where the timestamps exactly match "timestamps".

Parameters:

  • trajectory (Trajectory) –

    Input trajectory.

  • timestamps (ndarray) –

    Input timestamps.

  • inplace (bool, default: True ) –

    Perform matching in-place. Defaults to True.

Returns:

  • Trajectory ( Trajectory ) –

    Trajectory with matched timestamps.

Source code in trajectopy\processing\matching.py 
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
def match_timestamps(trajectory: Trajectory, timestamps: np.ndarray, inplace: bool = True) -> Trajectory:
    """Truncates trajectory to only those poses where the timestamps exactly match "timestamps".

    Args:
        trajectory (Trajectory): Input trajectory.
        timestamps (np.ndarray): Input timestamps.
        inplace (bool, optional): Perform matching in-place. Defaults to True.

    Returns:
        Trajectory: Trajectory with matched timestamps.
    """
    traj_self = trajectory if inplace else trajectory.copy()
    _, idx_self, _ = np.intersect1d(traj_self.timestamps, timestamps, return_indices=True)
    traj_self.mask(idx_self)
    return traj_self

match_trajectories 

match_trajectories(
    trajectory: Trajectory,
    other: Trajectory,
    matching_settings: MatchingSettings = MatchingSettings(),
    inplace: bool = True,
) -> Tuple[Trajectory, Trajectory]

Matches two trajectories using the specified method.

Supported methods:

  • INTERPOLATION: Interpolates the test trajectory onto the reference trajectory using its timestamps. The interpolation is linear for both positions and rotations (SLERP).
  • NEAREST_TEMPORAL: Finds the nearest temporal match without interpolation by finding the nearest timestamp in the target trajectory for each timestamp in the source trajectory.
  • NEAREST_SPATIAL: Finds the nearest spatial match without interpolation by finding the nearest pose in the target trajectory for each pose in the source trajectory using Euclidean distance.
  • NEAREST_SPATIAL_INTERPOLATED: Finds the nearest k spatial matches and spatially interpolates using a 3d line. Both trajectories will have the length of the test trajectory. This method does not support rotation matching.

Parameters:

  • trajectory (Trajectory) –

    Trajectory to match.

  • other (Trajectory) –

    Other trajectory to match against.

  • matching_settings (MatchingSettings, default: MatchingSettings() ) –

    Matching settings. Defaults to MatchingSettings().

  • inplace (bool, default: True ) –

    Whether to modify the input trajectories. Defaults to True.

Returns:

Source code in trajectopy\processing\matching.py 
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
def match_trajectories(
    trajectory: Trajectory,
    other: Trajectory,
    matching_settings: settings.MatchingSettings = settings.MatchingSettings(),
    inplace: bool = True,
) -> Tuple[Trajectory, Trajectory]:
    """Matches two trajectories using the specified method.

    Supported methods:

    - **INTERPOLATION**: Interpolates the test trajectory onto the reference trajectory using
      its timestamps. The interpolation is linear for both positions and rotations (SLERP).
    - **NEAREST_TEMPORAL**: Finds the nearest temporal match without interpolation by finding
      the nearest timestamp in the target trajectory for each timestamp in the source trajectory.
    - **NEAREST_SPATIAL**: Finds the nearest spatial match without interpolation by finding
      the nearest pose in the target trajectory for each pose in the source trajectory using
      Euclidean distance.
    - **NEAREST_SPATIAL_INTERPOLATED**: Finds the nearest k spatial matches and spatially
      interpolates using a 3d line. Both trajectories will have the length of the test trajectory.
      This method does not support rotation matching.

    Args:
        trajectory (Trajectory): Trajectory to match.
        other (Trajectory): Other trajectory to match against.
        matching_settings (MatchingSettings, optional): Matching settings. Defaults to
            MatchingSettings().
        inplace (bool, optional): Whether to modify the input trajectories. Defaults to True.

    Returns:
        Tuple[Trajectory, Trajectory]: Matched trajectories.
    """
    trajectory = trajectory if inplace else trajectory.copy()
    other = other if inplace else other.copy()

    if not trajectory.overlaps_with(other) and matching_settings.method in [
        settings.MatchingMethod.NEAREST_TEMPORAL,
        settings.MatchingMethod.INTERPOLATION,
    ]:
        logger.warning("Trajectories do not overlap! Performing rough matching first.")
        timeshift = match_non_overlapping_timestamps(
            trajectory=other, other=trajectory, max_distance=matching_settings.max_distance
        )
        logger.info("Rough matching time offset: %.3f s", timeshift)
        trajectory.timestamps += timeshift

    logger.info("Matching trajectories using method %s", matching_settings.method.name)

    if matching_settings.method == settings.MatchingMethod.INTERPOLATION:
        return _match_trajectories_interpolation(
            trajectory=trajectory, other=other, max_gap_size=matching_settings.max_gap_size
        )

    if matching_settings.method == settings.MatchingMethod.NEAREST_TEMPORAL:
        return _match_trajectories_temporal(
            trajectory=trajectory, other=other, max_distance=matching_settings.max_time_diff
        )

    if matching_settings.method == settings.MatchingMethod.NEAREST_SPATIAL:
        return _match_trajectories_spatial(
            trajectory=trajectory, other=other, max_distance=matching_settings.max_distance
        )

    if matching_settings.method == settings.MatchingMethod.NEAREST_SPATIAL_INTERPOLATED:
        return _match_trajectories_spatial_interpolation(
            trajectory=trajectory,
            other=other,
            max_distance=matching_settings.max_distance,
            k_nearest=matching_settings.k_nearest,
        )

    raise ValueError(f"Matching method {matching_settings.method} not supported!")