def align_trajectory(traj,
traj_ref,
correct_scale=False,
correct_only_scale=False,
n=-1,
return_parameters=False,
correct_origin=False):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:param return_parameters: also return result parameters of Umeyama's method
:return: the aligned trajectory
If return_parameters is set, the rotation matrix, translation vector and
scaling parameter of Umeyama's method are also returned.
"""
# otherwise np arrays will be references and mess up stuff
traj_aligned = copy.deepcopy(traj)
# quat = traj_ref.orientations_quat_wxyz[0,:]
# r_ref = tr.quaternion_matrix(quat)[:3,:3]
# t_ref = traj_ref.positions_xyz[0,:]
# traj_aligned.transform(lie.se3(r_ref, t_ref),propagate=True)
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz.T,
traj_ref.positions_xyz.T,
with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
if return_parameters:
return traj_aligned, r_a, t_a, s
else:
return traj_aligned
def xyz_quat_wxyz_to_se3_poses(
xyz: np.ndarray, quat: np.ndarray) -> typing.Sequence[np.ndarray]:
poses = [
lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
for quat, xyz in zip(quat, xyz)
]
return poses
def convert_rel_traj_to_abs_traj(traj):
""" Converts a relative pose trajectory to an absolute-pose trajectory.
The incoming trajectory is processed elemente-wise. Poses at each
timestamp are appended to the absolute pose from the previous timestamp.
Args:
traj: A PoseTrajectory3D object with timestamps as indices containing, at a minimum,
columns representing the xyz position and wxyz quaternion-rotation at each
timestamp, corresponding to the pose between previous and current timestamps.
Returns:
A PoseTrajectory3D object with xyz position and wxyz quaternion fields for the
relative pose trajectory corresponding to the relative one given in `traj`.
"""
from evo.core import lie_algebra as lie
new_poses = [lie.se3()] # origin at identity
for i in range(0, len(traj.timestamps)):
abs_pose = np.dot(new_poses[-1], traj.poses_se3[i])
new_poses.append(abs_pose)
return trajectory.PoseTrajectory3D(timestamps=traj.timestamps[1:],
poses_se3=new_poses)
def scale(self, s):
"""
apply a scaling to the whole path
:param s: scale factor
"""
if hasattr(self, "_poses_se3"):
self._poses_se3 = [lie.se3(p[:3, :3], s*p[:3, 3]) for p in self._poses_se3]
if hasattr(self, "_positions_xyz"):
self._positions_xyz = s * self._positions_xyz
def scale(self, s):
"""
apply a scaling to the whole path
:param s: scale factor
"""
if hasattr(self, "_poses_se3"):
self._poses_se3 = [lie.se3(p[:3, :3], s*p[:3, 3]) for p in self._poses_se3]
if hasattr(self, "_positions_xyz"):
self._positions_xyz = s * self._positions_xyz
def test_sim3_scale_effect(self):
r = lie.random_so3()
t = np.array([0, 0, 0])
s = random.random() * 10
x = np.array([1, 0, 0, 1]).T # homogeneous vector
p = lie.sim3(r, t, s)
self.assertTrue(lie.is_sim3(p, s))
x = p.dot(x) # apply Sim(3) transformation
self.assertTrue(np.equal(x, lie.se3(r).dot(np.array([s, 0, 0, 1]))).all())
def align(self,
traj_ref: 'PosePath3D',
correct_scale: bool = False,
correct_only_scale: bool = False,
n: int = -1) -> geometry.UmeyamaResult:
"""
align to a reference trajectory using Umeyama alignment
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the result parameters of the Umeyama algorithm
"""
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(self.positions_xyz.T,
traj_ref.positions_xyz.T,
with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
self.positions_xyz[:n, :].T, traj_ref.positions_xyz[:n, :].T,
with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
self.scale(s)
elif correct_scale:
self.scale(s)
self.transform(lie.se3(r_a, t_a))
else:
self.transform(lie.se3(r_a, t_a))
return r_a, t_a, s
def align_trajectory(traj, traj_ref, correct_scale=False,
correct_only_scale=False, n=-1):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the aligned trajectory
"""
# otherwise np arrays will be references and mess up stuff
traj_aligned = copy.deepcopy(traj)
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz.T, traj_ref.positions_xyz.T, with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
return traj_aligned
def align_trajectory(traj,
traj_ref,
correct_scale=False,
correct_only_scale=False,
n=-1):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the aligned trajectory
"""
traj_aligned = copy.deepcopy(
traj) # otherwise np arrays will be references and mess up stuff
with_scale = correct_scale or correct_only_scale
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz.T,
traj_ref.positions_xyz.T,
with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logging.debug("rotation of alignment:\n" + str(r_a) +
"\ntranslation of alignment:\n" + str(t_a))
logging.debug("scale correction: " + str(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
return traj_aligned
def align_trajectory(traj, traj_ref, correct_scale=False, correct_only_scale=False, n=-1):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:return: the aligned trajectory
"""
traj_aligned = copy.deepcopy(traj) # otherwise np arrays will be references and mess up stuff
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..."
+ (" (with scale correction)" if with_scale else ""))
if n == -1:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz.T,
traj_ref.positions_xyz.T, with_scale)
else:
r_a, t_a, s = geometry.umeyama_alignment(traj_aligned.positions_xyz[:n, :].T,
traj_ref.positions_xyz[:n, :].T, with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
return traj_aligned
def load_transform_json(json_path):
"""
load a transformation stored in xyz + quaternion format in a .json file
:param json_path: path to the .json file
:return: t (SE(3) matrix), xyz (position), quat (orientation quaternion)
"""
with open(json_path, 'r') as tf_file:
data = json.load(tf_file)
keys = ("x", "y", "z", "qx", "qy", "qz", "qw")
if not all(key in data for key in keys):
raise FileInterfaceException(
"invalid transform file - expected keys " + str(keys))
xyz = np.array([data["x"], data["y"], data["z"]])
quat = np.array([data["qw"], data["qx"], data["qy"], data["qz"]])
t = lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
return t, xyz, quat
You should have received a copy of the GNU General Public License
along with evo. If not, see <http://www.gnu.org/licenses/>.
"""
import math
import unittest
import numpy as np
import context
from evo.core import filters
from evo.core import lie_algebra as lie
# some synthetic poses for testing
# [0] [1]
poses_1 = [lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 0.5])),
lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 1]))]
# [2] [3]
# [0] [1]
poses_2 = [lie.se3(np.eye(3), np.array([0, 0, 0])), lie.se3(np.eye(3), np.array([0, 0, 0.5])),
lie.se3(np.eye(3), np.array([0, 0, 0.99])), lie.se3(np.eye(3), np.array([0, 0, 1.0]))]
# [2] [3]
# [0] [1]
poses_3 = [lie.se3(np.eye(3), np.array([0, 0, 0.0])), lie.se3(np.eye(3), np.array([0, 0, 0.9])),
lie.se3(np.eye(3), np.array([0, 0, 0.99])), lie.se3(np.eye(3), np.array([0, 0, 0.999])),
lie.se3(np.eye(3), np.array([0, 0, 0.9999])), lie.se3(np.eye(3), np.array([0, 0, 0.99999])),
lie.se3(np.eye(3), np.array([0, 0, 0.999999])), lie.se3(np.eye(3), np.array([0, 0, 0.9999999]))]
# [6] [7]
def align_trajectory(traj,
traj_ref,
correct_scale=False,
correct_only_scale=False,
return_parameters=False,
n=-1,
discard_n_start_poses=0,
discard_n_end_poses=0):
"""
align a trajectory to a reference using Umeyama alignment
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:param correct_scale: set to True to adjust also the scale
:param correct_only_scale: set to True to correct the scale, but not the pose
:param n: the number of poses to use, counted from the start (default: all)
:param return_parameters: also return result parameters of Umeyama's method
:param discard_n_start_poses: the number of poses to skip from the start (default: none)
:param discard_n_end_poses: the number of poses to discard counted from the end (default: none)
:return: the aligned trajectory
If return_parameters is set, the rotation matrix, translation vector and
scaling parameter of Umeyama's method are also returned.
"""
# otherwise np arrays will be references and mess up stuff
traj_aligned = copy.deepcopy(traj)
with_scale = correct_scale or correct_only_scale
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..." +
(" (with scale correction)" if with_scale else ""))
end_pose_idx = len(traj_aligned.positions_xyz)
if n != -1:
if discard_n_start_poses != 0 or discard_n_end_poses != 0:
print(
"WARNING: you asked for %d poses, as well as discarding %d start poses and %d end poses.",
n, discard_n_start_poses, discard_n_end_poses)
print("Selecting option given the smallest number of poses:")
if end_pose_idx - discard_n_end_poses < n + discard_n_start_poses:
print(
"We are requiring more poses n (%d) than available, using instead %d starting from pose %d",
n,
end_pose_idx - discard_n_end_poses - discard_n_start_poses,
discard_n_start_poses)
end_pose_idx = end_pose_idx - discard_n_end_poses
else:
end_pose_idx = n + discard_n_start_poses
else:
end_pose_idx -= discard_n_end_poses
assert (discard_n_start_poses < end_pose_idx
) # Otherwise there will be no poses to align.
assert (end_pose_idx <= len(traj_aligned.positions_xyz)
) # Otherwise we will be requiring more poses than there are.
r_a, t_a, s = geometry.umeyama_alignment(
traj_aligned.positions_xyz[discard_n_start_poses:end_pose_idx, :].T,
traj_ref.positions_xyz[discard_n_start_poses:end_pose_idx, :].T,
with_scale)
if not correct_only_scale:
logger.debug("Rotation of alignment:\n{}"
"\nTranslation of alignment:\n{}".format(r_a, t_a))
logger.debug("Scale correction: {}".format(s))
if correct_only_scale:
traj_aligned.scale(s)
elif correct_scale:
traj_aligned.scale(s)
traj_aligned.transform(lie.se3(r_a, t_a))
else:
traj_aligned.transform(lie.se3(r_a, t_a))
if return_parameters:
return traj_aligned, r_a, t_a, s
else:
return traj_aligned
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), np.array([0, 0, 0])))
traj_est.scale(0.5)
logger.info("\nUmeyama alignment without scaling")
traj_est_aligned = copy.deepcopy(traj_est)
traj_est_aligned.align(traj_ref)
logger.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = copy.deepcopy(traj_est)
traj_est_aligned_scaled.align(traj_ref, correct_scale=True)
logger.info("\nUmeyama alignment with scaling only")
traj_est_aligned_only_scaled = copy.deepcopy(traj_est)
traj_est_aligned_only_scaled.align(traj_ref, correct_only_scale=True)
fig = plt.figure(figsize=(8, 8))
def xyz_quat_wxyz_to_se3_poses(xyz, quat):
poses = [
lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
for quat, xyz in zip(quat, xyz)
]
return poses
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with evo. If not, see <http://www.gnu.org/licenses/>.
"""
import math
import unittest
import numpy as np
from evo.core import filters
from evo.core import lie_algebra as lie
poses_1 = [
lie.se3(np.eye(3), np.array([0, 0, 0])),
lie.se3(np.eye(3), np.array([0, 0, 0.5])),
lie.se3(np.eye(3), np.array([0, 0, 0])),
lie.se3(np.eye(3), np.array([0, 0, 1]))
]
poses_2 = [
lie.se3(np.eye(3), np.array([0, 0, 0])),
lie.se3(np.eye(3), np.array([0, 0, 0.5])),
lie.se3(np.eye(3), np.array([0, 0, 0.99])),
lie.se3(np.eye(3), np.array([0, 0, 1.0]))
]
poses_3 = [
lie.se3(np.eye(3), np.array([0, 0, 0.0])),
lie.se3(np.eye(3), np.array([0, 0, 0.9])),
def xyz_quat_wxyz_to_se3_poses(xyz, quat):
poses = [lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
for quat, xyz in zip(quat, xyz)]
return poses
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(
traj_est, traj_ref, correct_only_scale=True)
fig = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xz
from evo.tools import plot, file_interface
import evo.core.lie_algebra as lie
import numpy as np
import matplotlib.pyplot as plt
logging.basicConfig(format="%(message)s",
stream=sys.stdout,
level=logging.DEBUG)
traj_ref = file_interface.read_kitti_poses_file("../data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file("../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)
logging.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logging.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(traj_est,
traj_ref,
correct_scale=True)
logging.info("\nUmeyama alignment with scaling only")
traj_est_aligned_only_scaled = trajectory.align_trajectory(
traj_est, traj_ref, correct_only_scale=True)
fig = plt.figure(figsize=(8, 8))
# fig.suptitle("Umeyama $\mathrm{Sim}(3)$ alignment")
plot_mode = plot.PlotMode.xz
