Alignment

alignment

estimate_alignment

estimate_alignment(
    trajectory: Trajectory,
    other: Trajectory,
    alignment_settings: AlignmentSettings | None = None,
    matching_settings: MatchingSettings | None = None,
    initial_parameters: AlignmentParameters | None = None,
) -> AlignmentResult

Estimates the alignment between two trajectories.

Performs Helmert, Leverarm, and Time shift estimation depending on the configuration. This function only estimates the alignment parameters, it does not apply them to the trajectory.

Parameters:

• trajectory (Trajectory) –

  Trajectory to align.

• other (Trajectory) –

  Reference trajectory to align to.

• alignment_settings (AlignmentSettings, default: None ) –

  Settings for the alignment process. Defaults to AlignmentSettings().

• matching_settings (MatchingSettings, default: None ) –

  Settings for the matching process. Defaults to MatchingSettings().

• initial_parameters (AlignmentParameters, default: None ) –

  Prior (e.g. from a manual alignment) used as start values of the least squares adjustment instead of the direct initialization. Parameters that are not estimated stay fixed at their prior value. Defaults to None.

Returns:

• AlignmentResult ( AlignmentResult ) –

  Result of the alignment process.

Source code in trajectopy\processing\alignment.py

def estimate_alignment(
    trajectory: Trajectory,
    other: Trajectory,
    alignment_settings: settings.AlignmentSettings | None = None,
    matching_settings: settings.MatchingSettings | None = None,
    initial_parameters: AlignmentParameters | None = None,
) -> AlignmentResult:
    """Estimates the alignment between two trajectories.

    Performs Helmert, Leverarm, and Time shift estimation depending on the configuration.
    This function only estimates the alignment parameters, it does not apply them to the trajectory.

    Args:
        trajectory (Trajectory): Trajectory to align.
        other (Trajectory): Reference trajectory to align to.
        alignment_settings (AlignmentSettings, optional): Settings for the alignment process. Defaults to AlignmentSettings().
        matching_settings (MatchingSettings, optional): Settings for the matching process. Defaults to MatchingSettings().
        initial_parameters (AlignmentParameters, optional): Prior (e.g. from a manual alignment) used as
            start values of the least squares adjustment instead of the direct initialization. Parameters
            that are not estimated stay fixed at their prior value. Defaults to None.

    Returns:
        AlignmentResult: Result of the alignment process.
    """
    if alignment_settings is None:
        alignment_settings = settings.AlignmentSettings()
    if matching_settings is None:
        matching_settings = settings.MatchingSettings()

    logger.info("Aligning trajectory positions ...")

    alignment_data = AlignmentData(
        traj_from=trajectory,
        traj_to=other,
        alignment_settings=alignment_settings,
        matching_settings=matching_settings,
    )
    ghm_alignment = AlignmentEstimator(alignment_data=alignment_data, initial_parameters=initial_parameters)
    estimated_parameters = ghm_alignment.estimate_parameters()

    pre_aligned_rotations = (
        alignment_data.traj_from.transform(estimated_parameters.sim3_matrix, inplace=False).rotations
        if alignment_settings.estimation_settings.sensor_rotation
        else None
    )
    if pre_aligned_rotations is not None and alignment_data.traj_to.rotations is not None:
        logger.info("Aligning rotations ...")
        sensor_rot_params = align_rotations(rot_from=pre_aligned_rotations, rot_to=alignment_data.traj_to.rotations)
        logger.info("Estimated sensor rotation:\n%s", sensor_rot_params)
    else:
        sensor_rot_params = SensorRotationParameters(enabled=False)

    return AlignmentResult(
        name=f"{alignment_data.traj_from.name} to {alignment_data.traj_to.name}",
        position_parameters=estimated_parameters,
        rotation_parameters=sensor_rot_params,
        estimation_of=ghm_alignment.settings.estimation_settings,
        # without least squares parameters there is nothing that could fail to converge
        converged=ghm_alignment.has_results or alignment_settings.estimation_settings.all_lq_disabled,
    )

manual_alignment

manual_alignment(
    trajectory: Trajectory,
    other: Trajectory,
    parameters: AlignmentParameters,
) -> AlignmentResult

Creates an alignment result from manually chosen parameters (no estimation).

All parameters are marked as enabled so that their values are applied by apply_alignment.

Parameters:

• trajectory (Trajectory) –

  Trajectory that is aligned.

• other (Trajectory) –

  Reference trajectory.

• parameters (AlignmentParameters) –

  Manually chosen alignment parameters.

Returns:

• AlignmentResult ( AlignmentResult ) –

  Alignment result holding the manual parameters.

Source code in trajectopy\processing\alignment.py

def manual_alignment(trajectory: Trajectory, other: Trajectory, parameters: AlignmentParameters) -> AlignmentResult:
    """Creates an alignment result from manually chosen parameters (no estimation).

    All parameters are marked as enabled so that their values are applied by `apply_alignment`.

    Args:
        trajectory (Trajectory): Trajectory that is aligned.
        other (Trajectory): Reference trajectory.
        parameters (AlignmentParameters): Manually chosen alignment parameters.

    Returns:
        AlignmentResult: Alignment result holding the manual parameters.
    """
    position_parameters = copy.deepcopy(parameters)
    position_parameters.enable()
    return AlignmentResult(
        name=f"{trajectory.name} to {other.name} (manual)",
        position_parameters=position_parameters,
        rotation_parameters=SensorRotationParameters(enabled=False),
        estimation_of=position_parameters.to_estimation_settings(),
        converged=True,
    )

apply_alignment

apply_alignment(
    trajectory: Trajectory,
    alignment_result: AlignmentResult,
    inplace: bool = True,
) -> Trajectory

Transforms trajectory using alignment parameters.

After computing the alignment parameters needed to align two trajectories, they can be applied to arbitrary trajectories.

Parameters:

• trajectory (Trajectory) –

  Trajectory to apply alignment to.

• alignment_result (AlignmentResult) –

  Alignment result containing transformation parameters.

• inplace (bool, default: True ) –

  Perform in-place. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  Aligned trajectory.

Source code in trajectopy\processing\alignment.py

def apply_alignment(trajectory: Trajectory, alignment_result: AlignmentResult, inplace: bool = True) -> "Trajectory":
    """Transforms trajectory using alignment parameters.

    After computing the alignment parameters needed to align two trajectories, they can be
    applied to arbitrary trajectories.

    Args:
        trajectory (Trajectory): Trajectory to apply alignment to.
        alignment_result (AlignmentResult): Alignment result containing transformation parameters.
        inplace (bool, optional): Perform in-place. Defaults to True.

    Returns:
        Trajectory: Aligned trajectory.
    """

    def _prepare_alignment_application(
        trajectory: Trajectory, alignment_parameters: AlignmentParameters
    ) -> tuple[float, ...]:
        if trajectory.rotations is not None:
            rpy = trajectory.rotations.as_euler("xyz", degrees=False)
            euler_x, euler_y, euler_z = rpy[:, 0], rpy[:, 1], rpy[:, 2]
            lever_x, lever_y, lever_z = (
                alignment_parameters.lever_x.value,
                alignment_parameters.lever_y.value,
                alignment_parameters.lever_z.value,
            )
        else:
            if any(
                parameter.value != 0.0
                for parameter in (
                    alignment_parameters.lever_x,
                    alignment_parameters.lever_y,
                    alignment_parameters.lever_z,
                )
            ):
                logger.warning("Trajectory has no orientations. Cannot apply leverarm.")
            euler_x, euler_y, euler_z = 0, 0, 0
            lever_x, lever_y, lever_z = 0, 0, 0

        return euler_x, euler_y, euler_z, lever_x, lever_y, lever_z

    trajectory = trajectory if inplace else trajectory.copy()
    has_orientations = trajectory.has_orientation

    # leverarm and time
    (
        euler_x,
        euler_y,
        euler_z,
        lever_x,
        lever_y,
        lever_z,
    ) = _prepare_alignment_application(trajectory, alignment_result.position_parameters)

    # do not use velocities for time shift application
    trafo_x, trafo_y, trafo_z = leverarm_time_component(
        euler_x=euler_x,
        euler_y=euler_y,
        euler_z=euler_z,
        lever_x=lever_x,
        lever_y=lever_y,
        lever_z=lever_z,
        time_shift=0,
        speed_x=0,
        speed_y=0,
        speed_z=0,
    )
    trajectory.positions.xyz += np.c_[trafo_x, trafo_y, trafo_z]

    # instead, apply time shift as a simple shift of the timestamps
    trajectory.timestamps -= alignment_result.position_parameters.time_shift.value
    logger.info(
        f"Applied time shift of {alignment_result.position_parameters.time_shift.value * 1000:.1f} ms to timestamps."
    )

    # similiarity transformation
    trajectory.transform(alignment_result.position_parameters.sim3_matrix)

    logger.info("Applied alignment parameters to positions.")

    # sensor orientation (mounting rotation in the body frame)
    if trajectory.rotations is not None:
        trajectory.rotations = trajectory.rotations * alignment_result.rotation_parameters.rotation_set
        logger.info("Applied alignment parameters to orientations.")

    if not has_orientations:
        trajectory.rotations = None

    return trajectory

adopt_first_pose

adopt_first_pose(
    trajectory: Trajectory,
    other: Trajectory,
    inplace: bool = True,
) -> Trajectory

Transform trajectory so that the first pose is identical in both.

Parameters:

• trajectory (Trajectory) –

  Trajectory to transform.

• other (Trajectory) –

  Reference trajectory.

• inplace (bool, default: True ) –

  Perform transformation in place. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  Transformed trajectory.

Source code in trajectopy\processing\alignment.py

def adopt_first_pose(trajectory: Trajectory, other: Trajectory, inplace: bool = True) -> Trajectory:
    """Transform trajectory so that the first pose is identical in both.

    Args:
        trajectory (Trajectory): Trajectory to transform.
        other (Trajectory): Reference trajectory.
        inplace (bool, optional): Perform transformation in place. Defaults to True.

    Returns:
        Trajectory: Transformed trajectory.
    """
    trajectory = trajectory if inplace else trajectory.copy()
    adopt_first_position(trajectory=trajectory, other=other)
    adopt_first_orientation(trajectory=trajectory, other=other)
    return trajectory

adopt_first_position

adopt_first_position(
    trajectory: Trajectory,
    other: Trajectory,
    inplace: bool = True,
) -> Trajectory

Transform trajectory so that the first position is identical in both.

Parameters:

• trajectory (Trajectory) –

  Trajectory to transform.

• other (Trajectory) –

  Reference trajectory.

• inplace (bool, default: True ) –

  Perform transformation in place. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  Transformed trajectory.

Source code in trajectopy\processing\alignment.py

def adopt_first_position(trajectory: Trajectory, other: Trajectory, inplace: bool = True) -> Trajectory:
    """Transform trajectory so that the first position is identical in both.

    Args:
        trajectory (Trajectory): Trajectory to transform.
        other (Trajectory): Reference trajectory.
        inplace (bool, optional): Perform transformation in place. Defaults to True.

    Returns:
        Trajectory: Transformed trajectory.
    """
    trajectory = trajectory if inplace else trajectory.copy()
    position_difference = other.positions.xyz[0, :] - trajectory.positions.xyz[0, :]
    trajectory.positions.xyz += position_difference
    return trajectory

adopt_first_orientation

adopt_first_orientation(
    trajectory: Trajectory,
    other: Trajectory,
    inplace: bool = True,
) -> Trajectory

Transform trajectory so that the first orientation is identical in both.

Parameters:

• trajectory (Trajectory) –

  Trajectory to transform.

• other (Trajectory) –

  Reference trajectory.

• inplace (bool, default: True ) –

  Perform transformation in place. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  Transformed trajectory.

Source code in trajectopy\processing\alignment.py

def adopt_first_orientation(trajectory: Trajectory, other: Trajectory, inplace: bool = True) -> Trajectory:
    """Transform trajectory so that the first orientation is identical in both.

    Args:
        trajectory (Trajectory): Trajectory to transform.
        other (Trajectory): Reference trajectory.
        inplace (bool, optional): Perform transformation in place. Defaults to True.

    Returns:
        Trajectory: Transformed trajectory.
    """
    trajectory = trajectory if inplace else trajectory.copy()
    if trajectory.rotations is not None and other.rotations is not None:
        # one rotation for all poses, so that the rotations between the poses are kept
        first = Rotations.from_quat(trajectory.rotations.as_quat()[0])
        first_other = Rotations.from_quat(other.rotations.as_quat()[0])
        trajectory.rotations = first_other * first.inv() * trajectory.rotations

    return trajectory

align

align(
    trajectory: Trajectory,
    other: Trajectory,
    alignment_settings: AlignmentSettings | None = None,
    matching_settings: MatchingSettings | None = None,
    inplace: bool = True,
) -> Trajectory

Aligns the trajectory with another trajectory.

Parameters:

• trajectory (Trajectory) –

  Trajectory to align.

• other (Trajectory) –

  Reference trajectory.

• alignment_settings (AlignmentSettings, default: None ) –

  Settings for the alignment process. Defaults to AlignmentSettings().

• matching_settings (MatchingSettings, default: None ) –

  Settings for the matching process. Defaults to MatchingSettings().

• inplace (bool, default: True ) –

  Perform in-place. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  Aligned trajectory.

Source code in trajectopy\processing\alignment.py

def align(
    trajectory: Trajectory,
    other: Trajectory,
    alignment_settings: settings.AlignmentSettings | None = None,
    matching_settings: settings.MatchingSettings | None = None,
    inplace: bool = True,
) -> Trajectory:
    """Aligns the trajectory with another trajectory.

    Args:
        trajectory (Trajectory): Trajectory to align.
        other (Trajectory): Reference trajectory.
        alignment_settings (AlignmentSettings, optional): Settings for the alignment process.
            Defaults to AlignmentSettings().
        matching_settings (MatchingSettings, optional): Settings for the matching process.
            Defaults to MatchingSettings().
        inplace (bool, optional): Perform in-place. Defaults to True.

    Returns:
        Trajectory: Aligned trajectory.
    """
    if alignment_settings is None:
        alignment_settings = settings.AlignmentSettings()
    if matching_settings is None:
        matching_settings = settings.MatchingSettings()

    alignment = estimate_alignment(
        trajectory=trajectory,
        other=other,
        alignment_settings=alignment_settings,
        matching_settings=matching_settings,
    )
    return apply_alignment(trajectory=trajectory, alignment_result=alignment, inplace=inplace)