Fix convexhull boundary computation

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,27 +121,19 @@ 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
+        polygon: shapely Polygon representing the points along the convex hull's boundary
     """
-    points = points
     hull = ConvexHull(points)
 
-    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 +149,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)
-
-    # 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],
-        ]
-    )
+    # vector need not be unit length
+    angle_rad = np.arctan2(v[1], v[0])
+    q = yaw_to_quaternion3d(angle_rad)
 
-    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 +206,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 +252,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"