def test_matrix_euler(self):
rx = math.radians(10)
ry = math.radians(20)
rz = math.radians(30)
euler = [rz, ry, rx]
m = utils.euler_to_matrix(euler)
euler2 = utils.matrix_to_euler(m)
for i in range(3):
self.assertAlmostEqual(euler[i], euler2[i])
def cylindrical_symmetric_rot(self, det_rot, gt_rot, axis):
"""
Compute rot that zeroes out the rotation error between
<det_rot> and <gt_rot> along axis <axis>.
Inputs
det_rot (Rot3) - rotation for detection
gt_rot (Rot3) - rotation for ground truth
axis (int) - cylindrical rotation axis (0=x, 1=y, 2=z)
Return:
Rot3 - new rotation matrix with gt set to det rotation on target axis
Algorithm:
1. convert to zyx Euler angles
2. set gt to detection value for target axis
3. convert back to matrix rep
"""
det_euler = utils.matrix_to_euler(det_rot.R)
gt_euler = utils.matrix_to_euler(gt_rot.R)
# note e[0] is z
det_euler[2 - axis] = gt_euler[2 - axis]
return Rot3(utils.euler_to_matrix(det_euler))