Demo 3D tracking

projects/python/perception/object_detection_3d/demos/voxel_object_detection_3d/README.md

@@ -19,12 +19,12 @@ pip install -e .
 ## Running the example
 Car 3D Object Detection using [TANet](https://arxiv.org/abs/1912.05163) from [KITTI](http://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d)-like dataset
 ```bash
-python3 demo.py --ip=0.0.0.0 --port=2605 --algorithm=voxel --model_name=tanet_car_xyres_16 --source=disk --data_path=/data/sets/kitti_second/training/velodyne --model_config=configs/tanet_car_xyres_16.proto
+python3 demo.py --ip=0.0.0.0 --port=2605 --algorithm=voxel --model_name=tanet_car_xyres_16 --source=disk --data_path=/data/sets/kitti_tracking/training/velodyne/0000--model_config=configs/tanet_car_xyres_16.proto
 ```
 
 Car 3D Object Detection using [PointPillars](https://arxiv.org/abs/1812.05784) from [KITTI](http://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d)-like dataset
 ```bash
-python3 demo.py --ip=0.0.0.0 --port=2605 --algorithm=voxel --model_name=pointpillars_car_xyres_16 --source=disk --data_path=/data/sets/kitti_second/training/velodyne --model_config=configs/tanet_car_xyres_16.proto
+python3 demo.py --ip=0.0.0.0 --port=2605 --algorithm=voxel --model_name=pointpillars_car_xyres_16 --source=disk --data_path=/data/sets/kitti_tracking/training/velodyne/0000 --model_config=configs/pointpillars_car_xyres_16.proto
 ```
 
 3D Object Detection using a specially trained model X for O3M Lidar

projects/python/perception/object_detection_3d/demos/voxel_object_detection_3d/demo.py

@@ -23,6 +23,7 @@
 
 # OpenDR imports
 from opendr.perception.object_detection_3d import VoxelObjectDetection3DLearner
+from opendr.perception.object_tracking_3d import ObjectTracking3DAb3dmotLearner
 from data_generators import (
     lidar_point_cloud_generator,
     disk_point_cloud_generator,
@@ -162,6 +163,7 @@ def voxel_object_detection_3d(config_path, model_name=None):
 
         # Init model
         detection_learner = VoxelObjectDetection3DLearner(config_path)
+        tracking_learner = ObjectTracking3DAb3dmotLearner()
 
         if model_name is not None and not os.path.exists(
             "./models/" + model_name
@@ -172,6 +174,7 @@ def voxel_object_detection_3d(config_path, model_name=None):
 
     else:
         detection_learner = None
+        tracking_learner = None
 
     def process_key(key):
 
@@ -284,8 +287,10 @@ def process_key(key):
 
             if predict:
                 predictions = detection_learner.infer(point_cloud)
+                tracking_predictions = tracking_learner.infer(predictions)
             else:
                 predictions = []
+                tracking_predictions = []
 
             if len(predictions) > 0:
                 print(
@@ -296,7 +301,7 @@ def process_key(key):
             t = time.time()
 
             frame_bev_2 = draw_point_cloud_bev(
-                point_cloud.data, predictions, scale, xs, ys
+                point_cloud.data, tracking_predictions, scale, xs, ys
             )
             frame_proj_2 = draw_point_cloud_projected_numpy(
                 point_cloud.data,

src/opendr/perception/object_tracking_3d/ab3dmot/algorithm/ab3dmot.py

@@ -15,20 +15,8 @@
 import numpy as np
 from opendr.engine.target import BoundingBox3DList, TrackingAnnotation3DList
 from scipy.optimize import linear_sum_assignment
+from opendr.perception.object_tracking_3d.ab3dmot.algorithm.core import convert_3dbox_to_8corner, iou3D
 from opendr.perception.object_tracking_3d.ab3dmot.algorithm.kalman_tracker_3d import KalmanTracker3D
-from opendr.perception.object_detection_3d.voxel_object_detection_3d.second_detector.core.box_np_ops import (
-    center_to_corner_box3d,
-)
-from numba.cuda.cudadrv.error import CudaSupportError
-
-try:
-    from opendr.perception.object_detection_3d.voxel_object_detection_3d.\
-        second_detector.core.non_max_suppression.nms_gpu import (
-            rotate_iou_gpu_eval as iou3D,
-        )
-except (CudaSupportError, ValueError):
-    def iou3D(boxes, qboxes, criterion=-1):
-        return np.ones((boxes.shape[0], qboxes.shape[0]))
 
 
 class AB3DMOT():
@@ -46,9 +34,10 @@ def __init__(
 
         self.max_staleness = max_staleness
         self.min_updates = min_updates
-        self.frame = frame
+        self.frame = frame - 1
+        self.starting_frame = frame - 1
         self.tracklets = []
-        self.last_tracklet_id = 1
+        self.last_tracklet_id = 0
         self.iou_threshold = iou_threshold
 
         self.state_dimensions = state_dimensions
@@ -60,6 +49,8 @@ def __init__(
 
     def update(self, detections: BoundingBox3DList):
 
+        self.frame += 1
+
         if len(detections) > 0:
 
             predictions = np.zeros([len(self.tracklets), self.measurement_dimensions])
@@ -68,18 +59,16 @@ def update(self, detections: BoundingBox3DList):
                 box = tracklet.predict().reshape(-1)[:self.measurement_dimensions]
                 predictions[i] = [*box]
 
-            detection_corners = center_to_corner_box3d(
-                np.array([box.location for box in detections.boxes]),
-                np.array([box.dimensions for box in detections.boxes]),
-                np.array([box.rotation_y for box in detections.boxes]),
-            )
+            detection_corners = [
+                convert_3dbox_to_8corner(np.array([*box.location, box.rotation_y, *box.dimensions]))
+                for box in detections.boxes
+            ]
 
             if len(predictions) > 0:
-                prediction_corners = center_to_corner_box3d(
-                    predictions[:, :3],
-                    predictions[:, 4:],
-                    predictions[:, 3],
-                )
+                prediction_corners = [
+                    convert_3dbox_to_8corner(p)
+                    for p in predictions
+                ]
             else:
                 prediction_corners = np.zeros((0, 8, 3))
 
@@ -115,22 +104,22 @@ def update(self, detections: BoundingBox3DList):
                     tracked_boxes.append(tracklet.tracking_bounding_box_3d(self.frame))
 
         result = TrackingAnnotation3DList(tracked_boxes)
-
-        self.frame += 1
-
         return result
 
     def reset(self):
-        self.frame = 0
+        self.frame = self.starting_frame
         self.tracklets = []
-        self.last_tracklet_id = 1
+        self.last_tracklet_id = 0
 
 
 def associate(detection_corners, prediction_corners, iou_threshold):
 
-    ious = iou3D(detection_corners, prediction_corners)
+    iou_matrix = np.zeros((len(detection_corners), len(prediction_corners)), dtype=np.float32)
+    for d, det in enumerate(detection_corners):
+        for t, trk in enumerate(prediction_corners):
+            iou_matrix[d, t] = iou3D(det, trk)[0]
 
-    detection_match_ids, prediction_match_ids = linear_sum_assignment(-ious)
+    detection_match_ids, prediction_match_ids = linear_sum_assignment(-iou_matrix)
     unmatched_detections = []
     unmatched_predictions = []
 
@@ -148,7 +137,7 @@ def associate(detection_corners, prediction_corners, iou_threshold):
         detection_id = detection_match_ids[i]
         prediction_id = prediction_match_ids[i]
 
-        if ious[detection_id, prediction_id] < iou_threshold:
+        if iou_matrix[detection_id, prediction_id] < iou_threshold:
             unmatched_detections.append(detection_id)
             unmatched_predictions.append(prediction_id)
         else:

src/opendr/perception/object_tracking_3d/ab3dmot/algorithm/core.py

@@ -0,0 +1,127 @@
+# Copyright 2020-2022 OpenDR European Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import List
+import numba
+import copy
+import numpy as np
+from scipy.spatial import ConvexHull
+
+
+@numba.jit
+def polygon_area(x, y):
+    return 0.5 * np.abs(np.dot(x, np.roll(y, 1)) - np.dot(y, np.roll(x, 1)))
+
+
+@numba.jit
+def corner_box3d_volume(corners: np.array):  # [8, 3] -> []
+
+    result = (
+        np.sqrt(np.sum((corners[0, :] - corners[1, :]) ** 2)) *
+        np.sqrt(np.sum((corners[1, :] - corners[2, :]) ** 2)) *
+        np.sqrt(np.sum((corners[0, :] - corners[4, :]) ** 2))
+    )
+    return result
+
+
+def polygon_clip(subject_polygon, clip_polygon):  # [(x, y)] -> [(x, y)] -> [(x, y))
+    def is_inside(p, clip_polygon1, clip_polygon2):
+        return (clip_polygon2[0] - clip_polygon1[0]) * (p[1] - clip_polygon1[1]) > (
+            clip_polygon2[1] - clip_polygon1[1]
+        ) * (p[0] - clip_polygon1[0])
+
+    def intersection(clip_polygon1, clip_polygon2):
+        dc = [clip_polygon1[0] - clip_polygon2[0], clip_polygon1[1] - clip_polygon2[1]]
+        dp = [s[0] - e[0], s[1] - e[1]]
+        n1 = clip_polygon1[0] * clip_polygon2[1] - clip_polygon1[1] * clip_polygon2[0]
+        n2 = s[0] * e[1] - s[1] * e[0]
+        n3 = 1.0 / (dc[0] * dp[1] - dc[1] * dp[0])
+        return [(n1 * dp[0] - n2 * dc[0]) * n3, (n1 * dp[1] - n2 * dc[1]) * n3]
+
+    outputList = subject_polygon
+    cp1 = clip_polygon[-1]
+
+    for clip_vertex in clip_polygon:
+        cp2 = clip_vertex
+        inputList = outputList
+        outputList = []
+        s = inputList[-1]
+
+        for subjectVertex in inputList:
+            e = subjectVertex
+            if is_inside(e, cp1, cp2):
+                if not is_inside(s, cp1, cp2):
+                    outputList.append(intersection(cp1, cp2))
+                outputList.append(e)
+            elif is_inside(s, cp1, cp2):
+                outputList.append(intersection(cp1, cp2))
+            s = e
+        cp1 = cp2
+        if len(outputList) == 0:
+            return None
+    return outputList
+
+
+@numba.jit
+def convex_hull_intersection(
+    polygon1: List[tuple], polygon2: List[tuple]
+):  # [(x, y)] -> [(x, y)] -> ([(x, y), []])
+    inter_p = polygon_clip(polygon1, polygon2)
+    if inter_p is not None:
+        hull_inter = ConvexHull(inter_p)
+        return inter_p, hull_inter.volume
+    else:
+        return None, 0.0
+
+
+def iou3D(corners1, corners2):  # [8, 3] -> [8, 3] -> ([], [])
+    # corner points are in counter clockwise order
+    rect1 = [(corners1[i, 0], corners1[i, 2]) for i in range(3, -1, -1)]
+    rect2 = [(corners2[i, 0], corners2[i, 2]) for i in range(3, -1, -1)]
+    area1 = polygon_area(np.array(rect1)[:, 0], np.array(rect1)[:, 1])
+    area2 = polygon_area(np.array(rect2)[:, 0], np.array(rect2)[:, 1])
+    _, inter_area = convex_hull_intersection(rect1, rect2)
+    iou_2d = inter_area / (area1 + area2 - inter_area)
+    y_max = min(corners1[0, 1], corners2[0, 1])
+    y_min = max(corners1[4, 1], corners2[4, 1])
+    inter_vol = inter_area * max(0.0, y_max - y_min)
+    vol1 = corner_box3d_volume(corners1)
+    vol2 = corner_box3d_volume(corners2)
+    iou = inter_vol / (vol1 + vol2 - inter_vol)
+    return iou, iou_2d
+
+
+@numba.jit
+def rotation_matrix_y(t):  # [] -> [3, 3]
+    c = np.cos(t)
+    s = np.sin(t)
+    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
+
+
+def convert_3dbox_to_8corner(bbox3d_input):  # [7] -> [8, 3]
+    bbox3d = copy.copy(bbox3d_input)
+    rot_matrix = rotation_matrix_y(bbox3d[3])
+
+    l, w, h = bbox3d[4:7]
+
+    x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]
+    y_corners = [0, 0, 0, 0, -h, -h, -h, -h]
+    z_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]
+
+    corners_3d = np.dot(rot_matrix, np.vstack([x_corners, y_corners, z_corners]))
+    corners_3d[0, :] = corners_3d[0, :] + bbox3d[0]
+    corners_3d[1, :] = corners_3d[1, :] + bbox3d[1]
+    corners_3d[2, :] = corners_3d[2, :] + bbox3d[2]
+
+    return np.transpose(corners_3d)

src/opendr/perception/object_tracking_3d/ab3dmot/algorithm/evaluate.py

@@ -284,46 +284,47 @@ def _load_data(
             for boxList in input_seq_data:
                 input_seq_boxes += boxList.boxes
 
-            f_data = [[] for x in range(input_seq_boxes[-1].frame + 1)]
+            # f_data = [[] for x in range(input_seq_boxes[-1].frame + 1)]
+            f_data = [[] for x in range(len(input_seq_data))]
 
-            for TrackingAnnotation3D in input_seq_boxes:
+            for trackingAnnotation3D in input_seq_boxes:
                 # KITTI tracking benchmark data format:
                 # (frame,tracklet_id,objectType,truncation,occlusion,alpha,x1,y1,x2,y2,h,w,l,X,Y,Z,ry)
 
-                if not any([s for s in classes if s == TrackingAnnotation3D.name.lower()]):
+                if not any([s for s in classes if s == trackingAnnotation3D.name.lower()]):
                     continue
                 # get fields from table
-                t_data.frame = int(TrackingAnnotation3D.frame)
-                t_data.track_id = int(TrackingAnnotation3D.id)
-                t_data.obj_type = TrackingAnnotation3D.name.lower()  # object type [car, pedestrian, cyclist, ...]
+                t_data.frame = int(trackingAnnotation3D.frame)
+                t_data.track_id = int(trackingAnnotation3D.id)
+                t_data.obj_type = trackingAnnotation3D.name.lower()  # object type [car, pedestrian, cyclist, ...]
                 t_data.truncation = int(
-                    TrackingAnnotation3D.truncated
+                    trackingAnnotation3D.truncated
                 )  # truncation [-1,0,1,2]
                 t_data.occlusion = int(
-                    TrackingAnnotation3D.occluded
+                    trackingAnnotation3D.occluded
                 )  # occlusion  [-1,0,1,2]
-                t_data.obs_angle = float(TrackingAnnotation3D.alpha)  # observation angle [rad]
-                t_data.x1 = float(TrackingAnnotation3D.bbox2d[0])  # left   [px]
-                t_data.y1 = float(TrackingAnnotation3D.bbox2d[1])  # top    [px]
-                t_data.x2 = float(TrackingAnnotation3D.bbox2d[2])  # right  [px]
-                t_data.y2 = float(TrackingAnnotation3D.bbox2d[3])  # bottom [px]
-                t_data.h = float(TrackingAnnotation3D.dimensions[0])  # height [m]
-                t_data.w = float(TrackingAnnotation3D.dimensions[1])  # width  [m]
-                t_data.length = float(TrackingAnnotation3D.dimensions[2])  # length [m]
-                t_data.X = float(TrackingAnnotation3D.location[0])  # X [m]
-                t_data.Y = float(TrackingAnnotation3D.location[1])  # Y [m]
-                t_data.Z = float(TrackingAnnotation3D.location[2])  # Z [m]
-                t_data.yaw = float(TrackingAnnotation3D.rotation_y)  # yaw angle [rad]
-                t_data.score = float(TrackingAnnotation3D.confidence)
+                t_data.obs_angle = float(trackingAnnotation3D.alpha)  # observation angle [rad]
+                t_data.x1 = float(trackingAnnotation3D.bbox2d[0])  # left   [px]
+                t_data.y1 = float(trackingAnnotation3D.bbox2d[1])  # top    [px]
+                t_data.x2 = float(trackingAnnotation3D.bbox2d[2])  # right  [px]
+                t_data.y2 = float(trackingAnnotation3D.bbox2d[3])  # bottom [px]
+                t_data.h = float(trackingAnnotation3D.dimensions[0])  # height [m]
+                t_data.w = float(trackingAnnotation3D.dimensions[1])  # width  [m]
+                t_data.length = float(trackingAnnotation3D.dimensions[2])  # length [m]
+                t_data.X = float(trackingAnnotation3D.location[0])  # X [m]
+                t_data.Y = float(trackingAnnotation3D.location[1])  # Y [m]
+                t_data.Z = float(trackingAnnotation3D.location[2])  # Z [m]
+                t_data.yaw = float(trackingAnnotation3D.rotation_y)  # yaw angle [rad]
+                t_data.score = float(trackingAnnotation3D.confidence)
 
                 # do not consider objects marked as invalid
-                if t_data.track_id is -1 and t_data.obj_type != "dontcare":
+                if t_data.track_id == -1 and t_data.obj_type != "dontcare":
                     continue
 
                 idx = t_data.frame
                 # check if length for frame data is sufficient
                 if idx >= len(f_data):
-                    raise ValueError("Frame " + str(idx) + "is out of range")
+                    raise ValueError("Frame " + str(idx) + " is out of range")
 
                 id_frame = (t_data.frame, t_data.track_id)
                 if id_frame in id_frame_cache and not loading_groundtruth:

src/opendr/perception/object_tracking_3d/ab3dmot/algorithm/kalman_tracker_3d.py

@@ -135,7 +135,7 @@ def tracking_bounding_box_3d(self, frame):
         return TrackingAnnotation3D(
             self.name, self.truncated, self.occluded,
             self.alpha, self.bbox2d,
-            self.kalman_filter.x[4:].reshape(-1),
+            self.kalman_filter.x[4:7].reshape(-1),
             self.kalman_filter.x[:3].reshape(-1),
             float(self.kalman_filter.x[3]),
             self.id,

src/opendr/perception/object_tracking_3d/ab3dmot/object_tracking_3d_ab3dmot_learner.py

@@ -49,6 +49,7 @@ def __init__(
         self.covariance_matrix = covariance_matrix
         self.process_uncertainty_matrix = process_uncertainty_matrix
         self.iou_threshold = iou_threshold
+        self.model = None
 
         self.infers_count = 0
         self.infers_time = 0