Skip to content
Snippets Groups Projects
Commit 3bd4828c authored by Illia Oleksiienko's avatar Illia Oleksiienko
Browse files

Update ab3dmot inference and evaluation

parent c664aad4
No related branches found
No related tags found
No related merge requests found
......@@ -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 @@ class AB3DMOT():
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 @@ class AB3DMOT():
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 @@ class AB3DMOT():
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 @@ class AB3DMOT():
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:
......
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)
......@@ -284,46 +284,47 @@ class TrackingEvaluator(object):
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:
......
......@@ -135,7 +135,7 @@ class KalmanTracker3D():
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,
......
......@@ -49,6 +49,7 @@ class ObjectTracking3DAb3dmotLearner(Learner):
self.covariance_matrix = covariance_matrix
self.process_uncertainty_matrix = process_uncertainty_matrix
self.iou_threshold = iou_threshold
self.model: AB3DMOT = None
self.infers_count = 0
self.infers_time = 0
......
......@@ -247,10 +247,10 @@ class KittiTrackingDatasetIterator(DatasetIterator):
if frame not in results:
results[frame] = []
max_frame = max(max_frame, frame)
if not (remove_dontcare and box.name == "DontCare"):
results[frame].append(box)
max_frame = max(max_frame, frame)
if return_format == "tracking":
......
......@@ -48,6 +48,9 @@ class TestObjectTracking3DAb3dmot(unittest.TestCase):
cls.temp_dir, True
)
cls.use_long_tests = os.environ.get("OPENDR_USE_LONG_TESTS", "False") == "True"
cls.long_tracking_dataset_path = os.environ.get("OPENDR_KITTI_TRACKING_PATH", "")
print("Dataset downloaded", file=sys.stderr)
@classmethod
......@@ -70,11 +73,25 @@ class TestObjectTracking3DAb3dmot(unittest.TestCase):
def test_eval(self):
learner = ObjectTracking3DAb3dmotLearner()
results = learner.eval(self.dataset, count=1)
self.assertTrue("car" in results)
self.assertTrue("pedestrian" in results)
self.assertTrue("cyclist" in results)
if self.use_long_tests:
self.assertTrue(len(self.long_tracking_dataset_path) > 0)
dataset = KittiTrackingDatasetIterator(self.long_tracking_dataset_path, self.long_tracking_dataset_path, "tracking")
results = learner.eval(dataset)
self.assertTrue("car" in results)
self.assertTrue("pedestrian" in results)
self.assertTrue("cyclist" in results)
for k, v in results.items():
print(k, v)
else:
results = learner.eval(self.dataset, count=1)
self.assertTrue("car" in results)
self.assertTrue("pedestrian" in results)
self.assertTrue("cyclist" in results)
def test_infer(self):
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment