Example #1
0
def plot(args, result, traj_ref, traj_est, traj_ref_full=None):
    from evo.tools import plot
    from evo.tools.settings import SETTINGS

    import matplotlib.pyplot as plt
    import numpy as np

    logger.debug(SEP)
    logger.debug("Plotting results... ")
    plot_mode = plot.PlotMode(args.plot_mode)

    # Plot the raw metric values.
    fig1 = plt.figure(figsize=SETTINGS.plot_figsize)
    if "seconds_from_start" in result.np_arrays:
        seconds_from_start = result.np_arrays["seconds_from_start"]
    else:
        seconds_from_start = None

    plot.error_array(fig1,
                     result.np_arrays["error_array"],
                     x_array=seconds_from_start,
                     statistics={
                         s: result.stats[s]
                         for s in SETTINGS.plot_statistics
                         if s not in ("min", "max")
                     },
                     name=result.info["label"],
                     title=result.info["title"],
                     xlabel="$t$ (s)" if seconds_from_start else "index")

    # Plot the values color-mapped onto the trajectory.
    fig2 = plt.figure(figsize=SETTINGS.plot_figsize)
    ax = plot.prepare_axis(fig2, plot_mode)
    if args.ros_map_yaml:
        plot.ros_map(ax, args.ros_map_yaml, plot_mode)

    plot.traj(ax,
              plot_mode,
              traj_ref_full if traj_ref_full else traj_ref,
              style=SETTINGS.plot_reference_linestyle,
              color=SETTINGS.plot_reference_color,
              label='reference',
              alpha=SETTINGS.plot_reference_alpha)
    plot.draw_coordinate_axes(ax, traj_ref, plot_mode,
                              SETTINGS.plot_axis_marker_scale)

    if args.plot_colormap_min is None:
        args.plot_colormap_min = result.stats["min"]
    if args.plot_colormap_max is None:
        args.plot_colormap_max = result.stats["max"]
    if args.plot_colormap_max_percentile is not None:
        args.plot_colormap_max = np.percentile(
            result.np_arrays["error_array"], args.plot_colormap_max_percentile)

    plot.traj_colormap(ax,
                       traj_est,
                       result.np_arrays["error_array"],
                       plot_mode,
                       min_map=args.plot_colormap_min,
                       max_map=args.plot_colormap_max,
                       title="Error mapped onto trajectory")
    plot.draw_coordinate_axes(ax, traj_est, plot_mode,
                              SETTINGS.plot_axis_marker_scale)
    if SETTINGS.plot_pose_correspondences:
        plot.draw_correspondence_edges(
            ax,
            traj_est,
            traj_ref,
            plot_mode,
            style=SETTINGS.plot_pose_correspondences_linestyle,
            color=SETTINGS.plot_reference_color,
            alpha=SETTINGS.plot_reference_alpha)
    fig2.axes.append(ax)

    plot_collection = plot.PlotCollection(result.info["title"])
    plot_collection.add_figure("raw", fig1)
    plot_collection.add_figure("map", fig2)
    if args.plot:
        plot_collection.show()
    if args.save_plot:
        plot_collection.export(args.save_plot,
                               confirm_overwrite=not args.no_warnings)
    if args.serialize_plot:
        logger.debug(SEP)
        plot_collection.serialize(args.serialize_plot,
                                  confirm_overwrite=not args.no_warnings)
Example #2
0
def plot_multi(args, result, traj_ref_list, traj_est_list):
    from evo.tools import plot
    from evo.tools.settings import SETTINGS

    import matplotlib.pyplot as plt
    import numpy as np

    logger.debug(SEP)
    logger.debug("Plotting results... ")
    plot_mode = plot.PlotMode(args.plot_mode)

    figs = []
    # Plot the raw metric values.
    error_array_comb = np.array([])
    for i in range(len(result)):
        figs.append(plt.figure(figsize=SETTINGS.plot_figsize))
        if "seconds_from_start" in result[i].np_arrays:
            seconds_from_start = result[i].np_arrays["seconds_from_start"]
        else:
            seconds_from_start = None

        plot.error_array(
            figs[i], result[i].np_arrays["error_array"], x_array=seconds_from_start,
            statistics={
                s: result[i].stats[s]
                for s in SETTINGS.plot_statistics if s not in ("min", "max")
            }, name=result[i].info["label"], title=result[i].info["title"],
            xlabel="$t$ (s)" if seconds_from_start else "index")

    # Plot the values color-mapped onto the trajectory.
    figs.append(plt.figure(figsize=SETTINGS.plot_figsize))
    ax = plot.prepare_axis(figs[-1], plot_mode)
    if args.ros_map_yaml:
        plot.ros_map(ax, args.ros_map_yaml, plot_mode)

    if args.plot_colormap_min is None:
        args.plot_colormap_min = min([result[i].stats["min"] for i in range(len(result))])
    if args.plot_colormap_max is None:
        args.plot_colormap_max = max([result[i].stats["max"] for i in range(len(result))])
    if args.plot_colormap_max_percentile is not None:
        args.plot_colormap_max = np.percentile(
            error_array_comb, args.plot_colormap_max_percentile)

    traj_est_list_comb = np.array([])
    for i in range(len(result)):
        plot.traj(ax, plot_mode, traj_ref_list[i], style=SETTINGS.plot_reference_linestyle,
                  color=multirobot_reference_color[i], label='reference'+str(i),
                  alpha=0.8)
        plot.draw_coordinate_axes(ax, traj_ref_list[i], plot_mode,
                                  SETTINGS.plot_axis_marker_scale)

        plot.draw_coordinate_axes(ax, traj_est_list[i], plot_mode,
                                  SETTINGS.plot_axis_marker_scale)
        figs[-1].axes.append(ax)
    
    plot.traj_colormap_multi(ax, traj_est_list, [r.np_arrays["error_array"] for r in result],
                       plot_mode, min_map=args.plot_colormap_min,
                       max_map=args.plot_colormap_max,
                       title="Error mapped onto trajectory")

    plot_collection = plot.PlotCollection("Multi-robot APE analysis")
    for i in range(len(result)):
        plot_collection.add_figure("raw" + str(i), figs[i])
    plot_collection.add_figure("map", figs[-1])
    if args.plot:
        plot_collection.show()
    if args.save_plot:
        plot_collection.export(args.save_plot,
                               confirm_overwrite=not args.no_warnings)
    if args.serialize_plot:
        logger.debug(SEP)
        plot_collection.serialize(args.serialize_plot,
                                  confirm_overwrite=not args.no_warnings)
Example #3
0
def run(args):
    import os
    import sys

    import numpy as np

    import evo.core.lie_algebra as lie
    from evo.core import trajectory
    from evo.core.trajectory import PoseTrajectory3D
    from evo.tools import file_interface, log
    from evo.tools.settings import SETTINGS

    log.configure_logging(verbose=args.verbose,
                          silent=args.silent,
                          debug=args.debug,
                          local_logfile=args.logfile)
    if args.debug:
        import pprint
        logger.debug(
            "main_parser config:\n" +
            pprint.pformat({arg: getattr(args, arg)
                            for arg in vars(args)}) + "\n")
    logger.debug(SEP)

    trajectories, ref_traj = load_trajectories(args)

    if args.merge:
        if args.subcommand == "kitti":
            die("Can't merge KITTI files.")
        if len(trajectories) == 0:
            die("No trajectories to merge (excluding --ref).")
        trajectories = {
            "merged_trajectory": trajectory.merge(trajectories.values())
        }

    if args.transform_left or args.transform_right:
        tf_type = "left" if args.transform_left else "right"
        tf_path = args.transform_left \
                if args.transform_left else args.transform_right
        transform = file_interface.load_transform_json(tf_path)
        logger.debug(SEP)
        if not lie.is_se3(transform):
            logger.warning("Not a valid SE(3) transformation!")
        if args.invert_transform:
            transform = lie.se3_inverse(transform)
        logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
            tf_type, transform))
        for traj in trajectories.values():
            traj.transform(transform,
                           right_mul=args.transform_right,
                           propagate=args.propagate_transform)

    if args.t_offset:
        logger.debug(SEP)
        for name, traj in trajectories.items():
            if type(traj) is trajectory.PosePath3D:
                die("{} doesn't have timestamps - can't add time offset.".
                    format(name))
            logger.info("Adding time offset to {}: {} (s)".format(
                name, args.t_offset))
            traj.timestamps += args.t_offset

    if args.n_to_align != -1 and not (args.align or args.correct_scale):
        die("--n_to_align is useless without --align or/and --correct_scale")

    if args.sync or args.align or args.correct_scale or args.align_origin:
        from evo.core import sync
        if not args.ref:
            logger.debug(SEP)
            die("Can't align or sync without a reference! (--ref)  *grunt*")
        for name, traj in trajectories.items():
            if args.subcommand == "kitti":
                ref_traj_tmp = ref_traj
            else:
                logger.debug(SEP)
                ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
                    ref_traj,
                    traj,
                    max_diff=args.t_max_diff,
                    first_name="reference",
                    snd_name=name)
            if args.align or args.correct_scale:
                logger.debug(SEP)
                logger.debug("Aligning {} to reference.".format(name))
                trajectories[name] = trajectory.align_trajectory(
                    trajectories[name],
                    ref_traj_tmp,
                    correct_scale=args.correct_scale,
                    correct_only_scale=args.correct_scale and not args.align,
                    n=args.n_to_align)
            if args.align_origin:
                logger.debug(SEP)
                logger.debug("Aligning {}'s origin to reference.".format(name))
                trajectories[name] = trajectory.align_trajectory_origin(
                    trajectories[name], ref_traj_tmp)

    print_compact_name = not args.subcommand == "bag"
    for name, traj in trajectories.items():
        print_traj_info(name, traj, args.verbose, args.full_check,
                        print_compact_name)
    if args.ref:
        print_traj_info(args.ref, ref_traj, args.verbose, args.full_check,
                        print_compact_name)

    if args.plot or args.save_plot or args.serialize_plot:
        import numpy as np
        from evo.tools import plot
        import matplotlib.pyplot as plt
        import matplotlib.cm as cm

        plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
        fig_xyz, axarr_xyz = plt.subplots(3,
                                          sharex="col",
                                          figsize=tuple(SETTINGS.plot_figsize))
        fig_rpy, axarr_rpy = plt.subplots(3,
                                          sharex="col",
                                          figsize=tuple(SETTINGS.plot_figsize))
        fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))

        plot_mode = plot.PlotMode[args.plot_mode]
        ax_traj = plot.prepare_axis(fig_traj, plot_mode)

        if args.ref:
            short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
            if SETTINGS.plot_usetex:
                short_traj_name = short_traj_name.replace("_", "\\_")
            plot.traj(ax_traj,
                      plot_mode,
                      ref_traj,
                      style=SETTINGS.plot_reference_linestyle,
                      color=SETTINGS.plot_reference_color,
                      label=short_traj_name,
                      alpha=SETTINGS.plot_reference_alpha)
            plot.draw_coordinate_axes(ax_traj, ref_traj, plot_mode,
                                      SETTINGS.plot_axis_marker_scale)
            plot.traj_xyz(axarr_xyz,
                          ref_traj,
                          style=SETTINGS.plot_reference_linestyle,
                          color=SETTINGS.plot_reference_color,
                          label=short_traj_name,
                          alpha=SETTINGS.plot_reference_alpha)
            plot.traj_rpy(axarr_rpy,
                          ref_traj,
                          style=SETTINGS.plot_reference_linestyle,
                          color=SETTINGS.plot_reference_color,
                          label=short_traj_name,
                          alpha=SETTINGS.plot_reference_alpha)

        if args.ros_map_yaml:
            plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)

        cmap_colors = None
        if SETTINGS.plot_multi_cmap.lower() != "none":
            cmap = getattr(cm, SETTINGS.plot_multi_cmap)
            cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))

        for name, traj in trajectories.items():
            if cmap_colors is None:
                color = next(ax_traj._get_lines.prop_cycler)['color']
            else:
                color = next(cmap_colors)
            if print_compact_name:
                short_traj_name = os.path.splitext(os.path.basename(name))[0]
            else:
                short_traj_name = name
            if SETTINGS.plot_usetex:
                short_traj_name = short_traj_name.replace("_", "\\_")
            plot.traj(ax_traj,
                      plot_mode,
                      traj,
                      SETTINGS.plot_trajectory_linestyle,
                      color,
                      short_traj_name,
                      alpha=SETTINGS.plot_trajectory_alpha)
            plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
                                      SETTINGS.plot_axis_marker_scale)
            if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
                start_time = ref_traj.timestamps[0]
            else:
                start_time = None
            plot.traj_xyz(axarr_xyz,
                          traj,
                          SETTINGS.plot_trajectory_linestyle,
                          color,
                          short_traj_name,
                          alpha=SETTINGS.plot_trajectory_alpha,
                          start_timestamp=start_time)
            plot.traj_rpy(axarr_rpy,
                          traj,
                          SETTINGS.plot_trajectory_linestyle,
                          color,
                          short_traj_name,
                          alpha=SETTINGS.plot_trajectory_alpha,
                          start_timestamp=start_time)
            if not SETTINGS.plot_usetex:
                fig_rpy.text(0.,
                             0.005,
                             "euler_angle_sequence: {}".format(
                                 SETTINGS.euler_angle_sequence),
                             fontsize=6)

        plot_collection.add_figure("trajectories", fig_traj)
        plot_collection.add_figure("xyz_view", fig_xyz)
        plot_collection.add_figure("rpy_view", fig_rpy)
        if args.plot:
            plot_collection.show()
        if args.save_plot:
            logger.info(SEP)
            plot_collection.export(args.save_plot,
                                   confirm_overwrite=not args.no_warnings)
        if args.serialize_plot:
            logger.info(SEP)
            plot_collection.serialize(args.serialize_plot,
                                      confirm_overwrite=not args.no_warnings)

    if args.save_as_tum:
        logger.info(SEP)
        for name, traj in trajectories.items():
            dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
            file_interface.write_tum_trajectory_file(
                dest, traj, confirm_overwrite=not args.no_warnings)
        if args.ref:
            dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
            file_interface.write_tum_trajectory_file(
                dest, ref_traj, confirm_overwrite=not args.no_warnings)
    if args.save_as_kitti:
        logger.info(SEP)
        for name, traj in trajectories.items():
            dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
            file_interface.write_kitti_poses_file(
                dest, traj, confirm_overwrite=not args.no_warnings)
        if args.ref:
            dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
            file_interface.write_kitti_poses_file(
                dest, ref_traj, confirm_overwrite=not args.no_warnings)
    if args.save_as_bag:
        import datetime
        import rosbag
        dest_bag_path = str(
            datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
        logger.info(SEP)
        logger.info("Saving trajectories to " + dest_bag_path + "...")
        bag = rosbag.Bag(dest_bag_path, 'w')
        try:
            for name, traj in trajectories.items():
                dest_topic = os.path.splitext(os.path.basename(name))[0]
                frame_id = traj.meta[
                    "frame_id"] if "frame_id" in traj.meta else ""
                file_interface.write_bag_trajectory(bag, traj, dest_topic,
                                                    frame_id)
            if args.ref:
                dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
                frame_id = ref_traj.meta[
                    "frame_id"] if "frame_id" in ref_traj.meta else ""
                file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
                                                    frame_id)
        finally:
            bag.close()
Example #4
0
def run(args):
    import sys

    import numpy as np

    import evo.core.lie_algebra as lie
    from evo.core import trajectory
    from evo.core.trajectory import PoseTrajectory3D
    from evo.tools import file_interface, log

    log.configure_logging(verbose=args.verbose,
                          silent=args.silent,
                          debug=args.debug,
                          local_logfile=args.logfile)
    if args.debug:
        import pprint
        logger.debug(
            "main_parser config:\n" +
            pprint.pformat({arg: getattr(args, arg)
                            for arg in vars(args)}) + "\n")
    logger.debug(SEP)

    trajectories, ref_traj = load_trajectories(args)

    if args.merge:
        if args.subcommand == "kitti":
            die("Can't merge KITTI files.")
        if len(trajectories) == 0:
            die("No trajectories to merge (excluding --ref).")
        trajectories = {
            "merged_trajectory": trajectory.merge(trajectories.values())
        }

    if args.t_offset:
        logger.debug(SEP)
        for name, traj in trajectories.items():
            if type(traj) is trajectory.PosePath3D:
                die("{} doesn't have timestamps - can't add time offset.".
                    format(name))
            logger.info("Adding time offset to {}: {} (s)".format(
                name, args.t_offset))
            traj.timestamps += args.t_offset

    if args.n_to_align != -1 and not (args.align or args.correct_scale):
        die("--n_to_align is useless without --align or/and --correct_scale")

    # TODO: this is fugly, but is a quick solution for remembering each synced
    # reference when plotting pose correspondences later...
    synced = (args.subcommand == "kitti" and ref_traj) or any(
        (args.sync, args.align, args.correct_scale, args.align_origin))
    synced_refs = {}
    if synced:
        from evo.core import sync
        if not args.ref:
            logger.debug(SEP)
            die("Can't align or sync without a reference! (--ref)  *grunt*")
        for name, traj in trajectories.items():
            if args.subcommand == "kitti":
                ref_traj_tmp = ref_traj
            else:
                logger.debug(SEP)
                ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
                    ref_traj,
                    traj,
                    max_diff=args.t_max_diff,
                    first_name="reference",
                    snd_name=name)
            if args.align or args.correct_scale:
                logger.debug(SEP)
                logger.debug("Aligning {} to reference.".format(name))
                trajectories[name].align(ref_traj_tmp,
                                         correct_scale=args.correct_scale,
                                         correct_only_scale=args.correct_scale
                                         and not args.align,
                                         n=args.n_to_align)
            if args.align_origin:
                logger.debug(SEP)
                logger.debug("Aligning {}'s origin to reference.".format(name))
                trajectories[name].align_origin(ref_traj_tmp)
            if SETTINGS.plot_pose_correspondences:
                synced_refs[name] = ref_traj_tmp

    if args.transform_left or args.transform_right:
        tf_type = "left" if args.transform_left else "right"
        tf_path = args.transform_left \
                if args.transform_left else args.transform_right
        transform = file_interface.load_transform_json(tf_path)
        logger.debug(SEP)
        if not lie.is_se3(transform):
            logger.warning("Not a valid SE(3) transformation!")
        if args.invert_transform:
            transform = lie.se3_inverse(transform)
        logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
            tf_type, transform))
        for traj in trajectories.values():
            traj.transform(transform,
                           right_mul=args.transform_right,
                           propagate=args.propagate_transform)

    for name, traj in trajectories.items():
        print_traj_info(to_compact_name(name, args), traj, args.verbose,
                        args.full_check)
    if args.ref:
        print_traj_info(to_compact_name(args.ref, args), ref_traj,
                        args.verbose, args.full_check)

    if args.plot or args.save_plot or args.serialize_plot:
        import numpy as np
        from evo.tools import plot
        import matplotlib.pyplot as plt
        import matplotlib.cm as cm

        plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
        fig_xyz, axarr_xyz = plt.subplots(3,
                                          sharex="col",
                                          figsize=tuple(SETTINGS.plot_figsize))
        fig_rpy, axarr_rpy = plt.subplots(3,
                                          sharex="col",
                                          figsize=tuple(SETTINGS.plot_figsize))
        fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))

        plot_mode = plot.PlotMode[args.plot_mode]
        ax_traj = plot.prepare_axis(fig_traj, plot_mode)

        # for x-axis alignment starting from 0 with --plot_relative_time
        start_time = None

        if args.ref:
            if isinstance(ref_traj, trajectory.PoseTrajectory3D) \
                    and args.plot_relative_time:
                start_time = ref_traj.timestamps[0]

            short_traj_name = to_compact_name(args.ref, args,
                                              SETTINGS.plot_usetex)
            plot.traj(ax_traj,
                      plot_mode,
                      ref_traj,
                      style=SETTINGS.plot_reference_linestyle,
                      color=SETTINGS.plot_reference_color,
                      label=short_traj_name,
                      alpha=SETTINGS.plot_reference_alpha)
            plot.draw_coordinate_axes(
                ax_traj, ref_traj, plot_mode,
                SETTINGS.plot_reference_axis_marker_scale)
            plot.traj_xyz(axarr_xyz,
                          ref_traj,
                          style=SETTINGS.plot_reference_linestyle,
                          color=SETTINGS.plot_reference_color,
                          label=short_traj_name,
                          alpha=SETTINGS.plot_reference_alpha,
                          start_timestamp=start_time)
            plot.traj_rpy(axarr_rpy,
                          ref_traj,
                          style=SETTINGS.plot_reference_linestyle,
                          color=SETTINGS.plot_reference_color,
                          label=short_traj_name,
                          alpha=SETTINGS.plot_reference_alpha,
                          start_timestamp=start_time)

        if args.ros_map_yaml:
            plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)

        cmap_colors = None
        if SETTINGS.plot_multi_cmap.lower() != "none":
            cmap = getattr(cm, SETTINGS.plot_multi_cmap)
            cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))

        for name, traj in trajectories.items():
            if cmap_colors is None:
                color = next(ax_traj._get_lines.prop_cycler)['color']
            else:
                color = next(cmap_colors)

            short_traj_name = to_compact_name(name, args, SETTINGS.plot_usetex)
            plot.traj(ax_traj,
                      plot_mode,
                      traj,
                      SETTINGS.plot_trajectory_linestyle,
                      color,
                      short_traj_name,
                      alpha=SETTINGS.plot_trajectory_alpha)
            plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
                                      SETTINGS.plot_axis_marker_scale)
            if ref_traj and synced and SETTINGS.plot_pose_correspondences:
                plot.draw_correspondence_edges(
                    ax_traj,
                    traj,
                    synced_refs[name],
                    plot_mode,
                    color=color,
                    style=SETTINGS.plot_pose_correspondences_linestyle,
                    alpha=SETTINGS.plot_trajectory_alpha)
            plot.traj_xyz(axarr_xyz,
                          traj,
                          SETTINGS.plot_trajectory_linestyle,
                          color,
                          short_traj_name,
                          alpha=SETTINGS.plot_trajectory_alpha,
                          start_timestamp=start_time)
            plot.traj_rpy(axarr_rpy,
                          traj,
                          SETTINGS.plot_trajectory_linestyle,
                          color,
                          short_traj_name,
                          alpha=SETTINGS.plot_trajectory_alpha,
                          start_timestamp=start_time)
            if not SETTINGS.plot_usetex:
                fig_rpy.text(0.,
                             0.005,
                             "euler_angle_sequence: {}".format(
                                 SETTINGS.euler_angle_sequence),
                             fontsize=6)

        plot_collection.add_figure("trajectories", fig_traj)
        plot_collection.add_figure("xyz_view", fig_xyz)
        plot_collection.add_figure("rpy_view", fig_rpy)
        if args.plot:
            plot_collection.show()
        if args.save_plot:
            logger.info(SEP)
            plot_collection.export(args.save_plot,
                                   confirm_overwrite=not args.no_warnings)
        if args.serialize_plot:
            logger.info(SEP)
            plot_collection.serialize(args.serialize_plot,
                                      confirm_overwrite=not args.no_warnings)

    if args.save_as_tum:
        logger.info(SEP)
        for name, traj in trajectories.items():
            dest = to_filestem(name, args) + ".tum"
            file_interface.write_tum_trajectory_file(
                dest, traj, confirm_overwrite=not args.no_warnings)
        if args.ref:
            dest = to_filestem(args.ref, args) + ".tum"
            file_interface.write_tum_trajectory_file(
                dest, ref_traj, confirm_overwrite=not args.no_warnings)
    if args.save_as_kitti:
        logger.info(SEP)
        for name, traj in trajectories.items():
            dest = to_filestem(name, args) + ".kitti"
            file_interface.write_kitti_poses_file(
                dest, traj, confirm_overwrite=not args.no_warnings)
        if args.ref:
            dest = to_filestem(args.ref, args) + ".kitti"
            file_interface.write_kitti_poses_file(
                dest, ref_traj, confirm_overwrite=not args.no_warnings)
    if args.save_as_bag or args.save_as_bag2:
        from rosbags.rosbag1 import Writer as Rosbag1Writer
        from rosbags.rosbag2 import Writer as Rosbag2Writer
        writers = []
        if args.save_as_bag:
            dest_bag_path = str(
                datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
            writers.append(Rosbag1Writer(dest_bag_path))
        if args.save_as_bag2:
            dest_bag_path = str(
                datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))
            writers.append(Rosbag2Writer(dest_bag_path))
        for writer in writers:
            logger.info(SEP)
            logger.info("Saving trajectories to " + str(writer.path) + "...")
            try:
                writer.open()
                for name, traj in trajectories.items():
                    dest_topic = to_topic_name(name, args)
                    frame_id = traj.meta[
                        "frame_id"] if "frame_id" in traj.meta else ""
                    file_interface.write_bag_trajectory(
                        writer, traj, dest_topic, frame_id)
                if args.ref:
                    dest_topic = to_topic_name(args.ref, args)
                    frame_id = ref_traj.meta[
                        "frame_id"] if "frame_id" in ref_traj.meta else ""
                    file_interface.write_bag_trajectory(
                        writer, ref_traj, dest_topic, frame_id)
            finally:
                writer.close()

    if args.save_table:
        from evo.tools import pandas_bridge
        logger.debug(SEP)
        df = pandas_bridge.trajectories_stats_to_df(trajectories)
        pandas_bridge.save_df_as_table(df,
                                       args.save_table,
                                       confirm_overwrite=not args.no_warnings)