Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def test_too_many_columns(self):
self.mock_file.write(u"1 2 3 4 5 6 7 8 9 10 11 12 13")
self.mock_file.seek(0)
with self.assertRaises(file_interface.FileInterfaceException):
file_interface.read_kitti_poses_file(self.mock_file)
def test_too_few_columns_with_trailing_delim(self):
self.mock_file.write(u"1 2 3 4 5 6 7 8 9 10 11 ")
self.mock_file.seek(0)
with self.assertRaises(file_interface.FileInterfaceException):
file_interface.read_kitti_poses_file(self.mock_file)
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
args.align = True
logger.info("forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)")
logger.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import rosbag
logger.debug("opening bag file " + args.bag)
bag = rosbag.Bag(args.bag, 'r')
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file == args.ref:
continue
trajectories[traj_file] = file_interface.read_tum_trajectory_file(
traj_file)
if args.ref:
ref_traj = file_interface.read_tum_trajectory_file(args.ref)
elif args.subcommand == "kitti":
for pose_file in args.pose_files:
if pose_file == args.ref:
continue
trajectories[pose_file] = file_interface.read_kitti_poses_file(
pose_file)
if args.ref:
ref_traj = file_interface.read_kitti_poses_file(args.ref)
elif args.subcommand == "euroc":
for csv_file in args.state_gt_csv:
if csv_file == args.ref:
continue
else:
trajectories[
csv_file] = file_interface.read_euroc_csv_trajectory(
csv_file)
if args.ref:
ref_traj = file_interface.read_euroc_csv_trajectory(args.ref)
elif args.subcommand == "bag":
if not (args.topics or args.all_topics):
die("No topics used - specify topics or set --all_topics.")
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
"""
import logging
import sys
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.tools import plot, file_interface, log
import numpy as np
import matplotlib.pyplot as plt
logger = logging.getLogger("evo")
log.configure_logging(verbose=True)
traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file(
"../test/data/KITTI_00_ORB.txt")
# add artificial Sim(3) transformation
traj_est.transform(lie.se3(np.eye(3), [0, 0, 0]))
traj_est.scale(0.5)
logger.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logger.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale=True)
logger.info("\nUmeyama alignment with scaling only")
traj_est_aligned_only_scaled = trajectory.align_trajectory(
from evo.tools import file_interface
trajectories = OrderedDict()
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file == args.ref:
continue
trajectories[traj_file] = file_interface.read_tum_trajectory_file(
traj_file)
if args.ref:
ref_traj = file_interface.read_tum_trajectory_file(args.ref)
elif args.subcommand == "kitti":
for pose_file in args.pose_files:
if pose_file == args.ref:
continue
trajectories[pose_file] = file_interface.read_kitti_poses_file(
pose_file)
if args.ref:
ref_traj = file_interface.read_kitti_poses_file(args.ref)
elif args.subcommand == "euroc":
for csv_file in args.state_gt_csv:
if csv_file == args.ref:
continue
else:
trajectories[
csv_file] = file_interface.read_euroc_csv_trajectory(
csv_file)
if args.ref:
ref_traj = file_interface.read_euroc_csv_trajectory(args.ref)
elif args.subcommand == "bag":
if not (args.topics or args.all_topics):
die("No topics used - specify topics or set --all_topics.")
pose_relation = metrics.PoseRelation.rotation_angle_deg
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
args.align = True
logger.info("forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)")
logger.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import rosbag
logger.debug("opening bag file " + args.bag)
def load_trajectories(args):
from evo.core import sync
from evo.tools import file_interface
if args.subcommand == "tum":
traj_ref = file_interface.read_tum_trajectory_file(args.ref_file)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import os
logger.debug("Opening bag file " + args.bag)
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
import rosbag
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic)
def load_trajectories(args):
from evo.core import sync
from evo.tools import file_interface
if args.subcommand == "tum":
traj_ref = file_interface.read_tum_trajectory_file(args.ref_file)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import os
logger.debug("Opening bag file " + args.bag)
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
import rosbag
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic)
traj_est = file_interface.read_bag_trajectory(bag, args.est_topic)