# Buggy code
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,
            )
            fig_rpy.text(
                0.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()


# Traceback (RuntimeError)
[DEBUG][2020-01-30 17:55:20,837][main_traj.run():284]
--------------------------------------------------------------------------------
[DEBUG][2020-01-30 17:55:21,225][file_interface.read_bag_trajectory():271]
Loaded 1762 nav_msgs/Odometry messages of topic: /odom
[DEBUG][2020-01-30 17:55:21,832][file_interface.read_bag_trajectory():271]
Loaded 18871 geometry_msgs/PoseStamped messages of topic: /ground_truth
[DEBUG][2020-01-30 17:55:21,853][main_traj.run():335]
--------------------------------------------------------------------------------
[DEBUG][2020-01-30 17:55:21,930][sync.associate_trajectories():106]
Found 314 of max. 18871 possible matching timestamps between...
reference
and:	/odom
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
[DEBUG][2020-01-30 17:55:21,930][main_traj.run():340]
--------------------------------------------------------------------------------
[DEBUG][2020-01-30 17:55:21,930][main_traj.run():341]
Aligning /odom to reference.
[DEBUG][2020-01-30 17:55:21,930][trajectory.align_trajectory():394]
Aligning using Umeyama's method...
[DEBUG][2020-01-30 17:55:21,933][trajectory.align_trajectory():404]
Rotation of alignment:
[[ 0.04979001 -0.99868181 -0.01247348]
[ 0.99874792  0.04972487  0.00547931]
[-0.00485184 -0.01273067  0.99990719]]
Translation of alignment:
[-0.31561156  2.12629718 -0.04271305]
[DEBUG][2020-01-30 17:55:21,933][trajectory.align_trajectory():405]
Scale correction: 1.0
[INFO][2020-01-30 17:55:21,952][main_traj.print_traj_info():231]
--------------------------------------------------------------------------------
[INFO][2020-01-30 17:55:21,952][main_traj.print_traj_info():235]
name:	/odom
[INFO][2020-01-30 17:55:21,953][main_traj.print_traj_info():242]
infos:
duration (s)	176.05091094970703
nr. of poses	314
path length (m)	23.9148423636
pos_end (m)	[-0.25087702  2.17551795  1.6117157 ]
pos_start (m)	[-0.28801754  2.18658985  1.58796842]
t_end (s)	1576166447.401164
t_start (s)	1576166271.350253
[INFO][2020-01-30 17:55:21,985][main_traj.print_traj_info():248]
checks:
SE(3) conform	yes
array shapes	ok
nr. of stamps	ok
quaternions	ok
timestamps	ok
[INFO][2020-01-30 17:55:21,991][main_traj.print_traj_info():260]
stats:
v_avg (km/h)	0.500316
v_avg (m/s)	0.138977
v_max (km/h)	1.310963
v_max (m/s)	0.364156
v_min (km/h)	0.001935
v_min (m/s)	0.000538
[INFO][2020-01-30 17:55:21,992][main_traj.print_traj_info():231]
--------------------------------------------------------------------------------
[INFO][2020-01-30 17:55:21,992][main_traj.print_traj_info():235]
name:	/ground_truth
[INFO][2020-01-30 17:55:21,994][main_traj.print_traj_info():242]
infos:
duration (s)	176.06113266944885
nr. of poses	18871
path length (m)	26.072393337
pos_end (m)	[-0.26233882  2.16755581  1.5922972 ]
pos_start (m)	[-0.29931393  2.17557931  1.59043825]
t_end (s)	1576166447.4128075
t_start (s)	1576166271.3516748
[INFO][2020-01-30 17:55:24,239][main_traj.print_traj_info():248]
checks:
SE(3) conform	yes
array shapes	ok
nr. of stamps	ok
quaternions	ok
timestamps	ok
[INFO][2020-01-30 17:55:24,355][main_traj.print_traj_info():260]
stats:
v_avg (km/h)	5.076679
v_avg (m/s)	1.410189
v_max (km/h)	1248.546319
v_max (m/s)	346.818422
v_min (km/h)	0.000102
v_min (m/s)	0.000028
[ERROR][2020-01-30 17:55:26,409][entry_points.launch():99]
Unhandled error in evo.main_traj
Traceback (most recent call last):
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/evo/entry_points.py", line 90, in launch
main_module.run(args)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/evo/main_traj.py", line 436, in run
plot_collection.show()
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/evo/tools/plot.py", line 193, in show
self.tabbed_tk_window()
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/evo/tools/plot.py", line 168, in tabbed_tk_window
canvas.draw()
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/backends/backend_tkagg.py", line 12, in draw
super(FigureCanvasTkAgg, self).draw()
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/backends/backend_agg.py", line 437, in draw
self.figure.draw(self.renderer)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/artist.py", line 55, in draw_wrapper
return draw(artist, renderer, *args, **kwargs)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/figure.py", line 1493, in draw
renderer, self, artists, self.suppressComposite)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/image.py", line 141, in _draw_list_compositing_images
a.draw(renderer)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/artist.py", line 55, in draw_wrapper
return draw(artist, renderer, *args, **kwargs)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/text.py", line 706, in draw
bbox, info, descent = textobj._get_layout(renderer)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/text.py", line 309, in _get_layout
ismath=ismath)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/backends/backend_agg.py", line 236, in get_text_width_height_descent
s, fontsize, renderer=self)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/texmanager.py", line 501, in get_text_width_height_descent
dvifile = self.make_dvi(tex, fontsize)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/texmanager.py", line 365, in make_dvi
texfile], tex)
File "/home/riccardo/.virtualenvs/evaluation/local/lib/python2.7/site-packages/matplotlib/texmanager.py", line 344, in _run_checked_subprocess
exc=exc.output.decode('utf-8')))
RuntimeError: latex was not able to process the following string:
'euler_angle_sequence: sxyz'

Here is the full report generated by latex:
This is pdfTeX, Version 3.14159265-2.6-1.40.16 (TeX Live 2015/Debian) (preloaded format=latex)
restricted \write18 enabled.
entering extended mode

(/home/riccardo/.cache/matplotlib/tex.cache/28d4ede8ae4c43d4c3576b57c32bd235.te
x
LaTeX2e <2016/02/01>
Babel <3.9q> and hyphenation patterns for 6 language(s) loaded.
(/usr/share/texlive/texmf-dist/tex/latex/base/article.cls
Document Class: article 2014/09/29 v1.4h Standard LaTeX document class
(/usr/share/texlive/texmf-dist/tex/latex/base/size10.clo))
(/usr/share/texlive/texmf-dist/tex/latex/type1cm/type1cm.sty)
(/usr/share/texlive/texmf-dist/tex/latex/base/textcomp.sty
(/usr/share/texlive/texmf-dist/tex/latex/base/ts1enc.def))
(/usr/share/texlive/texmf-dist/tex/latex/geometry/geometry.sty
(/usr/share/texlive/texmf-dist/tex/latex/graphics/keyval.sty)
(/usr/share/texlive/texmf-dist/tex/generic/oberdiek/ifpdf.sty)
(/usr/share/texlive/texmf-dist/tex/generic/oberdiek/ifvtex.sty)
(/usr/share/texlive/texmf-dist/tex/generic/ifxetex/ifxetex.sty)

Package geometry Warning: Over-specification in `h'-direction.
`width' (5058.9pt) is ignored.


Package geometry Warning: Over-specification in `v'-direction.
`height' (5058.9pt) is ignored.

) (./28d4ede8ae4c43d4c3576b57c32bd235.aux)
(/usr/share/texlive/texmf-dist/tex/latex/base/ts1cmr.fd)
*geometry* driver: auto-detecting
*geometry* detected driver: dvips
! Missing $ inserted.
<inserted text>
$
l.13 ...tsize{6.000000}{7.500000}{\rmfamily euler_
angle_sequence: sxyz}
No pages of output.
Transcript written on 28d4ede8ae4c43d4c3576b57c32bd235.log.



[ERROR][2020-01-30 17:55:26,411][entry_points.launch():106]
evo module evo.main_traj crashed - see /home/riccardo/.evo/evo.log

# Fixed code
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,
                    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()