navlie.utils.alignment.associate_and_align_trajectories¶
- navlie.utils.alignment.associate_and_align_trajectories(traj_ref_list: List[SE3State], traj_est_list: List[SE3State], max_diff: float = 0.02, offset: float = 0.0, align: bool = True, correct_scale: bool = False, n_to_align: int = -1, verbose: bool = False) → Tuple[List[SE3State], List[SE3State], Dict[str, ndarray]]¶
Associates the stamps of two trajectories and aligns the estimated trajectory to the reference trajectory.
- Parameters:
traj_ref (List[SE3State]) – Reference trajectory, a list of SE3State
traj_est (List[SE3State]) – Estimated trajectory to be aligned to the reference, a list of SE3State
max_diff (float, optional) – The maximum allowable difference in timestamps between the estimate and the reference trajectory, by default 0.02
offset (float, optional) – optional time offset of the second trajectory, by default 0.0
align (bool, optional) – Whether to align the trajectories, by default True
correct_scale (bool, optional) – Whether to correct the scale, by default False. If correct_scale is false, the alignment transformation is an element of SE(3), otherwise, it is an element of Sim(3).
n_to_align (int, optional) – The number of poses to use for alignment, by default -1. If n_to_align is -1, all poses are used.
verbose (bool, optional) – Verbosity flag, by default False
- Returns:
typing.List[SE3State] – The reference trajectory, with timestamps at the same times as the estimated trajectory.
typing.List[SE3State] – The aligned estimated trajectory.
typing.Dict[str, np.ndarray] – A dictionary containing the transformation information. The keys are: - “rotation”: The DCM of the alignment transformation - “position”: The transformation component of the alignment transformation - “scale”: The scale component alignment transformation.