Fix convexhull boundary computation

argoverse/evaluation/competition_util.py

@@ -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,64 +149,49 @@ 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 dist(p1: Tuple[float, float], p2: Tuple[float, float]) -> float:
-    return math.sqrt(((p1[0] - p2[0]) ** 2) + ((p1[1] - p2[1]) ** 2))
-
-
 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.
 
     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]
 
-    x = bbox.exterior.xy[0]
-    y = bbox.exterior.xy[1]
-    z = np.array([z for _, _, z in poly.exterior.coords])
+    # 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)
 
-    # z = poly.exterior.xy[2]
+    v0, v1, v2, v3, _ = np.hstack([x, y])
+
+    z = np.array([z for _, _, z in poly.exterior.coords])
+    height = max(z) - min(z)
 
-    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]))
+        # vector points from v0 -> v1
+        v = v0 - v1
 
-    angle_rad = np.arctan2(unit_v[1], unit_v[0])
+    # vector need not be unit length
+    angle_rad = np.arctan2(v[1], v[0])
     q = yaw_to_quaternion3d(angle_rad)
 
-    height = max(z) - min(z)
-
     # location of object in egovehicle coordinates
-    center = np.array([bbox.centroid.xy[0][0], bbox.centroid.xy[1][0], min(z) + height / 2])
-
-    c = np.cos(angle_rad)
-    s = np.sin(angle_rad)
-    R = np.array(
-        [
-            [c, -s, 0],
-            [s, c, 0],
-            [0, 0, 1],
-        ]
-    )
+    center = np.array([centroid[0], centroid[1], min(z) + height / 2])
 
     return ObjectLabelRecord(
         quaternion=q,
@@ -230,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_

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"