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