Fix convexhull boundary computation

argoverse/data_loading/object_label_record.py

@@ -37,6 +37,12 @@
 
 
 class ObjectLabelRecord:
+    """Parameterizes an object via a 3d bounding box and the object's pose within the egovehicle's frame.
+
+    We refer to the object's pose as `egovehicle_SE3_object` and is parameterized by (R,t), where R is
+    a quaternion in scalar-first order.
+    """
+
     def __init__(
         self,
         quaternion: np.ndarray,
@@ -52,7 +58,7 @@ def __init__(
         """Create an ObjectLabelRecord.
 
         Args:
-           quaternion: Numpy vector representing quaternion, box/cuboid orientation
+           quaternion: Numpy vector representing quaternion (qw,qx,qy,qz), box/cuboid orientation
            translation: Numpy vector representing translation, center of box given as x, y, z.
            length: object length.
            width: object width.
@@ -72,7 +78,7 @@ def __init__(
         self.score = score
 
     def as_2d_bbox(self) -> np.ndarray:
-        """Construct a 2D bounding box from this label.
+        """Convert the object cuboid to a 2D bounding box, with vertices inside the egovehicle's frame.
 
         Length is x, width is y, and z is height
 
@@ -97,10 +103,7 @@ def as_2d_bbox(self) -> np.ndarray:
         return bbox_in_egovehicle_frame
 
     def as_3d_bbox(self) -> np.ndarray:
-        r"""Calculate the 8 bounding box corners.
-
-        Args:
-            None
+        r"""Calculate the 8 bounding box corners (returned as points inside the egovehicle's frame).
 
         Returns:
             Numpy array of shape (8,3)
@@ -147,7 +150,7 @@ def render_clip_frustum_cv2(
     ) -> np.ndarray:
         r"""We bring the 3D points into each camera, and do the clipping there.
 
-        Renders box using OpenCV2. Roughly based on
+        Renders box using OpenCV2. Edge coloring and vertex ordering is roughly based on
         https://github.com/nutonomy/nuscenes-devkit/blob/master/python-sdk/nuscenes_utils/data_classes.py
 
         ::

argoverse/evaluation/competition_util.py

@@ -10,14 +10,14 @@
 
 import h5py
 import numpy as np
-import quaternion
 from scipy.spatial import ConvexHull
 from shapely.geometry import Polygon
-from sklearn.cluster.dbscan_ import DBSCAN
+from sklearn.cluster import DBSCAN
 
 from argoverse.data_loading.argoverse_tracking_loader import ArgoverseTrackingLoader
 from argoverse.data_loading.object_label_record import ObjectLabelRecord
 from argoverse.utils.se3 import SE3
+from argoverse.utils.transform import yaw_to_quaternion3d
 
 TYPE_LIST = Union[List[np.ndarray], np.ndarray]
 
@@ -101,7 +101,6 @@ def generate_tracking_zip(input_path: str, output_path: str, filename: str = "ar
         filename: to be used as the name of the file
 
     """
-
     if not os.path.exists(output_path):
         os.makedirs(output_path)
     dirpath = tempfile.mkdtemp()
@@ -122,11 +121,10 @@ def generate_tracking_zip(input_path: str, output_path: str, filename: str = "ar
 
 
 def get_polygon_from_points(points: np.ndarray) -> Polygon:
-    """
-    function to generate (convex hull) shapely polygon from set of points
+    """Convert a 3d point set to a Shapely polygon representing its convex hull.
 
     Args:
-        points: list of 2d coordinate points
+        points: list of 3d coordinate points
 
     Returns:
         polygon: shapely polygon representing the results
@@ -136,13 +134,9 @@ def get_polygon_from_points(points: np.ndarray) -> Polygon:
 
     poly = []
 
-    for simplex in hull.simplices:
-        poly.append([points[simplex, 0][0], points[simplex, 1][0], points[simplex, 2][0]])
-        poly.append([points[simplex, 0][1], points[simplex, 1][1], points[simplex, 2][1]])
-
-        # plt.plot(points[simplex, 0], points[simplex, 1], 'k-')
-
-    return Polygon(poly)
+    # `simplices` contains indices of points forming the simplical facets of the convex hull.
+    poly_pts = hull.points[np.unique(hull.simplices)]
+    return Polygon(poly_pts)
 
 
 def get_rotated_bbox_from_points(points: np.ndarray) -> Polygon:
@@ -158,57 +152,52 @@ def get_rotated_bbox_from_points(points: np.ndarray) -> Polygon:
     return get_polygon_from_points(points).minimum_rotated_rectangle
 
 
-def unit_vector(pt0: Tuple[float, float], pt1: Tuple[float, float]) -> Tuple[float, float]:
-    # returns an unit vector that points in the direction of pt0 to pt1
-    dis_0_to_1 = math.sqrt((pt0[0] - pt1[0]) ** 2 + (pt0[1] - pt1[1]) ** 2)
-    return (pt1[0] - pt0[0]) / dis_0_to_1, (pt1[1] - pt0[1]) / dis_0_to_1
-
+def poly_to_label(poly: Polygon, category: str = "VEHICLE", track_id: str = "") -> ObjectLabelRecord:
+    """Convert a Shapely Polygon to a 3d cuboid by estimating the minimum-bounding rectangle.
 
-def dist(p1: Tuple[float, float], p2: Tuple[float, float]) -> float:
-    return math.sqrt(((p1[0] - p2[0]) ** 2) + ((p1[1] - p2[1]) ** 2))
+    Args:
+        poly: Shapely polygon object representing a convex hull of an object
+        category: object category to which object belongs, e.g. VEHICLE, PEDESTRIAN, etc
+        track_id: unique identifier
 
+    Returns:
+        object representing a 3d cuboid
+    """
+    bbox = poly.minimum_rotated_rectangle
+    centroid = bbox.centroid.coords[0]
 
-def poly_to_label(poly: Polygon, category: str = "VEHICLE", track_id: str = "") -> ObjectLabelRecord:
-    # poly in polygon format
+    # exterior consists of of x and y values for bbox vertices [0,1,2,3,0], i.e. the first vertex is repeated as last
+    x = np.array(bbox.exterior.xy[0]).reshape(5, 1)
+    y = np.array(bbox.exterior.xy[1]).reshape(5, 1)
 
-    bbox = poly.minimum_rotated_rectangle
+    v0, v1, v2, v3, _ = np.hstack([x, y])
 
-    x = bbox.exterior.xy[0]
-    y = bbox.exterior.xy[1]
     z = np.array([z for _, _, z in poly.exterior.coords])
+    height = max(z) - min(z)
 
-    # z = poly.exterior.xy[2]
-
-    d1 = dist((x[0], y[0]), (x[1], y[1]))
-    d2 = dist((x[1], y[1]), (x[2], y[2]))
+    d1 = np.linalg.norm(v0 - v1)
+    d2 = np.linalg.norm(v1 - v2)
 
+    # assign orientation so that the rectangle's longest side represents the object's length
     width = min(d1, d2)
     length = max(d1, d2)
 
-    if max(d1, d2) == d2:
-        unit_v = unit_vector((x[1], y[1]), (x[2], y[2]))
+    if d2 == length:
+        # vector points from v1 -> v2
+        v = v2 - v1
     else:
-        unit_v = unit_vector((x[0], y[0]), (x[1], y[1]))
-
-    angle = math.atan2(unit_v[1], unit_v[0])
+        # vector points from v0 -> v1
+        v = v0 - v1
 
-    height = max(z) - min(z)
+    # vector need not be unit length
+    angle_rad = np.arctan2(v[1], v[0])
+    q = yaw_to_quaternion3d(angle_rad)
 
-    # translation = center
-    center = np.array([bbox.centroid.xy[0][0], bbox.centroid.xy[1][0], min(z) + height / 2])
-
-    R = np.array(
-        [
-            [np.cos(angle), -np.sin(angle), 0],
-            [np.sin(angle), np.cos(angle), 0],
-            [0, 0, 1],
-        ]
-    )
-
-    q = quaternion.from_rotation_matrix(R)
+    # location of object in egovehicle coordinates
+    center = np.array([centroid[0], centroid[1], min(z) + height / 2])
 
     return ObjectLabelRecord(
-        quaternion=quaternion.as_float_array(q),
+        quaternion=q,
         translation=center,
         length=length,
         width=width,
@@ -220,7 +209,7 @@ def poly_to_label(poly: Polygon, category: str = "VEHICLE", track_id: str = "")
 
 
 def get_objects(clustering: DBSCAN, pts: np.ndarray, category: str = "VEHICLE") -> List[Tuple[np.ndarray, uuid.UUID]]:
-
+    """ """
     core_samples_mask = np.zeros_like(clustering.labels_, dtype=bool)
     core_samples_mask[clustering.core_sample_indices_] = True
     labels = clustering.labels_
@@ -266,18 +255,14 @@ def save_label(argoverse_data: ArgoverseTrackingLoader, labels: List[ObjectLabel
     timestamp = argoverse_data.lidar_timestamp_list[idx]
 
     for label in labels:
+        qw, qx, qy, qz = label.quaternion
         json_data = {
             "center": {
                 "x": label.translation[0],
                 "y": label.translation[1],
                 "z": label.translation[2],
             },
-            "rotation": {
-                "x": label.quaternion[0],
-                "y": label.quaternion[1],
-                "z": label.quaternion[2],
-                "w": label.quaternion[3],
-            },
+            "rotation": {"x": qx, "y": qy, "z": qz, "w": qw},
             "length": label.length,
             "width": label.width,
             "height": label.height,

argoverse/utils/transform.py

@@ -16,6 +16,26 @@
 logger = logging.getLogger(__name__)
 
 
+def yaw_to_quaternion3d(yaw: float) -> np.ndarray:
+    """Convert a rotation angle in the xy plane (i.e. about the z axis) to a quaternion.
+
+    Args:
+        yaw: angle to rotate about the z-axis, representing an Euler angle, in radians
+
+    Returns:
+        array w/ quaternion coefficients (qw,qx,qy,qz) in scalar-first order, per Argoverse convention.
+    """
+    qx, qy, qz, qw = Rotation.from_euler(seq="z", angles=yaw, degrees=False).as_quat()
+    return np.array([qw, qx, qy, qz])
+
+
+def rotmat2quat(R: np.ndarray) -> np.ndarray:
+    """Convert a rotation-matrix to a quaternion in Argo's scalar-first notation (w, x, y, z)."""
+    quat_xyzw = Rotation.from_matrix(R).as_quat()
+    quat_wxyz = quat_scipy2argo(quat_xyzw)
+    return quat_wxyz
+
+
 def quat2rotmat(q: np.ndarray) -> np.ndarray:
     """Normalizes a quaternion to unit-length, then converts it into a rotation matrix.
 
@@ -48,6 +68,13 @@ def quat_argo2scipy(q: np.ndarray) -> np.ndarray:
     return q_scipy
 
 
+def quat_scipy2argo(q: np.ndarray) -> np.ndarray:
+    """Re-order Scipy's scalar-last [x,y,z,w] quaternion order to Argoverse's scalar-first [w,x,y,z]."""
+    x, y, z, w = q
+    q_argo = np.array([w, x, y, z])
+    return q_argo
+
+
 def quat_argo2scipy_vectorized(q: np.ndarray) -> np.ndarray:
     """Re-order Argoverse's scalar-first [w,x,y,z] quaternion order to Scipy's scalar-last [x,y,z,w]"""
     return q[..., [1, 2, 3, 0]]

tests/test_competition_util.py

@@ -0,0 +1,104 @@
+import numpy as np
+
+from argoverse.evaluation.competition_util import get_polygon_from_points, poly_to_label
+
+
+def test_get_polygon_from_points() -> None:
+    """Ensure polygon contains only points within the convex hull.
+
+    Point set shape looks like:
+    .__.
+    |  |
+    |  |
+    .__.
+    """
+    # z values between -1 and 2
+    # Note: point 1 should be missing in convex hull
+    points = np.array(
+        [
+            # at upper level
+            [1, 0, 2],
+            [3, 3, 2],
+            [2, 3, 2],  # interior as linear combination of points 1 and 3
+            [1, 3, 2],
+            [3, 1, 2],
+            # now, at lower level
+            [1, 0, -1],
+            [3, 3, -1],
+            [2, 3, -1],  # interior as linear combination of points 1 and 3
+            [1, 3, -1],
+            [3, 1, -1],
+        ]
+    )
+    poly = get_polygon_from_points(points)
+
+    # note: first point is repeated as last point
+    expected_exterior_coords = [
+        (1.0, 0.0, 2.0),
+        (3.0, 3.0, 2.0),
+        (1.0, 3.0, 2.0),
+        (3.0, 1.0, 2.0),
+        (1.0, 0.0, -1.0),
+        (3.0, 3.0, -1.0),
+        (1.0, 3.0, -1.0),
+        (3.0, 1.0, -1.0),
+        (1.0, 0.0, 2.0),
+    ]
+
+    assert list(poly.exterior.coords) == expected_exterior_coords
+
+
+def test_poly_to_label() -> None:
+    """Make sure we can recover a cuboid, from a point set.
+
+    Shape should resemble a slanted bounding box, 2 * sqrt(2) in width, and 3 * sqrt(2) in length
+         .
+       /  \\
+    ./       \\
+    \\          \\
+       \\      /
+         \\  /
+            .
+    """
+    # fmt: off
+    points = np.array(
+        [
+            [4, 6, -1],
+            [4, 4, 2],
+            [7, 5, 1],
+            [7, 3, 0.5],
+            [6, 4, 0],
+            [6, 2, 0],
+            [7, 5, 0],
+            [5, 7, -1],
+            [8, 4, 0]
+        ]
+    )
+    # fmt: on
+    poly = get_polygon_from_points(points)
+    object_rec = poly_to_label(poly, category="VEHICLE", track_id="123")
+
+    bbox_verts_2d = object_rec.as_2d_bbox()
+
+    # fmt: off
+    expected_bbox_verts_2d = np.array(
+        [
+            [8, 4, 2],
+            [6, 2, 2],
+            [5, 7, 2],
+            [3, 5, 2]
+        ]
+    )
+    # fmt: on
+    assert np.allclose(expected_bbox_verts_2d, bbox_verts_2d)
+
+    expected_length = np.sqrt(2) * 3
+    expected_width = np.sqrt(2) * 2
+    expected_height = 3.0
+
+    assert np.isclose(object_rec.length, expected_length)
+    assert np.isclose(object_rec.width, expected_width)
+    assert np.isclose(object_rec.height, expected_height)
+
+    assert object_rec.label_class == "VEHICLE"
+    assert object_rec.track_id == "123"