def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_rotation(), Quaternion))
self.assertTrue(isinstance(value.get_translation(), np.ndarray))
# - Value.
self.assertTrue(
np.allclose(value.get_isometry().matrix(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = Isometry3(q, p)
value.set_translation(p)
value.set_rotation(q)
self.assertTrue(
np.allclose(value.get_isometry().matrix(), X_expected.matrix()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
self.assertTrue(
np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEquals(value.size(), PoseVector.kSize)
# - Accessors.
self.assertTrue(isinstance(
value.get_isometry(), Isometry3))
self.assertTrue(isinstance(
value.get_rotation(), Quaternion))
self.assertTrue(isinstance(
value.get_translation(), np.ndarray))
# - - Value.
self.assertTrue(np.allclose(
value.get_isometry().matrix(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = Isometry3(q, p)
value.set_translation(p)
value.set_rotation(q)
self.assertTrue(np.allclose(
value.get_isometry().matrix(), X_expected.matrix()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
def test_roll_pitch_yaw(self):
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0))
q_I = Quaternion()
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
def test_roll_pitch_yaw(self):
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.get_roll_angle(), rpy.get_pitch_angle(), rpy.get_yaw_angle()),
(0, 0, 0))
q_I = Quaternion()
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
(0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
def test_rotation_matrix(self):
R = mut.RotationMatrix()
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
self.assertTrue(
np.allclose(mut.RotationMatrix.Identity().matrix(), np.eye(3)))
R = mut.RotationMatrix(quaternion=Quaternion.Identity())
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0]))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = mut.RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz()))
# - Inverse.
R_I = R.inverse().multiply(R)
self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
def test_rotation_matrix(self):
R = mut.RotationMatrix()
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
self.assertTrue(np.allclose(
mut.RotationMatrix.Identity().matrix(), np.eye(3)))
R = mut.RotationMatrix(quaternion=Quaternion.Identity())
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0]))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = mut.RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz()))
# - Inverse.
R_I = R.inverse().multiply(R)
self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
(0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))