From c0491129c0a073cdbb9661aea390f54f38a3c848 Mon Sep 17 00:00:00 2001 From: Kun Lin Date: Wed, 9 Apr 2025 16:37:38 +0900 Subject: [PATCH] feat: add probability annotations to 3D key points and lines --- dgp/__init__.py | 2 +- dgp/annotations/__init__.py | 10 +- dgp/annotations/key_line_3d_annotation.py | 175 +- dgp/proto/annotations.proto | 65 + dgp/utils/math.py | 108 + dgp/utils/pose.py | 17 +- dgp/utils/structures/__init__.py | 6 + dgp/utils/structures/key_line_3d.py | 115 +- dgp/utils/structures/key_point_3d.py | 112 +- dgp/utils/visualization_utils.py | 260 ++- .../annotation/test_key_line_3d_annotation.py | 15 +- .../probabilistic_key_line_annotation.json | 1783 +++++++++++++++++ tests/pose_fixtures.py | 25 + tests/utils/structures/test_key_line_3d.py | 62 +- tests/utils/structures/test_key_point_3d.py | 53 + tests/utils/test_math.py | 43 + 16 files changed, 2727 insertions(+), 124 deletions(-) create mode 100644 dgp/utils/math.py create mode 100644 tests/data/dgp/key_line_3d/scene_000000/key_line_3d/lcm_25tm/probabilistic_key_line_annotation.json create mode 100644 tests/pose_fixtures.py create mode 100644 tests/utils/structures/test_key_point_3d.py create mode 100644 tests/utils/test_math.py diff --git a/dgp/__init__.py b/dgp/__init__.py index 01ca8b95..095b6bcb 100644 --- a/dgp/__init__.py +++ b/dgp/__init__.py @@ -1,7 +1,7 @@ # Copyright 2021-2022 Toyota Research Institute. All rights reserved. import os -__version__ = "2.0.1" +__version__ = "2.1.0" DGP_PATH = os.getenv("DGP_PATH", default=os.getenv("HOME", os.getcwd())) DGP_DATA_DIR = os.path.join(DGP_PATH, ".dgp") diff --git a/dgp/annotations/__init__.py b/dgp/annotations/__init__.py index db27b510..6d17e700 100644 --- a/dgp/annotations/__init__.py +++ b/dgp/annotations/__init__.py @@ -20,6 +20,7 @@ from dgp.annotations.semantic_segmentation_2d_annotation import SemanticSegmentation2DAnnotation # isort:skip from dgp.annotations.key_line_2d_annotation import KeyLine2DAnnotationList # isort:skip from dgp.annotations.key_line_3d_annotation import KeyLine3DAnnotationList # isort:skip +from dgp.annotations.key_line_3d_annotation import ProbabilisticKeyLine3DAnnotationList # isort:skip from dgp.annotations.key_point_2d_annotation import KeyPoint2DAnnotationList # isort:skip from dgp.annotations.key_point_3d_annotation import KeyPoint3DAnnotationList # isort:skip from dgp.annotations.depth_annotation import DenseDepthAnnotation # isort:skip @@ -36,12 +37,13 @@ "key_point_3d": KeyPointOntology, "key_line_2d": KeyLineOntology, "key_line_3d": KeyLineOntology, + "probabilistic_key_line_3d": KeyLineOntology, "agent_behavior": AgentBehaviorOntology, "depth": None, "surface_normals_2d": None, "surface_normals_3d": None, "motion_vectors_2d": None, - "motion_vectors_3d": None + "motion_vectors_3d": None, } # Annotation objects for each annotation type @@ -54,7 +56,8 @@ "key_point_3d": KeyPoint3DAnnotationList, "key_line_2d": KeyLine2DAnnotationList, "key_line_3d": KeyLine3DAnnotationList, - "depth": DenseDepthAnnotation + "probabilistic_key_line_3d": ProbabilisticKeyLine3DAnnotationList, + "depth": DenseDepthAnnotation, } # Annotation groups for each annotation type: 2d/3d @@ -73,5 +76,6 @@ "key_line_2d": "2d", "key_point_3d": "3d", "key_line_3d": "3d", - "depth": "2d" + "probabilistic_key_line_3d": "3d", + "depth": "2d", } diff --git a/dgp/annotations/key_line_3d_annotation.py b/dgp/annotations/key_line_3d_annotation.py index 8a003346..333156a7 100644 --- a/dgp/annotations/key_line_3d_annotation.py +++ b/dgp/annotations/key_line_3d_annotation.py @@ -1,16 +1,27 @@ # Copyright 2022 Woven Planet. All rights reserved. +from typing import Any, Dict, List, Union + import numpy as np from dgp.annotations.base_annotation import Annotation from dgp.annotations.ontology import KeyLineOntology from dgp.proto.annotations_pb2 import KeyLine3DAnnotation, KeyLine3DAnnotations +from dgp.proto.annotations_pb2 import \ + ProbabilisticKeyLine3DAnnotation as ProtoProbabilisticKeyLine3DAnnotation +from dgp.proto.annotations_pb2 import \ + ProbabilisticKeyLine3DAnnotations as ProtoProbabilisticKeyLine3DAnnotations +from dgp.proto.annotations_pb2 import \ + ProbabilisticKeyPoint3D as ProtoProbabilisticKeyPoint3D from dgp.utils.protobuf import ( generate_uid_from_pbobject, parse_pbobject, save_pbobject_as_json, ) -from dgp.utils.structures.key_line_3d import KeyLine3D -from dgp.utils.structures.key_point_3d import KeyPoint3D +from dgp.utils.structures.key_line_3d import KeyLine3D, ProbabilisticKeyLine3D +from dgp.utils.structures.key_point_3d import ( + KeyPoint3D, + ProbabilisticKeyPoint3D, +) class KeyLine3DAnnotationList(Annotation): @@ -79,10 +90,10 @@ def to_proto(self): class_id=line.class_id, instance_id=line.instance_id, color=line.color, - attributes=line.attributes + attributes=line.attributes, ).to_proto() for x, y, z in zip(line.x, line.y, line.z) ], - attributes=line.attributes + attributes=line.attributes, ) for line in self._linelist ] ) @@ -138,3 +149,159 @@ def instance_ids(self): def hexdigest(self): """Reproducible hash of annotation.""" return generate_uid_from_pbobject(self.to_proto()) + + +class ProbabilisticKeyLine3DAnnotationList(Annotation): + """Container for probabilistic 3D key line annotations. + + Parameters + ---------- + ontology: KeyLineOntology + Ontology for 3D key line tasks. + + linelist: list[ProbabilisticKeyLine3D] + List of ProbabilisticKeyLine3D objects. See `dgp/utils/structures/key_line_3d` for more details. + """ + def __init__(self, ontology: KeyLineOntology, linelist: List[ProbabilisticKeyLine3D]) -> None: + Annotation.__init__(self, ontology) + assert isinstance(self._ontology, KeyLineOntology), "Trying to load annotation with wrong type of ontology!" + for line in linelist: + assert isinstance( + line, ProbabilisticKeyLine3D + ), f"Can only instantiate an annotation from a list of ProbabilisticKeyLine3D, not {type(line)}!" + self._linelist = linelist + + @classmethod + def load( + cls, annotation_file: Union[str, bytes], ontology: KeyLineOntology + ) -> "ProbabilisticKeyLine3DAnnotationList": + """Load annotation from annotation file and ontology. + + Parameters + ---------- + annotation_file: str or bytes + Full path to annotation or bytestring + + ontology: KeyLineOntology + Ontology for 3D key line tasks. + + Returns + ------- + KeyLine3DAnnotationList + Annotation object instantiated from file. + """ + _annotation_pb2 = parse_pbobject( + annotation_file, + ProtoProbabilisticKeyLine3DAnnotations, + ) + annotation_list = [ + ProbabilisticKeyLine3D( + points=[ + ProbabilisticKeyPoint3D( + point=np.asarray([ + vert.x, + vert.y, + vert.z, + ]), + covariance=np.asarray([ + vert.var_x, + vert.cov_xy, + vert.cov_xz, + vert.var_y, + vert.cov_yz, + vert.var_z, + ]), + ) for vert in annotation.vertices + ], + class_id=ontology.class_id_to_contiguous_id[annotation.class_id], + instance_id=annotation.key, + color=ontology.colormap[annotation.class_id], + attributes=getattr(annotation, "attributes", {}), + ) for annotation in _annotation_pb2.annotations + ] + return cls(ontology=ontology, linelist=annotation_list) + + def to_proto(self): + """Return annotation as pb object. + + Returns + ------- + KeyLine3DAnnotations + Annotation as defined in `proto/annotations.proto` + """ + return ProtoProbabilisticKeyLine3DAnnotations( + annotations=[ + ProtoProbabilisticKeyLine3DAnnotation( + class_id=self._ontology.contiguous_id_to_class_id[line.class_id], + vertices=[ + ProtoProbabilisticKeyPoint3D( + x=point.x, + y=point.y, + z=point.z, + var_x=point.cov3.var_x, + cov_xy=point.cov3.cov_xy, + cov_xz=point.cov3.cov_xz, + var_y=point.cov3.var_y, + cov_yz=point.cov3.cov_yz, + var_z=point.cov3.var_z, + ) for point in line + ], + key=line.instance_id, + attributes=line.attributes, + ) for line in self._linelist + ] + ) + + def save(self, save_dir: str) -> None: + """Serialize Annotation object and saved to specified directory. + + Annotations are saved in format /. + + Parameters + ---------- + save_dir: str + Directory in which annotation is saved. + + Returns + ------- + output_annotation_file: str + Full path to saved annotation. + """ + return save_pbobject_as_json(self.to_proto(), save_path=save_dir) + + def __len__(self) -> int: + return len(self._linelist) + + def __getitem__(self, index: int) -> ProbabilisticKeyLine3D: + """Return a single 3D keyline""" + return self._linelist[index] + + def render(self): + """Batch rendering function for keylines.""" + raise NotImplementedError + + @property + def xyz(self) -> np.ndarray: + """Return lines as (N, 3) np.ndarray in format ([x, y, z])""" + return np.array([line.xyz.tolist() for line in self._linelist], dtype=np.float32) + + @property + def class_ids(self) -> np.ndarray: + """Return class ID for each line, with ontology applied: + class IDs mapped to a contiguous set. + """ + return np.array([line.class_id for line in self._linelist], dtype=np.int64) + + @property + def attributes(self) -> List[Dict[str, Any]]: + """Return a list of dictionaries of attribute name to value.""" + return [line.attributes for line in self._linelist] + + @property + def instance_ids(self) -> np.ndarray: + return np.array([line.instance_id for line in self._linelist], dtype=np.int64) + + @property + def hexdigest(self) -> str: + """Reproducible hash of annotation.""" + return generate_uid_from_pbobject(self.to_proto()) diff --git a/dgp/proto/annotations.proto b/dgp/proto/annotations.proto index 3a3dcca5..695073fd 100644 --- a/dgp/proto/annotations.proto +++ b/dgp/proto/annotations.proto @@ -252,6 +252,22 @@ message KeyPoint3D { float z = 3; } +// 3D point with uncertainty information. +message ProbabilisticKeyPoint3D { + // (x, y, z) point (in 3D Cartesian coordinates). + float x = 1; + float y = 2; + float z = 3; + + // The correlation terms in the 3x3 covariance matrix. + float var_x = 4; + float cov_xy = 5; + float cov_xz = 6; + float var_y = 7; + float cov_yz = 8; + float var_z = 9; +} + // 3D point annotation. message KeyPoint3DAnnotation { // Class identifier (should be in [0, num_classes - 1]), @@ -271,6 +287,26 @@ message KeyPoint3DAnnotation { string key = 4; } + +// 3D probabilistic point annotation. +message ProbabilisticKeyPoint3DAnnotation { + // Class identifier (should be in [0, num_classes - 1]), + // where num_classes is the total number of classes in your ontology. + uint32 class_id = 1; + + // 3D point. + ProbabilisticKeyPoint3D point = 2; + + // A map of attribute names to their values. + // Add only key/value pairs that are stored in a project document accessible + // to project contributors. + map attributes = 3; + + // An identifier key. Used to link with other annotations, which specify + // this key in their instance to link to corresponding ProbabilisticKeyPoint3DAnnotation. + string key = 4; +} + // 3D line annotation. message KeyLine3DAnnotation{ // Class identifier (should be in [0, num_classes - 1]), @@ -290,6 +326,25 @@ message KeyLine3DAnnotation{ string key = 4; } +// 3D probabilistic line annotation. +message ProbabilisticKeyLine3DAnnotation { + // Class identifier (should be in [0, num_classes - 1]), + // where num_classes is the total number of classes in your ontology. + uint32 class_id = 1; + + // 3D line. + repeated ProbabilisticKeyPoint3D vertices = 2; + + // A map of attribute names to their values. + // Add only key/value pairs that are stored in a project document accessible + // to project contributors. + map attributes = 3; + + // An identifier key. Used to link with other annotations, which specify + // this key in their instance to link to corresponding KeyLine3DAnnotation. + string key = 4; +} + message PolygonPoint3D { // (x, y, z) point (in 3D Cartesian coordinates). float x = 1; @@ -401,11 +456,21 @@ message KeyPoint3DAnnotations { repeated KeyPoint3DAnnotation annotations = 1; } +// List of ProbabilisticKeyPoint3DAnnotation. +message ProbabilisticKeyPoint3DAnnotations { + repeated ProbabilisticKeyPoint3DAnnotation annotations = 1; +} + // List of KeyLine3DAnnotation. message KeyLine3DAnnotations { repeated KeyLine3DAnnotation annotations = 1; } +// List of ProbabilisticKeyLine3DAnnotation. +message ProbabilisticKeyLine3DAnnotations { + repeated ProbabilisticKeyLine3DAnnotation annotations = 1; +} + // List of Polygon3DAnnotation. message Polygon3DAnnotations { repeated Polygon3DAnnotation annotations = 1; diff --git a/dgp/utils/math.py b/dgp/utils/math.py new file mode 100644 index 00000000..7db5749f --- /dev/null +++ b/dgp/utils/math.py @@ -0,0 +1,108 @@ +import numpy as np + +from dgp.utils.pose import Pose + + +class Covariance3D: + """3D covariance object. + + Parameters + ---------- + data: np.ndarray[np.float32] + Array of shape (6, ) or (3, 3). If 6-vector is used, + the order will be interpreted as [Var(X), Cov(X, Y), Cov(X, Z), Var(Y), COV(Y, Z), Var(Z)]. + + validate: bool, default: False + Validate if the input data satisfy the criteria of a covariance matrix. + It raises a ValueError if the validation fails. + + """ + def __init__(self, data: np.ndarray, validate: bool = False) -> None: + assert data.shape == (6, ) or data.shape == (3, 3) + if data.shape == (3, 3): + data = self._get_array(data) + self._data = data + if validate: + self._assert_symmetry(self.mat3) + self._assert_positive_definite(self.mat3) + + @staticmethod + def _assert_symmetry(mat: np.ndarray, rtol: float = 1e-5, atol: float = 1e-8) -> None: + if not np.allclose(mat, mat.T, rtol=rtol, atol=atol): + raise ValueError(f"{mat} is not symmetric!") + + @staticmethod + def _assert_positive_definite(mat: np.ndarray) -> None: + eigenvalues = np.linalg.eigvalsh(mat) + if not np.all(eigenvalues > 0.0): + raise ValueError(f"\n{mat} is not positive definite! Eigenvalues={eigenvalues}.") + + @staticmethod + def _get_mat(data: np.ndarray) -> np.ndarray: + assert data.shape == (6, ), f"data.shape={data.shape} != (6,)!" + var_x = data[0] + cov_xy = data[1] + cov_xz = data[2] + var_y = data[3] + cov_yz = data[4] + var_z = data[5] + + return np.array( + [ + [var_x, cov_xy, cov_xz], + [cov_xy, var_y, cov_yz], + [cov_xz, cov_yz, var_z], + ], + dtype=np.float64, + ) + + @staticmethod + def _get_array(data: np.ndarray) -> np.ndarray: + assert data.shape == (3, 3), f"data.shape={data.shape} != (3, 3)!" + return data[np.triu_indices(3)] + + @property + def arr6(self) -> np.ndarray: + return self._data + + @property + def var_x(self) -> float: + return self.arr6[0] + + @property + def cov_xy(self) -> float: + return self.arr6[1] + + @property + def cov_xz(self) -> float: + return self.arr6[2] + + @property + def var_y(self) -> float: + return self.arr6[3] + + @property + def cov_yz(self) -> float: + return self.arr6[4] + + @property + def var_z(self) -> float: + return self.arr6[5] + + @property + def mat3(self) -> np.ndarray: + return self._get_mat(self._data) + + def __repr__(self) -> str: + return f"{self.__class__.__name__}(data={self.arr6})" + + def tobytes(self) -> bytes: + return self.arr6.tobytes() + + def __mul__(self, pose: Pose) -> "Covariance3D": + R = pose.rotation_matrix + return Covariance3D(data=R.T @ self.mat3 @ R) + + def __rmul__(self, pose: Pose) -> "Covariance3D": + R = pose.rotation_matrix + return Covariance3D(data=R @ self.mat3 @ R.T) diff --git a/dgp/utils/pose.py b/dgp/utils/pose.py index 41086fcd..82ee5432 100644 --- a/dgp/utils/pose.py +++ b/dgp/utils/pose.py @@ -1,6 +1,5 @@ # Copyright 2021 Toyota Research Institute. All rights reserved. -"""General-purpose class for rigid-body transformations. -""" +"""General-purpose class for rigid-body transformations.""" import numpy as np from pyquaternion import Quaternion @@ -12,7 +11,7 @@ class Pose: and provides common transformations that are commonly seen in geometric problems. """ def __init__( - self, wxyz=np.float32([1., 0., 0., 0.]), tvec=np.float32([0., 0., 0.]), reference_coordinate_system="" + self, wxyz=np.float32([1.0, 0.0, 0.0, 0.0]), tvec=np.float32([0.0, 0.0, 0.0]), reference_coordinate_system="" ): """Initialize a Pose with Quaternion and 3D Position @@ -37,9 +36,9 @@ def __init__( self.tvec = tvec def __repr__(self): - formatter = {'float_kind': lambda x: '%.2f' % x} + formatter = {"float_kind": lambda x: "%.2f" % x} tvec_str = np.array2string(self.tvec, formatter=formatter) - return 'wxyz: {}, tvec: ({}) wrt. `{}`'.format(self.quat, tvec_str, self.reference_coordinate_system) + return "wxyz: {}, tvec: ({}) wrt. `{}`".format(self.quat, tvec_str, self.reference_coordinate_system) def copy(self): """Return a copy of this pose object. @@ -73,14 +72,14 @@ def __mul__(self, other): q = self.quat * other.quat return self.__class__(q, t) elif isinstance(other, np.ndarray): - assert other.shape[-1] == 3, 'Point cloud is not 3-dimensional' + assert other.shape[-1] == 3, "Point cloud is not 3-dimensional" X = np.hstack([other, np.ones((len(other), 1))]).T return (np.dot(self.matrix, X).T)[:, :3] else: return NotImplemented def __rmul__(self, other): - raise NotImplementedError('Right multiply not implemented yet!') + raise NotImplementedError("Right multiply not implemented yet!") def inverse(self, new_reference_coordinate_system=""): """Returns a new Pose that corresponds to the @@ -166,7 +165,7 @@ def from_matrix(cls, transformation_matrix, reference_coordinate_system=""): return cls( wxyz=Quaternion(matrix=transformation_matrix[:3, :3]), tvec=np.float32(transformation_matrix[:3, 3]), - reference_coordinate_system=reference_coordinate_system + reference_coordinate_system=reference_coordinate_system, ) @classmethod @@ -185,7 +184,7 @@ def from_rotation_translation(cls, rotation_matrix, tvec, reference_coordinate_s return cls( wxyz=Quaternion(matrix=rotation_matrix), tvec=np.float64(tvec), - reference_coordinate_system=reference_coordinate_system + reference_coordinate_system=reference_coordinate_system, ) @classmethod diff --git a/dgp/utils/structures/__init__.py b/dgp/utils/structures/__init__.py index 6dc7ee86..4415e345 100644 --- a/dgp/utils/structures/__init__.py +++ b/dgp/utils/structures/__init__.py @@ -3,4 +3,10 @@ from dgp.utils.structures.bounding_box_2d import BoundingBox2D from dgp.utils.structures.bounding_box_3d import BoundingBox3D from dgp.utils.structures.instance_mask import InstanceMask2D +from dgp.utils.structures.key_line_2d import KeyLine2D +from dgp.utils.structures.key_line_3d import KeyLine3D, ProbabilisticKeyLine3D from dgp.utils.structures.key_point_2d import KeyPoint2D +from dgp.utils.structures.key_point_3d import ( + KeyPoint3D, + ProbabilisticKeyPoint3D, +) diff --git a/dgp/utils/structures/key_line_3d.py b/dgp/utils/structures/key_line_3d.py index 787b626f..1f0c1611 100644 --- a/dgp/utils/structures/key_line_3d.py +++ b/dgp/utils/structures/key_line_3d.py @@ -1,9 +1,13 @@ -# Copyright 2022 Woven Planet. All rights reserved. +# Copyright 2022 Woven Planet. All rights reserved. import hashlib +from typing import Any, Dict, List, Optional, Tuple, Union import numpy as np import dgp.proto.annotations_pb2 as annotations_pb2 +from dgp.utils.math import Covariance3D +from dgp.utils.pose import Pose +from dgp.utils.structures.key_point_3d import ProbabilisticKeyPoint3D GENERIC_OBJECT_CLASS_ID = 1 @@ -29,7 +33,6 @@ class KeyLine3D: attributes: dict, default: None Dictionary of attributes associated with keyline. If None provided, defaults to empty dict. - """ def __init__(self, line, class_id=GENERIC_OBJECT_CLASS_ID, instance_id=None, color=(0, 0, 0), attributes=None): assert line.dtype in (np.float32, np.float64) @@ -98,3 +101,111 @@ def to_proto(self): annotations_pb2.KeyPoint3D(x=float(self.x[j]), y=float(self.y[j]), z=float(self.z[j])) for j, _ in enumerate(self.x) ] + + def __mul__(self, pose: Pose) -> "KeyLine3D": + return KeyLine3D( + line=pose * self.xyz.T, + class_id=self.class_id, + instance_id=self.instance_id, + attributes=self.attributes, + ) + + def __rmul__(self, pose: Pose) -> "KeyLine3D": + return self.__mul__(pose) + + +class ProbabilisticKeyLine3D: + """3D probabilistic key line object + + Parameters + ---------- + points: List[ProbabilisticKeyPoint3D] + List of probabilistic 3D key points. + + class_id: int, default: GENERIC_OBJECT_CLASS_ID + Integer class ID + + instance_id: int, default: None + Unique instance ID for keyline. If None provided, the ID is a hash of the keyline + location and class. + + color: tuple, default: (0, 0, 0) + RGB tuple for keyline color + + attributes: dict, default: None + Dictionary of attributes associated with keyline. If None provided, + defaults to empty dict. + """ + def __init__( + self, + points: List[ProbabilisticKeyPoint3D], + class_id: int = GENERIC_OBJECT_CLASS_ID, + instance_id: Optional[int] = None, + color: Tuple[int, int, int] = (0, 0, 0), + attributes: Optional[Dict[str, Any]] = None, + ): + self._points = points + self._class_id = class_id + self._instance_id = instance_id + self._color = color + self._attributes = dict(attributes) if attributes is not None else {} + + def __eq__(self, other: "ProbabilisticKeyLine3D") -> bool: + return self.hexdigest == other.hexdigest + + @property + def points(self) -> List[ProbabilisticKeyPoint3D]: + return self._points + + @property + def xyz(self) -> np.ndarray: + """Gets the xyz coordinates in column major""" + return np.array([point.xyz for point in self._points], dtype=np.float32).T + + @property + def cov3(self) -> List[Covariance3D]: + return [point.cov3 for point in self._points] + + @property + def __len__(self) -> int: + return len(self._points) + + def __getitem__(self, index: int) -> ProbabilisticKeyPoint3D: + return self._points[index] + + @property + def class_id(self): + return self._class_id + + @property + def instance_id(self): + if self._instance_id is None: + return self.hexdigest + return self._instance_id + + @property + def color(self): + return self._color + + @property + def attributes(self) -> Union[Dict[str, Any], None]: + return self._attributes + + @property + def hexdigest(self): + cov_bytes = np.asarray([point.cov3.arr6 for point in self.points]).tobytes() + return hashlib.md5( + self.xyz.tobytes() + cov_bytes + bytes(self._class_id) + str(self.attributes).encode("utf8") + ).hexdigest() + + def __mul__(self, pose: Pose) -> "ProbabilisticKeyLine3D": + return ProbabilisticKeyLine3D( + points=[pose * point for point in self.points], + class_id=self.class_id, + instance_id=self.instance_id, + color=self.color, + attributes=self.attributes, + ) + + def __rmul__(self, pose: Pose) -> "ProbabilisticKeyLine3D": + return self.__mul__(pose) diff --git a/dgp/utils/structures/key_point_3d.py b/dgp/utils/structures/key_point_3d.py index 3a03c5df..a40e97ef 100644 --- a/dgp/utils/structures/key_point_3d.py +++ b/dgp/utils/structures/key_point_3d.py @@ -1,9 +1,12 @@ # Copyright 2022 Woven Planet. All rights reserved. import hashlib +from typing import Any, Dict, Optional, Tuple, Union import numpy as np import dgp.proto.annotations_pb2 as annotations_pb2 +from dgp.utils.math import Covariance3D +from dgp.utils.pose import Pose GENERIC_OBJECT_CLASS_ID = 1 @@ -29,7 +32,6 @@ class KeyPoint3D: attributes: dict, default: None Dictionary of attributes associated with keypoint. If None provided, defaults to empty dict. - """ def __init__(self, point, class_id=GENERIC_OBJECT_CLASS_ID, instance_id=None, color=(0, 0, 0), attributes=None): assert point.dtype in (np.float32, np.float64) @@ -50,6 +52,11 @@ def __init__(self, point, class_id=GENERIC_OBJECT_CLASS_ID, instance_id=None, co def xyz(self): return np.array([self.x, self.y, self.z], dtype=np.float32) + @property + def xyzw(self): + """Homogeneous coordinates.""" + return np.array([self.x, self.y, self.z, 1.0], dtype=np.float32) + @property def class_id(self): return self._class_id @@ -92,3 +99,106 @@ def to_proto(self): As defined in `proto/annotations.proto` """ return annotations_pb2.KeyPoint3D(x=float(self.x), y=float(self.y), z=float(self.z)) + + +class ProbabilisticKeyPoint3D(KeyPoint3D): + """3D key point object. + + Parameters + ---------- + point: np.ndarray[np.float32] + Array of 3 floats describing key point coordinates [x, y, z]. + + covariance: np.ndarray[np.float32] | Covariance3D + Array of 6 floats of shape (6,) or 9 floats of shape (3, 3) or Covariance3D object, + describing the covariance matrix for this point. + + class_id: int, default: GENERIC_OBJECT_CLASS_ID + Integer class ID (0 reserved for background). + + instance_id: int, default: None + Unique instance ID for key point. If None provided, the ID is a hash of the key point + location and class. + + color: tuple, default: (0, 0, 0) + RGB tuple for key point color + + attributes: dict, default: None + Dictionary of attributes associated with key point. If None provided, + defaults to empty dict. + """ + def __init__( + self, + point: np.ndarray, + covariance: Union[np.ndarray, Covariance3D], + class_id: int = GENERIC_OBJECT_CLASS_ID, + instance_id: Optional[int] = None, + color: Tuple[int, int, int] = (0, 0, 0), + attributes: Optional[Dict[str, Any]] = None, + ): + KeyPoint3D.__init__( + self, + point=point, + class_id=class_id, + instance_id=instance_id, + color=color, + attributes=attributes, + ) + if isinstance(covariance, Covariance3D): + self._cov3 = covariance + else: + self._cov3 = Covariance3D(data=covariance) + + @property + def cov3(self) -> Covariance3D: + return self._cov3 + + @property + def hexdigest(self): + return hashlib.md5( + self.xyz.tobytes() + self.cov3.arr6.tobytes() + bytes(self._class_id) + str(self.attributes).encode("utf8") + ).hexdigest() + + def __mul__(self, pose: Pose) -> "ProbabilisticKeyPoint3D": + return ProbabilisticKeyPoint3D( + point=(self.xyzw @ pose.matrix.T)[:3], + covariance=pose * self.cov3, + class_id=self.class_id, + instance_id=self.instance_id, + attributes=self.attributes, + ) + + def __rmul__(self, pose: Pose) -> "ProbabilisticKeyPoint3D": + return ProbabilisticKeyPoint3D( + point=(pose.matrix @ self.xyzw)[:3], + covariance=self.cov3 * pose, + class_id=self.class_id, + instance_id=self.instance_id, + attributes=self.attributes, + ) + + def __repr__(self): + return f"{self.__class__.__name__}(xyz={self.xyz}, covariance={self._cov3}, class={self.class_id}, attributes={self.attributes})" + + def to_proto(self): + """Serialize keypoint to proto object. + + Does not serialize class or instance information, just point geometry. + To serialize a complete annotation, see `dgp/annotations/key_point_3d_annotation.py` + + Returns + ------- + ProbabilisticKeyPoint3D.pb2 + As defined in `proto/annotations.proto` + """ + return annotations_pb2.ProbabilisticKeyPoint3D( + x=float(self.x), + y=float(self.y), + z=float(self.z), + var_x=float(self.cov3.var_x), + cov_xy=float(self.cov3.cov_xy), + cov_xz=float(self.cov3.cov_xz), + var_y=float(self.cov3.var_y), + cov_yz=float(self.cov3.cov_yz), + var_z=float(self.cov3.var_z), + ) diff --git a/dgp/utils/visualization_utils.py b/dgp/utils/visualization_utils.py index 6a9806b3..0c038627 100644 --- a/dgp/utils/visualization_utils.py +++ b/dgp/utils/visualization_utils.py @@ -1,6 +1,8 @@ # Copyright 2021 Toyota Research Institute. All rights reserved. """Visualization tools for a variety of tasks""" import logging +import math +from typing import List, Optional, Tuple import cv2 import numpy as np @@ -16,6 +18,7 @@ YELLOW, get_unique_colors, ) +from dgp.utils.math import Covariance3D from dgp.utils.pose import Pose # Time to wait before key press in debug visualizations @@ -26,7 +29,7 @@ LOG.setLevel(logging.INFO) -def make_caption(dataset, idx, prefix=''): +def make_caption(dataset, idx, prefix=""): """Make caption that tells scene directory and sample index. Parameters @@ -69,7 +72,7 @@ def print_status(image, text): status_ymin = H - 40 text_offset = int(5 * 1) cv2.rectangle(image, (0, status_ymin), (status_xmax, H), DARKGRAY, thickness=-1) - cv2.putText(image, '%s' % text, (text_offset, H - text_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.8, WHITE, thickness=1) + cv2.putText(image, "%s" % text, (text_offset, H - text_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.8, WHITE, thickness=1) return image @@ -97,7 +100,7 @@ def mosaic(items, scale=1.0, pad=3, grid_width=None): """ # Determine tile width and height N = len(items) - assert N > 0, 'No items to mosaic!' + assert N > 0, "No items to mosaic!" grid_width = grid_width if grid_width else np.ceil(np.sqrt(N)).astype(int) grid_height = np.ceil(N * 1.0 / grid_width).astype(int) input_size = items[0].shape[:2] @@ -116,8 +119,7 @@ def mosaic(items, scale=1.0, pad=3, grid_width=None): im_pad = lambda im: cv2.copyMakeBorder(im, pad, pad, pad, pad, cv2.BORDER_CONSTANT, 0) mosaic_items = [im_pad(im) for im in mosaic_items] hstack = [np.hstack(mosaic_items[j:j + grid_width]) for j in range(0, len(mosaic_items), grid_width)] - mosaic_viz = np.vstack(hstack) if len(hstack) > 1 \ - else hstack[0] + mosaic_viz = np.vstack(hstack) if len(hstack) > 1 else hstack[0] return mosaic_viz @@ -150,14 +152,17 @@ def render_bbox2d_on_image(img, bboxes2d, instance_masks=None, colors=None, text Image with rendered bounding boxes. """ boxes = [ - np.int32([[bbox2d[0], bbox2d[1]], [bbox2d[0] + bbox2d[2], bbox2d[1]], - [bbox2d[0] + bbox2d[2], bbox2d[1] + bbox2d[3]], [bbox2d[0], bbox2d[1] + bbox2d[3]]]) - for bbox2d in bboxes2d + np.int32([ + [bbox2d[0], bbox2d[1]], + [bbox2d[0] + bbox2d[2], bbox2d[1]], + [bbox2d[0] + bbox2d[2], bbox2d[1] + bbox2d[3]], + [bbox2d[0], bbox2d[1] + bbox2d[3]], + ]) for bbox2d in bboxes2d ] if colors is None: cv2.polylines(img, boxes, True, RED, thickness=line_thickness) else: - assert len(boxes) == len(colors), 'len(boxes) != len(colors)' + assert len(boxes) == len(colors), "len(boxes) != len(colors)" for idx, box in enumerate(boxes): cv2.polylines(img, [box], True, colors[idx], thickness=line_thickness) @@ -175,7 +180,7 @@ def render_bbox2d_on_image(img, bboxes2d, instance_masks=None, colors=None, text # Add texts if texts: - assert len(boxes) == len(texts), 'len(boxes) != len(texts)' + assert len(boxes) == len(texts), "len(boxes) != len(texts)" for idx, box in enumerate(boxes): cv2.putText(img, texts[idx], tuple(box[0]), cv2.FONT_HERSHEY_SIMPLEX, 1, WHITE, 2, cv2.LINE_AA) return img @@ -212,7 +217,7 @@ def visualize_bounding_box_2d(image, bounding_box_2d, ontology, debug=False): class_names = [id_to_name[annotation.class_id] for annotation in bounding_box_2d.annotations] viz = render_bbox2d_on_image(np.copy(image), bboxes2d, colors=colors, texts=class_names) if debug: - cv2.imshow('image', viz) + cv2.imshow("image", viz) cv2.waitKey(DEBUG_WAIT_TIME) return viz @@ -251,9 +256,9 @@ def render_pointcloud_on_image(img, camera, Xw, cmap=MPL_JET_CMAP, norm_depth=10 uv = Camera(K=camera.K).project(Xc) # Colorize the point cloud based on depth z_c = Xc[:, 2] - zinv_c = 1. / (z_c + 1e-6) + zinv_c = 1.0 / (z_c + 1e-6) zinv_c *= norm_depth - colors = (cmap(np.clip(zinv_c, 0., 1.0))[:, :3] * 255).astype(np.uint8) + colors = (cmap(np.clip(zinv_c, 0.0, 1.0))[:, :3] * 255).astype(np.uint8) # Create an empty image to overlay H, W, _ = img.shape @@ -269,7 +274,7 @@ def render_pointcloud_on_image(img, camera, Xw, cmap=MPL_JET_CMAP, norm_depth=10 def render_radar_pointcloud_on_image( - img, camera, point_cloud, cmap=MPL_JET_CMAP, norm_depth=10, velocity=None, velocity_scale=1, velocity_max_pix=.05 + img, camera, point_cloud, cmap=MPL_JET_CMAP, norm_depth=10, velocity=None, velocity_scale=1, velocity_max_pix=0.05 ): """Render radar pointcloud on image. @@ -312,9 +317,9 @@ def render_radar_pointcloud_on_image( uv = Camera(K=camera.K).project(Xc) # Colorize the point cloud based on depth z_c = Xc[:, 2] - zinv_c = 1. / (z_c + 1e-6) + zinv_c = 1.0 / (z_c + 1e-6) zinv_c *= norm_depth - colors = (cmap(np.clip(zinv_c, 0., 1.0))[:, :3] * 255) + colors = cmap(np.clip(zinv_c, 0.0, 1.0))[:, :3] * 255 # Create an empty image to overlay H, W, _ = img.shape @@ -430,26 +435,34 @@ def __init__( self._center_pixel = ( int((metric_width * pixels_per_meter) // 2 - pixels_per_meter * center_offset_w), - int((metric_height * pixels_per_meter) // 2 - pixels_per_meter * center_offset_h) + int((metric_height * pixels_per_meter) // 2 - pixels_per_meter * center_offset_h), ) self.reset() def __repr__(self): - return 'width: {}, height: {}, data: {}'.format(self._metric_width, self._metric_height, type(self.data)) + return "width: {}, height: {}, data: {}".format(self._metric_width, self._metric_height, type(self.data)) def reset(self): - """Reset the canvas to a blank image with guideline circles of various radii. - """ - self.data = np.ones( - (int(self._metric_height * self._pixels_per_meter), int(self._metric_width * self._pixels_per_meter), 3), - dtype=np.uint8 - ) * self._bg_clr + """Reset the canvas to a blank image with guideline circles of various radii.""" + self.data = ( + np.ones( + ( + int(self._metric_height * self._pixels_per_meter), + int(self._metric_width * self._pixels_per_meter), + 3, + ), + dtype=np.uint8, + ) * self._bg_clr + ) # Draw metric polar grid for i in range(1, int(max(self._metric_width, self._metric_height)) // self._polar_step_size_meters): cv2.circle( - self.data, self._center_pixel, int(i * self._polar_step_size_meters * self._pixels_per_meter), - (50, 50, 50), 1 + self.data, + self._center_pixel, + int(i * self._polar_step_size_meters * self._pixels_per_meter), + (50, 50, 50), + 1, ) def render_point_cloud(self, point_cloud, extrinsics=Pose(), color=GRAY): @@ -473,8 +486,8 @@ def render_point_cloud(self, point_cloud, extrinsics=Pose(), color=GRAY): pointcloud_in_bev = combined_transform * point_cloud point_cloud2d = pointcloud_in_bev[:, :2] - point_cloud2d[:, 0] = (self._center_pixel[0] + point_cloud2d[:, 0] * self._pixels_per_meter) - point_cloud2d[:, 1] = (self._center_pixel[1] + point_cloud2d[:, 1] * self._pixels_per_meter) + point_cloud2d[:, 0] = self._center_pixel[0] + point_cloud2d[:, 0] * self._pixels_per_meter + point_cloud2d[:, 1] = self._center_pixel[1] + point_cloud2d[:, 1] * self._pixels_per_meter H, W = self.data.shape[:2] uv = point_cloud2d.astype(np.int32) @@ -487,7 +500,7 @@ def render_point_cloud(self, point_cloud, extrinsics=Pose(), color=GRAY): self.data[uv[:, 1], uv[:, 0], :] = color def render_radar_point_cloud( - self, point_cloud, extrinsics=Pose(), color=RED, velocity=None, velocity_scale=1, velocity_max_pix=.05 + self, point_cloud, extrinsics=Pose(), color=RED, velocity=None, velocity_scale=1, velocity_max_pix=0.05 ): """Render radar point cloud in BEV perspective. @@ -517,8 +530,8 @@ def render_radar_point_cloud( pointcloud_in_bev = combined_transform * point_cloud point_cloud2d = pointcloud_in_bev[:, :2] - point_cloud2d[:, 0] = (self._center_pixel[0] + point_cloud2d[:, 0] * self._pixels_per_meter) - point_cloud2d[:, 1] = (self._center_pixel[1] + point_cloud2d[:, 1] * self._pixels_per_meter) + point_cloud2d[:, 0] = self._center_pixel[0] + point_cloud2d[:, 0] * self._pixels_per_meter + point_cloud2d[:, 1] = self._center_pixel[1] + point_cloud2d[:, 1] * self._pixels_per_meter H, W = self.data.shape[:2] uv = point_cloud2d.astype(np.int32) @@ -543,8 +556,8 @@ def clip_norm(v, x): tail = point_cloud + velocity_scale * velocity pointcloud_in_bev_tail = combined_transform * tail point_cloud2d_tail = pointcloud_in_bev_tail[:, :2] - point_cloud2d_tail[:, 0] = (self._center_pixel[0] + point_cloud2d_tail[:, 0] * self._pixels_per_meter) - point_cloud2d_tail[:, 1] = (self._center_pixel[1] + point_cloud2d_tail[:, 1] * self._pixels_per_meter) + point_cloud2d_tail[:, 0] = self._center_pixel[0] + point_cloud2d_tail[:, 0] * self._pixels_per_meter + point_cloud2d_tail[:, 1] = self._center_pixel[1] + point_cloud2d_tail[:, 1] * self._pixels_per_meter uv_tail = point_cloud2d_tail.astype(np.int32) uv_tail = uv_tail[in_view] for row, row_tail in zip(uv, uv_tail): @@ -563,13 +576,24 @@ def clip_norm(v, x): color = (255, 110, 199) cv2.arrowedLine(self.data, (cx, cy), (cx2, cy2), color, thickness=1, line_type=cv2.LINE_AA) - def render_paths(self, paths, extrinsics=Pose(), colors=(GREEN, ), line_thickness=1, tint=1.0): + def render_paths( + self, + paths: List[List[Pose]], + covariances: Optional[List[Optional[List[Covariance3D]]]] = None, + extrinsics: Pose = Pose(), + colors: Optional[List[Tuple[int, int, int]]] = None, + line_thickness: int = 1, + tint: float = 1.0, + ) -> None: """Render object paths on bev. Parameters ---------- paths: list[list[Pose]] - List of object poses in the coordinate frame of the current timestep. + List of object poses in the coordinate frame of the current time step. + + covariances: list[list[CovarianceMatrix3D]], optional + List of 3D covariance objects for each path point. extrinsics: Pose, optional The pose of the pointcloud sensor wrt the body frame (Sensor frame -> (Vehicle) Body frame). @@ -582,9 +606,9 @@ def render_paths(self, paths, extrinsics=Pose(), colors=(GREEN, ), line_thicknes Thickness of lines. Default: 1. tint: float, optional - Mulitiplicative factor applied to color used to darken lines. Default: 1.0. + Multiplicative factor applied to color used to darken lines. Default: 1.0. """ - + colors = [GREEN] if colors is None else colors if len(colors) == 1: colors = list(colors) * len(paths) @@ -593,16 +617,47 @@ def render_paths(self, paths, extrinsics=Pose(), colors=(GREEN, ), line_thicknes combined_transform = self._bev_rotation * extrinsics - for path, color in zip(paths, colors): + yaw, _, _ = combined_transform.quat.yaw_pitch_roll # In [-pi, pi] + yaw_angle = np.rad2deg(yaw) + + if covariances is None: + covariances = [None] * len(paths) + + for path, covs, color in zip(paths, covariances, colors): # path should contain a list of Pose objects or None types. None types will be skipped. # TODO: add option to interpolate skipped poses. path3d = [combined_transform * pose.tvec.reshape(1, 3) for pose in path if pose is not None] - path2d = np.round(self._pixels_per_meter * np.stack(path3d, 0)[..., :2], - 0).astype(np.int32).reshape(1, -1, 2) + + path2d = ( + np.round(self._pixels_per_meter * np.stack(path3d, 0)[..., :2], 0).astype(np.int32).reshape(1, -1, 2) + ) + offset = np.array(self._center_pixel).reshape(1, 1, 2) # pylint: disable=E1121 path2d = path2d + offset # TODO: if we group the paths by color we can draw all paths with the same color at once - cv2.polylines(self.data, path2d, 0, color, line_thickness, cv2.LINE_AA) + cv2.polylines( + self.data, + path2d, + 0, + color, + line_thickness, + cv2.LINE_AA, + ) + + if covs is not None: + for point, cov in zip(path2d[0], covs): + x_scale = np.round(cov.var_x * self._pixels_per_meter).astype(int) + y_scale = np.round(cov.var_y * self._pixels_per_meter).astype(int) + cv2.ellipse( + self.data, + center=point, + axes=(x_scale, y_scale), + angle=yaw_angle, + startAngle=0.0, + endAngle=360.0, + color=(0, 255, 0), + thickness=line_thickness, + ) def render_bounding_box_3d( self, @@ -616,7 +671,7 @@ def render_bounding_box_3d( font_scale=0.5, font_colors=(WHITE, ), markers=None, - marker_scale=.5, + marker_scale=0.5, marker_colors=(RED, ), ): """Render bounding box 3d in BEV perspective. @@ -652,7 +707,7 @@ def render_bounding_box_3d( Color used for text labels. Default: (WHITE, ). markers: List[int], optional - List of opencv markers to draw in bottom right corner of cuboid. Should be one of: + List of opencv markers to draw in bottom right corner of cuboid. Should be one of: cv2.MARKER_CROSS, cv2.MARKER_DIAMOND, cv2.MARKER_SQUARE, cv2.MARKER_STAR, cv2.MARKER_TILTED_CROSS, cv2.MARKER_TRIANGLE_DOWN, cv2.MARKER_TRIANGLE_UP, or None. Default: None. @@ -687,8 +742,8 @@ def render_bounding_box_3d( corners2d = corners_in_bev[[0, 1, 5, 4], :2] # top surface of cuboid # Compute the center and offset of the corners - corners2d[:, 0] = (self._center_pixel[0] + corners2d[:, 0] * self._pixels_per_meter) - corners2d[:, 1] = (self._center_pixel[1] + corners2d[:, 1] * self._pixels_per_meter) + corners2d[:, 0] = self._center_pixel[0] + corners2d[:, 0] * self._pixels_per_meter + corners2d[:, 1] = self._center_pixel[1] + corners2d[:, 1] * self._pixels_per_meter center = np.mean(corners2d, axis=0).astype(np.int32) corners2d = corners2d.astype(np.int32) @@ -701,18 +756,29 @@ def render_bounding_box_3d( # Draw white light connecting center and font side. cv2.arrowedLine( - self.data, tuple(center), ( + self.data, + tuple(center), + ( (corners2d[0][0] + corners2d[1][0]) // 2, (corners2d[0][1] + corners2d[1][1]) // 2, - ), WHITE, 1, cv2.LINE_AA + ), + WHITE, + 1, + cv2.LINE_AA, ) if texts: if texts[bidx] is not None: top_left = np.argmin(np.linalg.norm(corners2d, axis=1)) cv2.putText( - self.data, texts[bidx], tuple(corners2d[top_left]), cv2.FONT_HERSHEY_SIMPLEX, font_scale, - font_colors[bidx], line_thickness // 2, cv2.LINE_AA + self.data, + texts[bidx], + tuple(corners2d[top_left]), + cv2.FONT_HERSHEY_SIMPLEX, + font_scale, + font_colors[bidx], + line_thickness // 2, + cv2.LINE_AA, ) if markers: @@ -720,13 +786,23 @@ def render_bounding_box_3d( bottom_right = np.argmax(np.linalg.norm(corners2d, axis=1)) assert markers[bidx] in [ - cv2.MARKER_CROSS, cv2.MARKER_DIAMOND, cv2.MARKER_SQUARE, cv2.MARKER_STAR, - cv2.MARKER_TILTED_CROSS, cv2.MARKER_TRIANGLE_DOWN, cv2.MARKER_TRIANGLE_UP + cv2.MARKER_CROSS, + cv2.MARKER_DIAMOND, + cv2.MARKER_SQUARE, + cv2.MARKER_STAR, + cv2.MARKER_TILTED_CROSS, + cv2.MARKER_TRIANGLE_DOWN, + cv2.MARKER_TRIANGLE_UP, ] cv2.drawMarker( - self.data, tuple(corners2d[bottom_right]), marker_colors[bidx], markers[bidx], - int(20 * marker_scale), 2, cv2.LINE_AA + self.data, + tuple(corners2d[bottom_right]), + marker_colors[bidx], + markers[bidx], + int(20 * marker_scale), + 2, + cv2.LINE_AA, ) def render_camera_frustrum(self, intrinsics, extrinsics, width, color=YELLOW, line_thickness=1): @@ -763,10 +839,10 @@ def render_camera_frustrum(self, intrinsics, extrinsics, width, color=YELLOW, li # Compute the center and offset of the corners frustrum_in_bev = frustrum_in_bev[:, :2] - frustrum_in_bev[:, 0] = (self._center_pixel[0] + frustrum_in_bev[:, 0] * self._pixels_per_meter) - frustrum_in_bev[:, 1] = (self._center_pixel[1] + frustrum_in_bev[:, 1] * self._pixels_per_meter) + frustrum_in_bev[:, 0] = self._center_pixel[0] + frustrum_in_bev[:, 0] * self._pixels_per_meter + frustrum_in_bev[:, 1] = self._center_pixel[1] + frustrum_in_bev[:, 1] * self._pixels_per_meter - frustrum_in_bev[1:] = (100 * (frustrum_in_bev[1:] - frustrum_in_bev[0]) + frustrum_in_bev[0]) + frustrum_in_bev[1:] = 100 * (frustrum_in_bev[1:] - frustrum_in_bev[0]) + frustrum_in_bev[0] frustrum_in_bev = frustrum_in_bev.astype(np.int32) cv2.line(self.data, tuple(frustrum_in_bev[0]), tuple(frustrum_in_bev[1]), color, line_thickness) @@ -852,7 +928,7 @@ def visualize_semantic_segmentation_2d( colored_semseg = (alpha * image + (1 - alpha) * colored_semseg).astype(np.uint8) if debug: - cv2.imshow('image', colored_semseg) + cv2.imshow("image", colored_semseg) cv2.waitKey(DEBUG_WAIT_TIME) return colored_semseg @@ -878,7 +954,7 @@ def visualize_bev( instance_colormap=None, cuboid_caption_fn=None, marker_fn=None, - marker_scale=.5, + marker_scale=0.5, show_paths_on_bev=False, bev_center_offset_w=0, bev_center_offset_h=0, @@ -959,7 +1035,7 @@ def visualize_bev( bev_left, bev_background_clr, center_offset_w=bev_center_offset_w, - center_offset_h=bev_center_offset_h + center_offset_h=bev_center_offset_h, ) # 1. Render pointcloud @@ -968,29 +1044,29 @@ def visualize_bev( else: pc_colors = [GRAY] for lidar_datum, clr in zip(lidar_datums, pc_colors): - bev.render_point_cloud(lidar_datum['point_cloud'], lidar_datum['extrinsics'], color=clr) + bev.render_point_cloud(lidar_datum["point_cloud"], lidar_datum["extrinsics"], color=clr) # 2. Render radars if radar_datums is not None: for radar_datum in radar_datums: bev.render_radar_point_cloud( - radar_datum['point_cloud'], radar_datum['extrinsics'], velocity=radar_datum['velocity'] + radar_datum["point_cloud"], radar_datum["extrinsics"], velocity=radar_datum["velocity"] ) # 3. Render 3D bboxes. for lidar_datum in lidar_datums: - if 'bounding_box_3d' in lidar_datum: + if "bounding_box_3d" in lidar_datum: - if len(lidar_datum['bounding_box_3d']) == 0: + if len(lidar_datum["bounding_box_3d"]) == 0: continue if instance_colormap is not None: colors = [ instance_colormap.get(bbox.instance_id, class_colormap[bbox.class_id]) - for bbox in lidar_datum['bounding_box_3d'] + for bbox in lidar_datum["bounding_box_3d"] ] else: - colors = [class_colormap[bbox.class_id] for bbox in lidar_datum['bounding_box_3d']] + colors = [class_colormap[bbox.class_id] for bbox in lidar_datum["bounding_box_3d"]] # If no caption function is supplied, generate one from the instance ids or class ids # Caption functions should return a tuple (string, color) @@ -1000,15 +1076,15 @@ def visualize_bev( elif cuboid_caption_fn is None: # show class names cuboid_caption_fn = lambda x: (id_to_name[x.class_id], WHITE) - labels, font_colors = zip(*[cuboid_caption_fn(bbox3d) for bbox3d in lidar_datum['bounding_box_3d']]) + labels, font_colors = zip(*[cuboid_caption_fn(bbox3d) for bbox3d in lidar_datum["bounding_box_3d"]]) markers, marker_colors = None, (RED, ) if marker_fn is not None: - markers, marker_colors = zip(*[marker_fn(bbox3d) for bbox3d in lidar_datum['bounding_box_3d']]) + markers, marker_colors = zip(*[marker_fn(bbox3d) for bbox3d in lidar_datum["bounding_box_3d"]]) bev.render_bounding_box_3d( - lidar_datum['bounding_box_3d'], - lidar_datum['extrinsics'], + lidar_datum["bounding_box_3d"], + lidar_datum["extrinsics"], colors=colors, texts=labels if bev_font_scale > 0 else None, line_thickness=bev_line_thickness, @@ -1022,22 +1098,28 @@ def visualize_bev( if show_paths_on_bev: # Collect the paths and path colors paths, path_colors = zip( - *[(bbox.attributes['path'], c) - for bbox, c in zip(lidar_datum['bounding_box_3d'], colors) - if 'path' in bbox.attributes] + *[(bbox.attributes["path"], c) + for bbox, c in zip(lidar_datum["bounding_box_3d"], colors) + if "path" in bbox.attributes] ) if len(paths) > 0: - bev.render_paths(paths, extrinsics=lidar_datum['extrinsics'], colors=path_colors, line_thickness=1) + bev.render_paths( + paths, + covariances=None, + extrinsics=lidar_datum["extrinsics"], + colors=path_colors, + line_thickness=1, + ) # 4. Render camera frustrums. if camera_datums is not None: for cam_datum, cam_color in zip(camera_datums, camera_colors): bev.render_camera_frustrum( - cam_datum['intrinsics'], - cam_datum['extrinsics'], - cam_datum['rgb'].size[0], + cam_datum["intrinsics"], + cam_datum["extrinsics"], + cam_datum["rgb"].size[0], color=cam_color, - line_thickness=bev_line_thickness // 2 + line_thickness=bev_line_thickness // 2, ) return bev.data @@ -1087,41 +1169,41 @@ def visualize_cameras( """ rgb_viz = [] for cam_datum in camera_datums: - rgb = np.array(cam_datum['rgb']).copy() + rgb = np.array(cam_datum["rgb"]).copy() if lidar_datums is not None: # 1. Render pointcloud for lidar_datum in lidar_datums: - p_LC = cam_datum['extrinsics'].inverse() * lidar_datum['extrinsics'] # lidar -> body -> camera + p_LC = cam_datum["extrinsics"].inverse() * lidar_datum["extrinsics"] # lidar -> body -> camera rgb = render_pointcloud_on_image( rgb, - Camera(K=cam_datum['intrinsics'], p_cw=p_LC), - lidar_datum['point_cloud'], + Camera(K=cam_datum["intrinsics"], p_cw=p_LC), + lidar_datum["point_cloud"], cmap=pc_rgb_cmap, norm_depth=pc_rgb_norm_depth, - dilation=pc_rgb_dilation + dilation=pc_rgb_dilation, ) if radar_datums is not None: # 2. Render radar pointcloud for radar_datum in radar_datums: - p_LC = cam_datum['extrinsics'].inverse() * radar_datum['extrinsics'] # radar -> body -> camera + p_LC = cam_datum["extrinsics"].inverse() * radar_datum["extrinsics"] # radar -> body -> camera rgb = render_radar_pointcloud_on_image( rgb, - Camera(K=cam_datum['intrinsics'], p_cw=p_LC), - radar_datum['point_cloud'], + Camera(K=cam_datum["intrinsics"], p_cw=p_LC), + radar_datum["point_cloud"], norm_depth=pc_rgb_norm_depth, - velocity=radar_datum['velocity'] + velocity=radar_datum["velocity"], ) # 3. Render 3D bboxes # for bbox3d, class_id in zip(cam_datum['bounding_box_3d'], cam_datum['class_ids']): - if 'bounding_box_3d' in cam_datum: - for bbox3d in cam_datum['bounding_box_3d']: + if "bounding_box_3d" in cam_datum: + for bbox3d in cam_datum["bounding_box_3d"]: class_name = id_to_name[bbox3d.class_id] rgb = bbox3d.render( rgb, - Camera(K=cam_datum['intrinsics']), + Camera(K=cam_datum["intrinsics"]), line_thickness=bbox3d_line_thickness, class_name=class_name, font_scale=bbox3d_font_scale, diff --git a/tests/annotation/test_key_line_3d_annotation.py b/tests/annotation/test_key_line_3d_annotation.py index 736dc0d2..674cce02 100644 --- a/tests/annotation/test_key_line_3d_annotation.py +++ b/tests/annotation/test_key_line_3d_annotation.py @@ -3,7 +3,10 @@ import numpy as np import pytest -from dgp.annotations.key_line_3d_annotation import KeyLine3DAnnotationList +from dgp.annotations.key_line_3d_annotation import ( + KeyLine3DAnnotationList, + ProbabilisticKeyLine3DAnnotationList, +) from dgp.datasets.synchronized_dataset import SynchronizedSceneDataset from dgp.utils.structures.key_line_3d import KeyLine3D from tests import TEST_DATA_DIR @@ -55,6 +58,16 @@ def test_kl3d_proto(kl_ontology): assert output_proto.__sizeof__() in {64, 80} +def test_prob_kl3d_proto(kl_ontology): + DGP_TEST_DATASET_DIR = os.path.join(TEST_DATA_DIR, "dgp") + scenes_dataset_json = os.path.join( + DGP_TEST_DATASET_DIR, "key_line_3d/scene_000000/key_line_3d/lcm_25tm/probabilistic_key_line_annotation.json" + ) + kl3d_list = ProbabilisticKeyLine3DAnnotationList.load(scenes_dataset_json, kl_ontology) + output_proto = kl3d_list.to_proto() + assert output_proto.__sizeof__() == 64 + + def test_kl3d_save(kl_ontology): DGP_TEST_DATASET_DIR = os.path.join(TEST_DATA_DIR, "dgp") scenes_dataset_json = os.path.join( diff --git a/tests/data/dgp/key_line_3d/scene_000000/key_line_3d/lcm_25tm/probabilistic_key_line_annotation.json b/tests/data/dgp/key_line_3d/scene_000000/key_line_3d/lcm_25tm/probabilistic_key_line_annotation.json new file mode 100644 index 00000000..0e89d189 --- /dev/null +++ b/tests/data/dgp/key_line_3d/scene_000000/key_line_3d/lcm_25tm/probabilistic_key_line_annotation.json @@ -0,0 +1,1783 @@ +{ + "annotations": [ + { + "vertices": [ + { + "x": -2.089055, + "y": 1.6612228, + "z": 5.5502887, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.144171, + "y": 1.9179286, + "z": 9.2943735, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.211071, + "y": 2.1553297, + "z": 12.736838, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.2840962, + "y": 2.3885832, + "z": 16.158052, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.3597474, + "y": 2.6154032, + "z": 19.573105, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.4369226, + "y": 2.8339982, + "z": 22.989477, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.5164948, + "y": 3.0463562, + "z": 26.414253, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.6024356, + "y": 3.2565043, + "z": 29.861938, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.702379, + "y": 3.4674394, + "z": 33.34829, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.810339, + "y": 3.6844406, + "z": 36.85309, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.923361, + "y": 3.9080756, + "z": 40.382607, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.0466492, + "y": 4.1394486, + "z": 43.973923, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.1899557, + "y": 4.381072, + "z": 47.69494, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.3420885, + "y": 4.6252284, + "z": 51.463062, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.4969435, + "y": 4.8672733, + "z": 55.24915, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.6537743, + "y": 5.103134, + "z": 59.045055, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.811098, + "y": 5.3314614, + "z": 62.84453, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.9694698, + "y": 5.559306, + "z": 66.63612, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.1311097, + "y": 5.7895617, + "z": 70.43898, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.2989254, + "y": 6.020749, + "z": 74.22436, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.48259, + "y": 6.2473016, + "z": 77.96835, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.6718235, + "y": 6.4677234, + "z": 81.68087, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.8680606, + "y": 6.690045, + "z": 85.47157, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.0575347, + "y": 6.9028425, + "z": 89.10022, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.251365, + "y": 7.1159596, + "z": 92.79421, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.447508, + "y": 7.326644, + "z": 96.472084, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.647868, + "y": 7.5375195, + "z": 100.107086, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.8591833, + "y": 7.744711, + "z": 103.62032, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.9136868, + "y": 7.7970524, + "z": 104.497826, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.073763, + "y": 7.9507794, + "z": 107.07507, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.2905993, + "y": 8.156952, + "z": 110.50898, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.508053, + "y": 8.362639, + "z": 113.93518, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.72815, + "y": 8.56981, + "z": 117.36675, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.9520125, + "y": 8.779772, + "z": 120.814575, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.18666, + "y": 8.992638, + "z": 124.311134, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.444804, + "y": 9.2088785, + "z": 127.90669, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.7122903, + "y": 9.422916, + "z": 131.52982, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.983929, + "y": 9.634734, + "z": 135.17487, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.259978, + "y": 9.845639, + "z": 138.83958, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.539335, + "y": 10.0573435, + "z": 142.50996, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.728814, + "y": 10.19964, + "z": 144.93065, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.829059, + "y": 10.274923, + "z": 146.21132, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -9.140108, + "y": 10.503232, + "z": 149.96384, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -9.460176, + "y": 10.741443, + "z": 153.72722, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -9.782717, + "y": 10.9853945, + "z": 157.47879, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -10.109095, + "y": 11.230266, + "z": 161.22397, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -10.4388075, + "y": 11.469406, + "z": 164.93329, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -10.774219, + "y": 11.694452, + "z": 168.5102, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -11.10969, + "y": 11.910816, + "z": 172.01965, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -11.451755, + "y": 12.129138, + "z": 175.56058, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -11.78558, + "y": 12.345325, + "z": 179.00099, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -12.123556, + "y": 12.56595, + "z": 182.45448, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -12.464241, + "y": 12.786173, + "z": 185.88411, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -12.810647, + "y": 13.001762, + "z": 189.22997, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -13.157112, + "y": 13.214485, + "z": 192.52365, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -13.508577, + "y": 13.428223, + "z": 195.83115, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -13.862669, + "y": 13.641442, + "z": 199.1452, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.228256, + "y": 13.856666, + "z": 202.51787, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.616015, + "y": 14.082532, + "z": 205.99913, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -15.01267, + "y": 14.316065, + "z": 209.51979, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -15.412209, + "y": 14.555406, + "z": 213.05066, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -15.811806, + "y": 14.798351, + "z": 216.57367, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -16.597677, + "y": 15.27746, + "z": 223.47319, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -16.965359, + "y": 15.495051, + "z": 226.66666, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -17.33191, + "y": 15.706276, + "z": 229.84041, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -17.714159, + "y": 15.927859, + "z": 233.13756, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + } + ], + "attributes": { + "radius": "30.501609241201393", + "id": "0", + "line_side": "left", + "line_type": "DASHED", + "line_id": "0" + }, + "class_id": 0, + "key": "" + }, + { + "vertices": [ + { + "x": 1.9111695, + "y": 1.6615186, + "z": 5.5492544, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.9067647, + "y": 1.6830662, + "z": 5.8708277, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.8551489, + "y": 1.9199955, + "z": 9.362753, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.7880803, + "y": 2.1561732, + "z": 12.818354, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.7150108, + "y": 2.3874795, + "z": 16.245478, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.6395504, + "y": 2.6132312, + "z": 19.656515, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.5619266, + "y": 2.8363657, + "z": 23.08165, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.4821188, + "y": 3.0551112, + "z": 26.509645, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.3957512, + "y": 3.2708907, + "z": 29.967133, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.2954808, + "y": 3.4841976, + "z": 33.464348, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.1877551, + "y": 3.6968973, + "z": 36.969093, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 1.0745144, + "y": 3.9153097, + "z": 40.51347, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.9509736, + "y": 4.1401086, + "z": 44.119183, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.8073877, + "y": 4.3745847, + "z": 47.854336, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.6577025, + "y": 4.612326, + "z": 51.629, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.5030512, + "y": 4.851601, + "z": 55.425587, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.34778368, + "y": 5.0891433, + "z": 59.21479, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.19249244, + "y": 5.323003, + "z": 63.014713, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": 0.036289044, + "y": 5.5568743, + "z": 66.81471, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -0.12257662, + "y": 5.793621, + "z": 70.61382, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -0.28644034, + "y": 6.028242, + "z": 74.41185, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -0.46490127, + "y": 6.255222, + "z": 78.16932, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -0.65159535, + "y": 6.4777493, + "z": 81.93981, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -0.8376442, + "y": 6.6923103, + "z": 85.6074, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -0.9119874, + "y": 6.776422, + "z": 87.045006, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -1.0300293, + "y": 6.9099655, + "z": 89.32736, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -1.2239664, + "y": 7.1272187, + "z": 93.00762, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -1.4218007, + "y": 7.3441067, + "z": 96.68277, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -1.6249701, + "y": 7.5629306, + "z": 100.33665, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -1.83864, + "y": 7.7771273, + "z": 103.86255, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.0535915, + "y": 7.987728, + "z": 107.315025, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.26837, + "y": 8.196615, + "z": 110.75809, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.4802217, + "y": 8.4016075, + "z": 114.17983, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.69483, + "y": 8.605378, + "z": 117.62206, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -2.9162083, + "y": 8.809844, + "z": 121.076515, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.1530406, + "y": 9.017441, + "z": 124.58788, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.4182506, + "y": 9.228895, + "z": 128.19868, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.6958077, + "y": 9.439253, + "z": 131.8347, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -3.9776742, + "y": 9.649599, + "z": 135.49788, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.259713, + "y": 9.859129, + "z": 139.14267, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.5440197, + "y": 10.072251, + "z": 142.8032, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -4.8418183, + "y": 10.295004, + "z": 146.53111, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.154051, + "y": 10.528058, + "z": 150.29591, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.474724, + "y": 10.768479, + "z": 154.06502, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -5.798274, + "y": 11.011871, + "z": 157.82866, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.1251135, + "y": 11.253085, + "z": 161.5809, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.4551225, + "y": 11.487661, + "z": 165.29713, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -6.792296, + "y": 11.710512, + "z": 168.89513, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.128951, + "y": 11.927342, + "z": 172.41724, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.4666405, + "y": 12.144079, + "z": 175.91202, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -7.8034506, + "y": 12.362533, + "z": 179.38284, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.120223, + "y": 12.569137, + "z": 182.61943, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.143975, + "y": 12.584624, + "z": 182.86191, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.478287, + "y": 12.801822, + "z": 186.22679, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.832062, + "y": 13.024227, + "z": 189.64536, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -8.935206, + "y": 13.088279, + "z": 190.62552, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -9.181927, + "y": 13.241483, + "z": 192.96992, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -9.5317, + "y": 13.4567, + "z": 196.26012, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -9.886347, + "y": 13.672505, + "z": 199.57784, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -10.24753, + "y": 13.888017, + "z": 202.94589, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -10.632698, + "y": 14.111747, + "z": 206.44354, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -11.0278015, + "y": 14.34444, + "z": 209.96808, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -11.4284, + "y": 14.584588, + "z": 213.5003, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -11.83377, + "y": 14.830897, + "z": 217.04475, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -12.623721, + "y": 15.312908, + "z": 223.92596, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -12.991851, + "y": 15.532324, + "z": 227.12274, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -13.358001, + "y": 15.743514, + "z": 230.29294, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -13.741195, + "y": 15.96096, + "z": 233.60095, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + } + ], + "attributes": { + "line_side": "right", + "id": "1", + "line_id": "1", + "line_type": "SOLID", + "radius": "29.77469035588595" + }, + "class_id": 0, + "key": "" + }, + { + "vertices": [ + { + "x": -17.714926, + "y": 15.928299, + "z": 233.14418, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -18.069895, + "y": 16.169653, + "z": 236.14883, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -18.19339, + "y": 16.253994, + "z": 237.19315, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -18.399847, + "y": 16.395016, + "z": 238.93892, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -18.62715, + "y": 16.549133, + "z": 240.86249, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -18.780735, + "y": 16.64647, + "z": 242.15746, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -18.889278, + "y": 16.713177, + "z": 243.0872, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -19.002413, + "y": 16.779633, + "z": 244.04662, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -19.786108, + "y": 17.227652, + "z": 250.62985, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -20.006945, + "y": 17.3423, + "z": 252.33084, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -20.256626, + "y": 17.471125, + "z": 254.25897, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + } + ], + "attributes": { + "line_side": "left", + "line_type": "DASHED", + "id": "2", + "radius": "17.82871331054196", + "line_id": "0" + }, + "class_id": 0, + "key": "" + }, + { + "vertices": [ + { + "x": -13.741962, + "y": 15.961398, + "z": 233.60757, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.101324, + "y": 16.202288, + "z": 236.65076, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.42633, + "y": 16.422832, + "z": 239.39967, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.654303, + "y": 16.577078, + "z": 241.32912, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.6733465, + "y": 16.589153, + "z": 241.4897, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.807882, + "y": 16.674463, + "z": 242.62416, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -14.916425, + "y": 16.741295, + "z": 243.55391, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -15.028143, + "y": 16.80719, + "z": 244.50175, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -15.507923, + "y": 17.087702, + "z": 248.59627, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -15.816144, + "y": 17.263704, + "z": 251.14905, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -16.032705, + "y": 17.379366, + "z": 252.84259, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + }, + { + "x": -16.214134, + "y": 17.475554, + "z": 254.26033, + "var_x": 1.0, + "cov_xy": 0.1, + "cov_xz": 0.1, + "var_y": 1.0, + "cov_yz": 0.1, + "var_z": 1.0 + } + ], + "attributes": { + "line_type": "SOLID", + "line_side": "right", + "id": "3", + "radius": "20.3193714043737", + "line_id": "1" + }, + "class_id": 0, + "key": "" + } + ] +} diff --git a/tests/pose_fixtures.py b/tests/pose_fixtures.py new file mode 100644 index 00000000..33db5c66 --- /dev/null +++ b/tests/pose_fixtures.py @@ -0,0 +1,25 @@ +import numpy as np +import pytest +from pyquaternion import Quaternion + +from dgp.utils.pose import Pose + + +@pytest.fixture +def step_angle() -> float: + return 30.0 + + +@pytest.fixture +def dummy_pose(step_angle: float) -> Pose: + theta = np.radians(step_angle) + q = Quaternion._from_axis_angle(axis=np.float32([0.0, 0.0, 1.0]), angle=theta) + return Pose.from_rotation_translation( + rotation_matrix=q.rotation_matrix, + tvec=np.float32([1, 2, 3]), + ) + + +@pytest.fixture +def dummy_pose_inverse(dummy_pose: Pose) -> Pose: + return dummy_pose.inverse() diff --git a/tests/utils/structures/test_key_line_3d.py b/tests/utils/structures/test_key_line_3d.py index 29967f20..c6ed1195 100644 --- a/tests/utils/structures/test_key_line_3d.py +++ b/tests/utils/structures/test_key_line_3d.py @@ -1,34 +1,68 @@ import numpy as np import pytest -from dgp.utils.structures.key_line_3d import KeyLine3D +from dgp.utils.pose import Pose +from dgp.utils.structures.key_line_3d import KeyLine3D, ProbabilisticKeyLine3D +from dgp.utils.structures.key_point_3d import ProbabilisticKeyPoint3D + +from tests.pose_fixtures import dummy_pose, dummy_pose_inverse, step_angle # noqa: F401, isort: skip @pytest.fixture -def keyline(): +def key_line(): k = np.float32([[0.5, 2, -1], [-4, 0, 3], [0, -1, 2], [0.25, 1.25, -0.25], [100, 1, 200]]) return KeyLine3D(k) -def test_keyline_class_id(keyline): - assert keyline.class_id == 1 +def test_keyline_class_id(key_line): + assert key_line.class_id == 1 + + +def test_keyline_instance_id(key_line): + assert key_line.instance_id == "6b144d77fb6c1f915f56027b4fe34f5e" + + +def test_keyline_color(key_line): + assert key_line.color == (0, 0, 0) -def test_keyline_instance_id(keyline): - assert keyline.instance_id == "6b144d77fb6c1f915f56027b4fe34f5e" +def test_keyline_attributes(key_line): + assert key_line.attributes == {} -def test_keyline_color(keyline): - assert keyline.color == (0, 0, 0) +def test_keyline_hexdigest(key_line): + assert key_line.hexdigest == "6b144d77fb6c1f915f56027b4fe34f5e" + + +def test_keyline_to_proto(key_line): + assert len(key_line.to_proto()) == 5 + + +@pytest.fixture +def cov_data() -> np.ndarray: + return np.array([10, 2, 3, 10, 2, 10], dtype=np.float32) -def test_keyline_attributes(keyline): - assert keyline.attributes == {} +def test_key_line_3d(key_line: KeyLine3D, dummy_pose: Pose, dummy_pose_inverse: Pose): # noqa: F811 + transformed_key_line = dummy_pose * key_line + got_key_line = dummy_pose_inverse * transformed_key_line + np.testing.assert_allclose(key_line.xyz, got_key_line.xyz, rtol=1e-6, atol=1e-6) -def test_keyline_hexdigest(keyline): - assert keyline.hexdigest == "6b144d77fb6c1f915f56027b4fe34f5e" +def test_probabilistic_key_line_3d(cov_data: np.ndarray, dummy_pose: Pose, dummy_pose_inverse: Pose): # noqa: F811 + p1 = ProbabilisticKeyPoint3D(np.asarray([1, 1, 1], dtype=np.float32), covariance=cov_data) + p2 = ProbabilisticKeyPoint3D(np.asarray([2, 2, 2], dtype=np.float32), covariance=cov_data) + key_line_3d = ProbabilisticKeyLine3D(points=[p1, p2]) + np.testing.assert_equal(key_line_3d.cov3[0], p1.cov3) + np.testing.assert_equal(key_line_3d.cov3[1], p2.cov3) + # NOTE: xyz is in column major. + np.testing.assert_equal(key_line_3d.xyz.T[0], p1.xyz) + np.testing.assert_equal(key_line_3d.xyz.T[1], p2.xyz) + np.testing.assert_equal(key_line_3d.hexdigest, "bd11fdd87cebea6bc362631de2288bee") + transformed_key_line_3d = dummy_pose * key_line_3d + got_key_line_3d = dummy_pose_inverse * transformed_key_line_3d -def test_keyline_to_proto(keyline): - assert len(keyline.to_proto()) == 5 + for point1, point2 in zip(key_line_3d, got_key_line_3d): + np.testing.assert_almost_equal(point1.xyz, point2.xyz) + np.testing.assert_almost_equal(point1.cov3.arr6, point2.cov3.arr6) diff --git a/tests/utils/structures/test_key_point_3d.py b/tests/utils/structures/test_key_point_3d.py new file mode 100644 index 00000000..146cb889 --- /dev/null +++ b/tests/utils/structures/test_key_point_3d.py @@ -0,0 +1,53 @@ +import numpy as np +import pytest + +from dgp.utils.math import Covariance3D +from dgp.utils.pose import Pose +from dgp.utils.structures.key_point_3d import ProbabilisticKeyPoint3D +from tests.pose_fixtures import dummy_pose, step_angle # noqa: F401 + + +@pytest.fixture +def cov_data() -> np.ndarray: + return np.array([10, 2, 3, 10, 2, 10], dtype=np.float32) + + +@pytest.fixture() +def key_point(cov_data: np.ndarray) -> ProbabilisticKeyPoint3D: + return ProbabilisticKeyPoint3D( + point=np.asarray([1, 2, 3], dtype=np.float32), + covariance=Covariance3D(cov_data), + ) + + +def test_cov3(cov_data): + cov3 = Covariance3D(cov_data) + np.testing.assert_allclose(cov3._get_array(cov3._get_mat(cov_data)), cov_data) + + +@pytest.fixture +def dummy_cov3d() -> Covariance3D: + return Covariance3D(data=np.float32([ + 10.0, + 0.0, + 0.0, + 5.0, + 0.0, + 2.0, + ])) + + +def test_probabilistic_key_point_3d( + key_point: ProbabilisticKeyPoint3D, + step_angle: float, # noqa: F811 + dummy_pose: Pose, # noqa: F811 +) -> None: + np.testing.assert_equal(key_point.hexdigest, "be2da15917b1820742ef5b67b4e38d74") + + rotated_point1 = key_point + rotated_point2 = key_point + num_rotations = int(round(360.0 / step_angle)) + for _ in range(num_rotations): + rotated_point1 = dummy_pose * rotated_point1 + rotated_point2 = rotated_point2 * dummy_pose + np.testing.assert_almost_equal(rotated_point1.xyz, rotated_point2.xyz) diff --git a/tests/utils/test_math.py b/tests/utils/test_math.py new file mode 100644 index 00000000..d68c2515 --- /dev/null +++ b/tests/utils/test_math.py @@ -0,0 +1,43 @@ +import numpy as np +import pytest + +from dgp.utils.math import Covariance3D +from dgp.utils.pose import Pose + +from tests.pose_fixtures import ( # noqa: F401, isort: skip + dummy_pose, dummy_pose_inverse, step_angle, +) + + +@pytest.fixture +def dummy_cov3d() -> Covariance3D: + return Covariance3D(data=np.float32([ + 10.0, + 0.0, + 0.0, + 5.0, + 0.0, + 2.0, + ])) + + +def test_covariance_rotation( + dummy_cov3d: Covariance3D, + dummy_pose: Pose, # noqa: F811 + dummy_pose_inverse: Pose, # noqa: F811 + step_angle: float, # noqa: F811 +): + curr_cov1 = dummy_cov3d + curr_cov2 = dummy_cov3d + # Adjusted due to numerical errors. + rtol = 1e-6 + atol = 1e-6 + num_rotations = int(round(360.0 / step_angle)) + for _ in range(num_rotations): + # Counter clockwise rotation. + curr_cov1 = dummy_pose * curr_cov1 + curr_cov2 = curr_cov2 * dummy_pose_inverse + # Clockwise rotation. + np.testing.assert_allclose(curr_cov1.mat3, curr_cov2.mat3, rtol=rtol, atol=atol) + np.testing.assert_allclose(curr_cov1.mat3, dummy_cov3d.mat3, rtol=rtol, atol=atol) + np.testing.assert_allclose(curr_cov2.mat3, dummy_cov3d.mat3, rtol=rtol, atol=atol)