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])
Beispiel #2
0
    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))