def test_set_extrinsics(self):
radar_1 = Radar("test")
radar_1.set_extrinsics()
assert radar_1.extrinsics == Transform3D()
radar_2 = Radar("test")
radar_2.set_extrinsics(_TRANSLATION, _ROTATION)
assert radar_2.extrinsics == Transform3D(_TRANSLATION, _ROTATION)
def test_set_extrinsics(self):
camera_1 = Camera("test")
camera_1.set_extrinsics()
assert camera_1.extrinsics == Transform3D()
camera_2 = Camera("test")
camera_2.set_extrinsics(_TRANSLATION, _ROTATION)
assert camera_2.extrinsics == Transform3D(_TRANSLATION, _ROTATION)
def test_set_extrinsics(self):
lidar_1 = Lidar("test")
lidar_1.set_extrinsics()
assert lidar_1.extrinsics == Transform3D()
lidar_2 = Lidar("test")
lidar_2.set_extrinsics(_TRANSLATION, _ROTATION)
assert lidar_2.extrinsics == Transform3D(_TRANSLATION, _ROTATION)
def test_set_extrinsics(self):
fisheye_camera_1 = FisheyeCamera("test")
fisheye_camera_1.set_extrinsics()
assert fisheye_camera_1.extrinsics == Transform3D()
fisheye_camera_2 = FisheyeCamera("test")
fisheye_camera_2.set_extrinsics(_TRANSLATION, _ROTATION)
assert fisheye_camera_2.extrinsics == Transform3D(
_TRANSLATION, _ROTATION)
def test_rmul(self):
quaternion_1 = quaternion(0, 1, 0, 0)
transform_1 = Transform3D([1, 2, 3], [0, 1, 0, 0])
transform_2 = Transform3D([1, -2, -3], [-1, 0, 0, 0])
transform_3 = Transform3D(["a", "b", "c"], [-1, 0, 0, 0])
with pytest.raises(TypeError):
1 * transform_1
assert quaternion_1 * transform_1 == transform_2
assert transform_3.__rmul__(quaternion_1) == NotImplemented
def test_rmul(self):
size = [1, 2, 3]
translation = [1, 2, 3]
rotation = quaternion(0, 1, 0, 0)
transform = Transform3D(translation, rotation)
quaternion_1 = quaternion(1, 2, 3, 4)
labeledbox3d = LabeledBox3D(
size=size,
translation=translation,
rotation=rotation,
category="cat",
attributes={"gender": "male"},
instance="12345",
)
assert labeledbox3d.__rmul__(transform) == LabeledBox3D(
size=size,
translation=[2, 0, 0],
rotation=[-1, 0, 0, 0],
category="cat",
attributes={"gender": "male"},
instance="12345",
)
assert labeledbox3d.__rmul__(quaternion_1) == LabeledBox3D(
size=size,
translation=[1.7999999999999996, 2, 2.6],
rotation=[-2, 1, 4, -3],
category="cat",
attributes={"gender": "male"},
instance="12345",
)
assert labeledbox3d.__rmul__(1) == NotImplemented
def set_rotation(
self,
w: Optional[float] = None,
x: Optional[float] = None,
y: Optional[float] = None,
z: Optional[float] = None,
*,
quaternion: Optional[Quaternion] = None,
) -> None:
"""Set the rotation of the sensor.
Arguments:
w: The w componet of the roation quaternion.
x: The x componet of the roation quaternion.
y: The y componet of the roation quaternion.
z: The z componet of the roation quaternion.
quaternion: Numpy quaternion representing the rotation.
Examples:
>>> sensor.set_rotation(2, 3, 4, 5)
>>> sensor
Lidar("Lidar1")(
(extrinsics): Transform3D(
...
(rotation): quaternion(2, 3, 4, 5)
)
)
"""
if not hasattr(self, "extrinsics"):
self.extrinsics = Transform3D()
self.extrinsics.set_rotation(w, x, y, z, quaternion=quaternion)
def set_extrinsics(
self,
translation: Iterable[float] = (0, 0, 0),
rotation: Transform3D.RotationType = (1, 0, 0, 0),
*,
matrix: Optional[MatrixType] = None,
) -> None:
"""Set the extrinsics of the sensor.
Arguments:
translation: Translation parameters.
rotation: Rotation in a sequence of [w, x, y, z] or numpy quaternion.
matrix: A 3x4 or 4x4 transform matrix.
Examples:
>>> sensor.set_extrinsics(translation=translation, rotation=rotation)
>>> sensor
Lidar("Lidar1")(
(extrinsics): Transform3D(
(translation): Vector3D(1, 2, 3),
(rotation): quaternion(1, 2, 3, 4)
)
)
"""
self.extrinsics = Transform3D(translation, rotation, matrix=matrix)
def test_set_translation(self):
transform = Transform3D()
transform.set_translation(1, 2, 3)
assert transform.translation == Vector3D(1, 2, 3)
transform.set_translation(x=3, y=4, z=5)
assert transform.translation == Vector3D(3, 4, 5)
def test_mul(self):
sequence_1 = [1, 1, 1]
sequence_2 = [[1, 2, 3], [4, 5, 6]]
sequence_3 = ["a", "b", "c"]
quaternion_1 = quaternion(0, 1, 0, 0)
transform_1 = Transform3D([1, 2, 3], [0, 1, 0, 0])
transform_2 = Transform3D([2, 0, 0], [-1, 0, 0, 0])
transform_3 = Transform3D([1, 2, 3], [-1, 0, 0, 0])
assert transform_1 * transform_1 == transform_2
assert transform_1 * quaternion_1 == transform_3
assert transform_1 * sequence_1 == Vector3D(2.0, 1.0, 2.0)
assert transform_1 * np.array(sequence_1) == Vector3D(2.0, 1.0, 2.0)
assert transform_1.__mul__(1) == NotImplemented
assert transform_1.__mul__(sequence_2) == NotImplemented
assert transform_1.__mul__(np.array(sequence_2)) == NotImplemented
assert transform_1.__mul__(sequence_3) == NotImplemented
def test_set_rotation(self):
transform = Transform3D()
transform.set_rotation(0, 1, 0, 0)
assert transform.rotation == quaternion(0, 1, 0, 0)
quaternion_1 = quaternion(0, 1, 0, 0)
transform.set_rotation(quaternion=quaternion_1)
assert transform.rotation == quaternion_1
with pytest.raises(TypeError):
transform.set_rotation([0, 1, 0, 0])
def test_init(self):
sequence = [[1, 0, 0, 1], [0, 1, 0, 1]]
with pytest.raises(ValueError):
Transform3D(matrix=sequence)
transform = Transform3D()
assert transform.translation == Vector3D(0.0, 0.0, 0.0)
assert transform.rotation == quaternion(1.0, 0.0, 0.0, 0.0)
sequence = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1]]
transform = Transform3D(matrix=sequence)
assert transform.translation == Vector3D(1, 1, 1)
assert transform.rotation == quaternion(1.0, -0.0, -0.0, -0.0)
numpy = np.array(sequence)
transform = Transform3D(matrix=numpy)
assert transform.translation == Vector3D(1, 1, 1)
assert transform.rotation == quaternion(1.0, -0.0, -0.0, -0.0)
transform = Transform3D([1, 2, 3], [1, 0, 0, 0])
assert transform.translation == Vector3D(1, 2, 3)
assert transform.rotation == quaternion(1.0, 0.0, 0.0, 0.0)
def test_loads(self):
camera_1 = Sensor.loads(_CAMERA_DATA)
assert camera_1.extrinsics == Transform3D(_TRANSLATION, _ROTATION)
assert camera_1.intrinsics == CameraIntrinsics(fx=1,
fy=1,
cx=1,
cy=1,
skew=0,
p1=1,
p2=1,
k1=1,
k2=1)
def test_rmul(self):
transform = Transform3D([1, 2, 3], quaternion(0, 1, 0, 0))
quaternion_1 = quaternion(1, 2, 3, 4)
box3d = Box3D(size=(1, 1, 1), translation=[1, 2, 3], rotation=quaternion(0, 1, 0, 0))
assert box3d.__rmul__(transform) == Box3D(
size=(1, 1, 1), translation=[2, 0, 0], rotation=quaternion(-1, 0, 0, 0)
)
assert box3d.__rmul__(quaternion_1) == Box3D(
size=(1, 1, 1),
translation=[1.7999999999999996, 2, 2.6],
rotation=quaternion(-2, 1, 4, -3),
)
assert box3d.__rmul__(1) == NotImplemented
def test_mul_vector(self):
sequence_1 = [1, 1, 1]
quaternion_1 = quaternion(0, 1, 0, 0)
transform_1 = Transform3D([1, 2, 3], [0, 1, 0, 0])
with pytest.raises(ValueError):
transform_1._mul_vector(1)
with pytest.raises(TypeError):
transform_1._mul_vector(transform_1)
with pytest.raises(ValueError):
transform_1._mul_vector(quaternion_1)
assert transform_1._mul_vector(sequence_1) == Vector3D(2.0, 1.0, 2.0)
assert transform_1._mul_vector(np.array(sequence_1)) == Vector3D(
2.0, 1.0, 2.0)
def _get_labels(
current_frame_token: str,
lidar_ego_pose_info: Dict[str, Any],
lidar_to_ego: Transform3D,
annotation_info: Dict[str, Dict[str, Any]],
) -> List[LabeledBox3D]:
labels = []
sample_annotations = annotation_info["sample_annotations"]
if current_frame_token in sample_annotations:
lidar_ego_pose = Transform3D(
translation=lidar_ego_pose_info["translation"],
rotation=lidar_ego_pose_info["rotation"],
)
world_to_lidar = (lidar_ego_pose * lidar_to_ego).inverse()
for instance_annotation in sample_annotations[current_frame_token]:
labeled_box = _get_labeled_box(instance_annotation, annotation_info)
labels.append(world_to_lidar * labeled_box)
return labels
def set_translation(self, x: float, y: float, z: float) -> None:
"""Set the translation of the sensor.
Arguments:
x: The x coordinate of the translation.
y: The y coordinate of the translation.
z: The z coordinate of the translation.
Examples:
>>> sensor.set_translation(x=2, y=3, z=4)
>>> sensor
Lidar("Lidar1")(
(extrinsics): Transform3D(
(translation): Vector3D(2, 3, 4),
...
)
)
"""
if not hasattr(self, "extrinsics"):
self.extrinsics = Transform3D()
self.extrinsics.set_translation(x, y, z)
def test_loads(self):
transform = Transform3D.loads(_DATA_TRANSFORM)
assert transform.translation == Vector3D(1.0, 2.0, 3.0)
assert transform.rotation == quaternion(1.0, 0.0, 0.0, 0.0)
def test_create(self):
transform = Transform3D([1, 2, 3], [0, 1, 0, 0])
assert Transform3D._create(Vector3D(1, 2, 3),
quaternion(0, 1, 0, 0)) == transform
def test_eq(self):
transform_1 = Transform3D([1, 2, 3], [1, 0, 0, 0])
transform_2 = Transform3D([1, 2, 3], [1, 0, 0, 0])
transform_3 = Transform3D([1, 1, 1], [1, 0, 0, 0])
assert (transform_1 == transform_2) == True
assert (transform_1 == transform_3) == False
def _loads(self, contents: Dict[str, Any]) -> None:
super()._loads(contents)
extrinsics = contents.get("extrinsics")
if extrinsics:
self.extrinsics = Transform3D.loads(contents["extrinsics"])
def test_inverse(self):
transform_1 = Transform3D([1, 2, 3], [0, 1, 0, 0])
transform_2 = Transform3D([-1, 2, 3], [0, -1, 0, 0])
assert transform_1.inverse() == transform_2
def test_as_matrix(self):
matrix = np.array([[1, 0, 0, 1], [0, -1, 0, 2], [0, 0, -1, 3],
[0, 0, 0, 1]])
transform = Transform3D([1, 2, 3], [0, 1, 0, 0])
np.testing.assert_array_equal(transform.as_matrix(), matrix)
def test_dumps(self):
transform = Transform3D([1, 2, 3], [1, 0, 0, 0])
assert transform.dumps() == _DATA_TRANSFORM
