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_relative_se3(self):
a = lie.random_se3()
b = lie.random_se3()
self.assertTrue(lie.is_se3(a) and lie.is_se3(b))
a_to_b = lie.relative_se3(a, b)
self.assertTrue(lie.is_se3(a_to_b))
b_from_a = a.dot(a_to_b)
self.assertTrue(np.allclose(b_from_a, b))
def rpe_base(Q_i, Q_i_delta, P_i, P_i_delta):
"""
Computes the relative SE(3) error pose for a single pose pair
following the notation of the TUM RGB-D paper.
:param Q_i: reference SE(3) pose at i
:param Q_i_delta: reference SE(3) pose at i+delta
:param P_i: estimated SE(3) pose at i
:param P_i_delta: estimated SE(3) pose at i+delta
:return: the RPE matrix E_i in SE(3)
"""
Q_rel = lie.relative_se3(Q_i, Q_i_delta)
P_rel = lie.relative_se3(P_i, P_i_delta)
E_i = lie.relative_se3(Q_rel, P_rel)
return E_i
def ape_base(x_t, x_t_star):
"""
Computes the absolute error pose for a single SE(3) pose pair
following the notation of the Kümmerle paper.
:param x_t: estimated absolute pose at t
:param x_t_star: reference absolute pose at t
.:return: the delta pose
"""
return lie.relative_se3(x_t, x_t_star)
def rpe_base(Q_i, Q_i_delta, P_i, P_i_delta):
"""
Computes the relative SE(3) error pose for a single pose pair
following the notation of the TUM RGB-D paper.
:param Q_i: reference SE(3) pose at i
:param Q_i_delta: reference SE(3) pose at i+delta
:param P_i: estimated SE(3) pose at i
:param P_i_delta: estimated SE(3) pose at i+delta
:return: the RPE matrix E_i in SE(3)
"""
Q_rel = lie.relative_se3(Q_i, Q_i_delta)
P_rel = lie.relative_se3(P_i, P_i_delta)
E_i = lie.relative_se3(Q_rel, P_rel)
return E_i
def transform(self, t, right_mul=False, propagate=False):
"""
apply a left or right multiplicative transformation to the whole path
:param t: a 4x4 transformation matrix (e.g. SE(3) or Sim(3))
:param right_mul: whether to apply it right-multiplicative or not
:param propagate: whether to propagate drift with RHS transformations
"""
if right_mul and not propagate:
# Transform each pose individually.
self._poses_se3 = [np.dot(p, t) for p in self.poses_se3]
elif right_mul and propagate:
# Transform each pose and propagate resulting drift to the next.
ids = np.arange(0, self.num_poses, 1)
rel_poses = [
lie.relative_se3(self.poses_se3[i], self.poses_se3[j]).dot(t)
for i, j in zip(ids, ids[1:])
]
self._poses_se3 = [self.poses_se3[0]]
for i, j in zip(ids[:-1], ids):
self._poses_se3.append(self._poses_se3[j].dot(rel_poses[i]))
else:
self._poses_se3 = [np.dot(t, p) for p in self.poses_se3]
self._positions_xyz, self._orientations_quat_wxyz \
= se3_poses_to_xyz_quat_wxyz(self.poses_se3)
def rpe_base(Q_i, Q_i_delta, P_i, P_i_delta):
"""
Computes the relative SE(3) error pose for a single pose pair
following the notation of the TUM RGB-D paper.
:param Q_i: reference SE(3) pose at i
:param Q_i_delta: reference SE(3) pose at i+delta
:param P_i: estimated SE(3) pose at i
:param P_i_delta: estimated SE(3) pose at i+delta
:return: the RPE matrix E_i in SE(3)
"""
Q_rel = lie.relative_se3(Q_i, Q_i_delta)
P_rel = lie.relative_se3(P_i, P_i_delta)
E_i = lie.relative_se3(Q_rel, P_rel)
return E_i