├── .gitignore ├── .gitmodules ├── README.md ├── data_utils ├── __init__.py ├── dataset_stuff.py ├── fafe_stuff.py ├── generate_data.py ├── kitti_stuff.py ├── nuscenes_stuff.py └── test_kitti.py ├── images ├── pmbm-ca-0016.gif ├── pmbm-ca-0020.gif └── track_seq_0020_frame_0105.png ├── pmbm ├── .DS_Store ├── __init__.py ├── config.py ├── filter.py ├── global_hypothesis.py ├── pmbm.py ├── poisson.py ├── single_target_hypothesis.py ├── target.py ├── test_pmbm.py ├── test_poisson.py └── test_target.py ├── runforrest.ipynb └── utils ├── GA.ipynb ├── UKF.py ├── Uniform.png ├── __init__.py ├── constants.py ├── coord_transf.py ├── datadriven_poisson_birth.ipynb ├── environments.py ├── eval_metrics.py ├── gen_hyperparam_search.py ├── logger.py ├── matrix_stuff.py ├── moment_matching.py ├── mot_metrics.py ├── motion_models.py ├── plot_stuff.py ├── poisson_birth_state_models.py ├── test_UKF.py ├── test_coord_transf.py ├── test_eval_metrics.py ├── test_motion_models.py ├── test_motmetrics.py └── test_pmbm.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea* 2 | .DS_Store 3 | .ipynb_checkpoints 4 | log* 5 | *pytest_cache* 6 | *stats_seq* 7 | venv 8 | *__pycache__* 9 | *.png 10 | *.mp4 -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "murty"] 2 | path = murty 3 | url = git@github.com:erikbohnsack/murty.git 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Please note that this repository is not actively maintained. 2 | 3 | # PMBM 4 | 5 | This is the implementation of the Poisson Multi Bernoulli Mixture Filter 6 | for the Master Thesis `Multi-Object Tracking using either Deep Learning or PMBM filtering` by Erik Bohnsack and Adam Lilja 7 | at Chalmers University of Technology, spring of 2019. 8 | 9 | ![](images/track_seq_0020_frame_0105.png) 10 | 11 | The implementation is done in Python 3.7 and it has only been tested on Ubuntu 16.04 and MacOS 10.13.6. 12 | 13 | ## Requirements 14 | 15 | `python 3.7` 16 | 17 | 1. Get Murty-submodule `git submodule update` 18 | 1. Install Murty`pip3 install ./murty` 19 | 1. filterpy `pip3 install filterpy` 20 | 1. motmetrics `pip3 install motmetrics` 21 | 1. deap `pip3 install deap` 22 | 23 | ## Results in gif-format 24 | 25 | KITTI training sequence 20. Simulated object detections with noise, clutter and miss detections. 26 | Constant Acceleration motion model. 27 | 28 | ![](images/pmbm-ca-0020.gif) 29 | 30 | KITTI training sequence 16. Simulated object detections with noise, clutter and miss detections. 31 | Constant Acceleration motion model. 32 | 33 | ![](images/pmbm-ca-0016.gif) 34 | 35 | ## Run 36 | 37 | Check `runforrest.ipynb` 38 | -------------------------------------------------------------------------------- /data_utils/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /data_utils/dataset_stuff.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class Label: 4 | def __init__(self, 5 | frame, 6 | track_id, 7 | type, 8 | truncated, 9 | occluded, 10 | alpha, 11 | bbox, 12 | dimensions, 13 | location, 14 | rotation_y): 15 | self.frame = int(frame) 16 | self.track_id = int(track_id) 17 | self.type = str(type), 18 | self.truncated = int(truncated) 19 | self.occluded = int(occluded) 20 | self.alpha = float(alpha) 21 | self.bbox = [float(x) for x in bbox] 22 | self.dimensions = [float(x) for x in dimensions] 23 | self.location = [float(x) for x in location] 24 | self.rotation_y = float(rotation_y) 25 | 26 | def __repr__(self): 27 | return ''.format(self.frame, self.type, self.track_id, self.location) 28 | 29 | 30 | class IMU: 31 | def __init__(self, frame, vf, vl, ru): 32 | self.frame = frame 33 | self.vf = vf # Velocity forward 34 | self.vl = vl # Velocity left 35 | self.ru = ru # Rotation around up 36 | 37 | def __repr__(self): 38 | return ' \n'.format(self.frame, round(self.vf,2), round(self.vl,2), round(self.ru,2)) -------------------------------------------------------------------------------- /data_utils/fafe_stuff.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from utils import logger 4 | from scipy.spatial.distance import cdist 5 | 6 | class FafeDetections: 7 | def __init__(self, showroom_path, sequence): 8 | file_path = os.path.join(showroom_path, 'detections_' + str(sequence).zfill(4) + '.txt') 9 | assert os.path.exists(file_path), 'File does not exist: {}'.format(file_path) 10 | 11 | with open(file_path, 'r') as f: 12 | lines = f.read().splitlines() 13 | 14 | detections = {} 15 | frames = [] 16 | max_frame = 0 17 | 18 | for line in lines: 19 | l = line.split(' ') 20 | _frame = int(l[0]) 21 | _class = l[1] 22 | _x = float(l[2]) 23 | _y = float(l[3]) 24 | _r = float(l[4]) 25 | if _frame not in frames: 26 | frames.append(_frame) 27 | detections[_frame] = [[_x, _y, _r, _class]] 28 | else: 29 | detections[_frame].append([_x, _y, _r, _class]) 30 | 31 | if _frame > max_frame: 32 | max_frame = _frame 33 | 34 | # Add empty measurements in frames where we didn't find anyting 35 | for f in np.arange(0, max_frame + 1): 36 | if f not in frames: 37 | detections[f] = [] 38 | 39 | self.detections = detections 40 | self.max_frame_idx = max_frame 41 | 42 | def get_fafe_detections(self, frame_idx, measurement_dims): 43 | measurements = [] 44 | classes = [] 45 | for meas in self.detections[frame_idx]: 46 | measurements.append(np.array(meas[0:measurement_dims]).reshape(measurement_dims,1)) 47 | classes.append(meas[-1]) 48 | return measurements, classes 49 | 50 | class FafeMeasurements: 51 | def __init__(self, log_path): 52 | 53 | data = logger.load_logging_data(log_path) 54 | self.frames = [] 55 | self.measurements = [] 56 | 57 | for d in data[0]: 58 | self.frames.append(d['frame_id']) 59 | self.measurements.append(remove_duplicates(d['measurements'])) 60 | 61 | self.num_skip = self.frames[0] 62 | 63 | def get_fafe_measurements(self, frame_idx, measurement_dims): 64 | if frame_idx in self.frames: 65 | out_meas = [] 66 | out_cls = [] 67 | for meas in self.measurements[frame_idx-self.num_skip]: 68 | out_meas.append(np.array(meas[0:measurement_dims]).reshape(measurement_dims,1)) 69 | out_cls.append('Car') 70 | return out_meas, out_cls 71 | else: 72 | return [], [] 73 | 74 | def remove_duplicates(measurements): 75 | meas = [] 76 | for ir in measurements: 77 | a = ir[0][0] 78 | b = ir[1][0] 79 | c = [a, b] 80 | if c not in meas: 81 | meas.append(c) 82 | return np.array(meas) 83 | 84 | 85 | -------------------------------------------------------------------------------- /data_utils/generate_data.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from utils import motion_models 4 | import random 5 | 6 | 7 | class GenerateData: 8 | def __init__(self): 9 | pass 10 | 11 | def generate_2D_data(initial_states=[np.asarray([[-45], [45], [10], [-5]]), np.asarray([[-40], [-30], [5], [15]])], 12 | initial_variance=np.eye(4), 13 | motion_noise=0.1, 14 | mutation_probability=0.4, 15 | measurement_noise=0.4, 16 | missed_detection_probability=0.05, 17 | clutter_probability=0.1, 18 | dT=1, 19 | time_steps=20): 20 | ''' Generating two trajectories in a 2D grid 21 | Input: - 22 | 23 | Output: true_trajectory[ trajectory, time_step , [x,y,dx,dy] ] 24 | measurements[ time_step, measurements, [x,y] ] 25 | ''' 26 | 27 | targets = initial_states 28 | cv_model = motion_models.ConstantVelocityModel(motion_noise) 29 | 30 | true_trajectories = [] 31 | for i in range(len(targets)): 32 | true_trajectory = [] 33 | next_state = targets[i] 34 | true_trajectory.append(next_state) 35 | for t in range(time_steps): 36 | next_state, _ = cv_model(next_state, initial_variance, dT=dT) 37 | 38 | # Mutation 39 | if mutation_probability < random.random(): 40 | next_state[0] = next_state[0] + random.uniform(-0.05, 0.05) * next_state[0] 41 | if mutation_probability < random.random(): 42 | next_state[1] = next_state[1] + random.uniform(-0.05, 0.05) * next_state[1] 43 | if mutation_probability < random.random(): 44 | next_state[2] = next_state[2] + random.uniform(-0.05, 0.05) * next_state[2] 45 | if mutation_probability < random.random(): 46 | next_state[3] = next_state[3] + random.uniform(-0.05, 0.05) * next_state[3] 47 | 48 | true_trajectory.append(next_state) 49 | true_trajectories.append(true_trajectory) 50 | 51 | measurements = [] 52 | for t in range(time_steps): 53 | current_measurements = [] 54 | for i in range(len(targets)): 55 | x_pos = true_trajectories[i][t][0] 56 | y_pos = true_trajectories[i][t][1] 57 | 58 | x_pos = (1 - random.uniform(-measurement_noise, measurement_noise)) * x_pos 59 | y_pos = (1 - random.uniform(-measurement_noise, measurement_noise)) * y_pos 60 | 61 | if missed_detection_probability > random.random(): 62 | meas = np.array([], dtype=np.float).reshape(0, 2) 63 | else: 64 | meas = np.array((x_pos, y_pos)) 65 | current_measurements.append(meas) 66 | 67 | if clutter_probability > random.random(): 68 | x_pos = (1 - random.uniform(-2 * measurement_noise, 2 * measurement_noise)) * x_pos 69 | y_pos = (1 - random.uniform(-2 * measurement_noise, 2 * measurement_noise)) * y_pos 70 | meas = np.array((x_pos, y_pos)) 71 | current_measurements.append(meas) 72 | 73 | measurements.append(current_measurements) 74 | 75 | return true_trajectories, measurements 76 | 77 | 78 | 79 | 80 | def plot_generated_data(true_trajectories, measurements): 81 | plt.figure() 82 | for tt in true_trajectories: 83 | _x = [x[0] for x in tt] 84 | _y = [x[1] for x in tt] 85 | plt.subplot(1, 1, 1) 86 | plt.plot(_x, _y, 'o-') 87 | plt.title('A tale of 2 targets') 88 | plt.ylabel('y') 89 | plt.xlabel('x') 90 | plt.grid() 91 | 92 | for t, current_measurements in enumerate(measurements): 93 | for meas in current_measurements: 94 | try: 95 | _x = meas[0] 96 | _y = meas[1] 97 | plt.subplot(1, 1, 1) 98 | plt.plot(_x, _y, 'ks') 99 | except: 100 | continue 101 | 102 | plt.show() 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /data_utils/kitti_stuff.py: -------------------------------------------------------------------------------- 1 | import os 2 | import cv2 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | import matplotlib as mpl 6 | import random 7 | from .dataset_stuff import Label, IMU 8 | 9 | PossibleClasses = ['Car', 'Pedestrian', 'Van', 'Cyclist'] 10 | 11 | class Kitti: 12 | def __init__(self, ROOT, split='training'): 13 | self.path = os.path.join(ROOT, split) 14 | self.label_path = os.path.join(self.path, 'label_2') 15 | self.image_path = os.path.join(self.path, 'image_2') 16 | self.velo_path = os.path.join(self.path, 'velodyne') 17 | self.imu_path = os.path.join(self.path, 'oxts') 18 | self.measurements = None 19 | self.classes = None 20 | self.imgs, self.pcds, self.lbls, self.imus = None, None, None, None 21 | self.max_frame_idx = None 22 | 23 | self.dT = 0.1 # 10 Hz sampling rate for kitti tracking dataset 24 | 25 | def __repr__(self): 26 | return ''.format(self.path) 27 | 28 | def load(self, sequence_idx, frame_idx): 29 | self.imgs = self.load_image(sequence_idx, frame_idx) 30 | self.pcds = self.load_velos(sequence_idx) 31 | self.lbls = self.load_labels(sequence_idx) 32 | self.imus = self.load_imu(sequence_idx) 33 | 34 | def load_image(self, sequence_idx, frame_idx): 35 | file_path = self.image_path + '/' + str(sequence_idx).zfill(4) + '/' + str(frame_idx).zfill(6) + '.png' 36 | assert os.path.exists(file_path), 'File does not exist: {}'.format(file_path) 37 | image = cv2.imread(file_path) 38 | return cv2.cvtColor(image, cv2.COLOR_BGR2RGB) 39 | 40 | def load_velos(self, sequence_idx): 41 | return None 42 | 43 | def load_labels(self, sequence_idx): 44 | file_path = os.path.join(self.label_path, str(sequence_idx).zfill(4) + '.txt') 45 | assert os.path.exists(file_path), 'File does not exist: {}'.format(file_path) 46 | with open(file_path, 'r') as f: 47 | lines = f.read().splitlines() 48 | labels = [] 49 | for line in lines: 50 | l = line.split(' ') 51 | lbl = Label(l[0], 52 | l[1], 53 | l[2], 54 | l[3], 55 | l[4], 56 | l[5], 57 | l[6:10], 58 | l[10:13], 59 | l[13:16], 60 | l[16]) 61 | labels.append(lbl) 62 | max_frame_idx = labels[-1].frame 63 | ld = {i: [] for i in range(max_frame_idx+1)} 64 | {l.frame: ld[l.frame].append(l) for l in labels if l.type[0] != 'DontCare'} 65 | self.max_frame_idx = max_frame_idx 66 | return ld 67 | 68 | def load_imu(self, sequence_idx): 69 | file_path = os.path.join(self.imu_path, str(sequence_idx).zfill(4) + '.txt') 70 | assert os.path.exists(file_path), 'File does not exist: {}'.format(file_path) 71 | with open(file_path, 'r') as f: 72 | lines = f.read().splitlines() 73 | 74 | imud = {i: [] for i in range(len(lines))} 75 | 76 | for idx, line in enumerate(lines): 77 | l = line.split(' ') 78 | _vf = float(l[8]) # Velocity forward 79 | _vl = float(l[9]) # Velocity leftward 80 | _vu = float(l[22]) # Rotation around upward 81 | imud[idx] = IMU(idx, _vf, _vl, _vu) 82 | 83 | return imud 84 | 85 | 86 | def load_measurements(self, p_missed=0.05, p_clutter=0.05, sigma_xy=0.1, sigma_psi=0.2, radius_tuple=(0, 100), 87 | angle_tuple=(0.78, 2.35)): 88 | assert self.lbls is not None, 'Labels not loaded' 89 | 90 | measurement_dict = {} 91 | class_dict = {} 92 | 93 | for frame_idx in range(self.max_frame_idx): 94 | true_states = [np.array([[m.location[0]], [m.location[2]], [m.rotation_y]]) 95 | for m in self.lbls[frame_idx] 96 | if not m.type[0] == 'DontCare'] 97 | 98 | true_class = [m.type[0] for m in self.lbls[frame_idx] if not m.type[0] == 'DontCare'] 99 | 100 | measurements = [] 101 | classes = [] 102 | for ix, state in enumerate(true_states): 103 | if p_missed < random.random(): 104 | state[0] += random.normalvariate(0, sigma_xy) 105 | state[1] += random.normalvariate(0, sigma_xy) 106 | state[2] += random.normalvariate(0, sigma_psi) 107 | measurements.append(state) 108 | classes.append(true_class[ix]) 109 | if p_clutter > random.random(): 110 | r = random.uniform(radius_tuple[0], radius_tuple[1]) 111 | angle = random.uniform(angle_tuple[0], angle_tuple[1]) 112 | x = r * np.cos(angle) 113 | y = r * np.sin(angle) 114 | s = np.array([[x], [y], [angle]]) 115 | measurements.append(s) 116 | classes.append(random.choice(PossibleClasses)) 117 | 118 | measurement_dict[frame_idx] = measurements 119 | class_dict[frame_idx] = classes 120 | 121 | self.measurements = measurement_dict 122 | self.classes = class_dict 123 | 124 | def get_measurements(self, frame_idx, measurement_dims, classes_to_track=['all']): 125 | assert self.lbls is not None, 'Labels not loaded' 126 | assert self.measurements is not None, 'Measurements not loaded' 127 | assert self.classes is not None, 'Classes not loaded' 128 | # Lazy coding yes indeed 129 | output_meas = [] 130 | output_cls = [] 131 | for i, cls in enumerate(self.classes[frame_idx]): 132 | if cls not in classes_to_track and 'all' not in classes_to_track : 133 | continue 134 | if measurement_dims == 3: 135 | meas = self.measurements[frame_idx][i] 136 | elif measurement_dims == 2: 137 | m = self.measurements[frame_idx][i] 138 | meas = np.array([m[0], m[1]]) 139 | else: 140 | raise ValueError('Add what should happen for measurement_dims = {} and try again...'.format(measurement_dims)) 141 | output_meas.append(meas) 142 | output_cls.append(cls) 143 | return output_meas, output_cls 144 | 145 | def get_measurements_old(self, frame_idx, measurement_dims, p_missed=0.05, p_clutter=0.05, sigma_xy=0.1, sigma_psi=0.2, 146 | radius_tuple=(0, 100), angle_tuple=(0.78, 2.35)): 147 | assert self.lbls is not None, 'Labels not loaded' 148 | if measurement_dims == 3: 149 | true_states = [np.array([[m.location[0]], [m.location[2]], [m.rotation_y]]) for m in self.lbls[frame_idx] if not m.type[0]=='DontCare'] 150 | elif measurement_dims == 2: 151 | true_states = [np.array([[m.location[0]], [m.location[2]]]) for m in self.lbls[frame_idx] if not m.type[0]=='DontCare'] 152 | else: 153 | raise ValueError('Add what should happen for measurement_dims = {} and try again...'.format(measurement_dims)) 154 | 155 | true_class = [m.type[0] for m in self.lbls[frame_idx] if not m.type[0]=='DontCare'] 156 | measurements = [] 157 | classes = [] 158 | 159 | for ix, state in enumerate(true_states): 160 | if p_missed < random.random(): 161 | state[0] += random.normalvariate(0, sigma_xy) 162 | state[1] += random.normalvariate(0, sigma_xy) 163 | if measurement_dims == 3: 164 | state[2] += random.normalvariate(0, sigma_psi) 165 | measurements.append(state) 166 | classes.append(true_class[ix]) 167 | if p_clutter > random.random(): 168 | r = random.uniform(radius_tuple[0], radius_tuple[1]) 169 | angle = random.uniform(angle_tuple[0], angle_tuple[1]) 170 | x = r * np.cos(angle) 171 | y = r * np.sin(angle) 172 | if measurement_dims == 3: 173 | s = np.array([[x], [y], [angle]]) 174 | else: 175 | s = np.array([[x], [y]]) 176 | #print("r: {}\n, angle: {}\n, clutter: {}".format(r, angle, s)) 177 | measurements.append(s) 178 | classes.append(random.choice(PossibleClasses)) 179 | return measurements, classes 180 | 181 | def get_bev_states(self, frame_idx, classes_to_track=['all']): 182 | assert self.lbls is not None, 'Labels not loaded' 183 | if 'all' in classes_to_track: 184 | return [np.array([[m.location[0]], [m.location[2]]]) for m in self.lbls[frame_idx] if not m.type[0]=='DontCare'] 185 | else: 186 | return [np.array([[m.location[0]], [m.location[2]]]) for m in self.lbls[frame_idx] if m.type[0] in classes_to_track] 187 | 188 | def get_ego_bev_velocity(self, frame_idx): 189 | assert self.imus is not None, 'IMU data not loaded' 190 | return np.array([[-self.imus[frame_idx].vl], [self.imus[frame_idx].vf]]) 191 | 192 | def get_ego_bev_rotation(self, frame_idx): 193 | assert self.imus is not None, 'IMU data not loaded' 194 | _r = self.dT * self.imus[frame_idx].ru 195 | return np.array([[np.cos(_r), -np.sin(_r)],[np.sin(_r), np.cos(_r)]]) 196 | 197 | def get_ext_bev_rotation(self, frame_idx): 198 | assert self.imus is not None, 'IMU data not loaded' 199 | _r = - self.dT * self.imus[frame_idx].ru 200 | return np.array([[np.cos(_r), -np.sin(_r)],[np.sin(_r), np.cos(_r)]]) 201 | 202 | def get_ego_bev_motion(self): 203 | assert self.imus is not None, 'IMU data not loaded' 204 | ego_state_history = np.array([], dtype=np.float).reshape(2,0) 205 | ego_state = np.zeros((2,1)) 206 | for frame_idx in range(self.max_frame_idx+1): 207 | current_velocity = self.get_ego_bev_velocity(frame_idx) 208 | current_rotation = self.get_ego_bev_rotation(frame_idx) 209 | # TODO: test if better to rotate first and then take step 210 | ego_state = current_rotation @ ego_state + self.dT * current_velocity 211 | ego_state_history = np.hstack((ego_state_history, ego_state)) 212 | return ego_state_history 213 | 214 | def get_bev_gt_trajectories(self, up_to_frame_idx): 215 | assert self.imus is not None or self.lbls is not None, 'Pick sequence and load data first.' 216 | states_to_plot = [] 217 | for frame_idx in range(0, up_to_frame_idx): 218 | current_state = self.get_bev_states(frame_idx) 219 | current_velocity = self.get_ego_bev_velocity(frame_idx) 220 | current_rotation = self.get_ext_bev_rotation(frame_idx) 221 | # TODO: test if better to rotate first and then take step 222 | states_to_plot = [current_rotation @ (s - self.dT * current_velocity) for s in states_to_plot] 223 | states_to_plot += current_state 224 | 225 | _x = [x[0, 0] for x in states_to_plot] 226 | _z = [x[1, 0] for x in states_to_plot] 227 | return _x, _z 228 | 229 | def plot_bev_gt_trajectories(self): 230 | _x, _z = self.get_bev_gt_trajectories(up_to_frame_idx=self.max_frame_idx) 231 | fig, ax = plt.subplots(1, 1) 232 | fig.set_size_inches(5, 5, forward=True) 233 | plt.plot(_x, _z, 'o', ms=0.5) 234 | ax.set_xlabel('x') 235 | ax.set_ylabel('z') 236 | ax.set_title('History of ground truth targets') 237 | ax.grid(True) 238 | plt.show() 239 | 240 | def plot_ego_bev_motion(self): 241 | ego_states = self.get_ego_bev_motion() 242 | fig, ax = plt.subplots(1, 1) 243 | fig.set_size_inches(5, 5, forward=True) 244 | plt.plot(ego_states[0], ego_states[1], 'o', ms=0.5) 245 | ax.set_xlabel('x') 246 | ax.set_ylabel('z') 247 | ax.set_title('History of ego motion') 248 | ax.grid(True) 249 | plt.show() 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | -------------------------------------------------------------------------------- /data_utils/nuscenes_stuff.py: -------------------------------------------------------------------------------- 1 | import os 2 | import cv2 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | import matplotlib as mpl 6 | import random 7 | from .dataset_stuff import Label, IMU 8 | from utils.constants import PossibleClasses 9 | from nuscenes.nuscenes import NuScenes 10 | 11 | class Nuscenes: 12 | def __init__(self, ROOT): 13 | self.path = ROOT 14 | 15 | self.label_path = os.path.join(self.path, 'label_2') 16 | self.image_path = os.path.join(self.path, 'CAM_FRONT') 17 | self.velo_path = os.path.join(self.path, 'LIDAR_TOP') 18 | self.imu_path = os.path.join(self.path, 'oxts') 19 | 20 | self.imgs, self.pcds, self.lbls, self.imus = None, None, None, None 21 | self.max_frame_idx = None 22 | 23 | self.dT = 0.1 # 10 Hz sampling rate for kitti tracking dataset 24 | 25 | def __repr__(self): 26 | return ''.format(self.path) 27 | 28 | -------------------------------------------------------------------------------- /data_utils/test_kitti.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import platform 3 | from .kitti_stuff import Kitti 4 | 5 | if platform.system() == 'Darwin': 6 | root = '/Users/erikbohnsack/data' 7 | else: 8 | root = '/home/mlt/data' 9 | 10 | 11 | class TestKitti(unittest.TestCase): 12 | def test_imu(self): 13 | sequence_id = 0 14 | kitti = Kitti(root) 15 | imud = kitti.load_imu(sequence_id) 16 | print(imud) 17 | -------------------------------------------------------------------------------- /images/pmbm-ca-0016.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erikbohnsack/pmbm/b6e48462cca6c12f9fb425002515dc199658da14/images/pmbm-ca-0016.gif -------------------------------------------------------------------------------- /images/pmbm-ca-0020.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erikbohnsack/pmbm/b6e48462cca6c12f9fb425002515dc199658da14/images/pmbm-ca-0020.gif -------------------------------------------------------------------------------- /images/track_seq_0020_frame_0105.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erikbohnsack/pmbm/b6e48462cca6c12f9fb425002515dc199658da14/images/track_seq_0020_frame_0105.png -------------------------------------------------------------------------------- /pmbm/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erikbohnsack/pmbm/b6e48462cca6c12f9fb425002515dc199658da14/pmbm/.DS_Store -------------------------------------------------------------------------------- /pmbm/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erikbohnsack/pmbm/b6e48462cca6c12f9fb425002515dc199658da14/pmbm/__init__.py -------------------------------------------------------------------------------- /pmbm/config.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from utils import motion_models 3 | from utils import poisson_birth_state_models 4 | from utils.coord_transf import coordinate_transform_bicycle, coordinate_transform_CV, coordinate_transform_mixed, coordinate_transform_CA 5 | from .filter import LinearFilter, UKF, MixedFilter 6 | from filterpy.kalman import MerweScaledSigmaPoints 7 | 8 | 9 | class Config: 10 | def __init__(self, 11 | config_name='NA', 12 | detection_probability=0.9, 13 | survival_probability=0.9, 14 | prune_threshold_poisson=0.2, 15 | prune_threshold_global_hypo=-6, 16 | prune_threshold_targets=-6, 17 | prune_single_existence=1e-4, 18 | clutter_intensity=1e-4, 19 | poisson_merge_threshold=2, 20 | poisson_reduce_factor=0.1, 21 | uniform_weight=1, 22 | uniform_radius=100, 23 | uniform_angle=(0.78, 2.35), 24 | uniform_adjust=5., 25 | gating_distance=3, 26 | measurement_var_xy=1e-1, 27 | measurement_var_psi=0.5, 28 | birth_gating_distance=9, 29 | birth_weight=5e-3, 30 | global_init_weight=1, 31 | desired_nof_global_hypos=20, 32 | max_nof_global_hypos=25, 33 | min_new_nof_global_hypos=1, 34 | max_new_nof_global_hypos=10, 35 | motion_model='Bicycle', 36 | poisson_states_model_name='uniform', 37 | filter_class='UKF', 38 | bike_lr=0.5, 39 | bike_lf=0.75, 40 | car_lr=2, 41 | car_lf=2, 42 | pedestrian_lr=0.5, 43 | pedestrian_lf=0.5, 44 | tram_lr=10, 45 | tram_lf=2, 46 | truck_lr=10, 47 | truck_lf=2, 48 | van_lr=3, 49 | van_lf=2, 50 | dt=0.1, 51 | ukf_alpha=0.01, 52 | ukf_beta=4., 53 | ukf_kappa=1e5, 54 | sigma_cv=2, 55 | sigma_ca=2, 56 | sigma_xy_bicycle=0.5, 57 | sigma_v=1, 58 | sigma_d=0.01, 59 | sigma_phi=1, 60 | poisson_v=10, 61 | poisson_d=5, 62 | poisson_vx=10, 63 | poisson_vy=10, 64 | poisson_mean_v=0, 65 | poisson_mean_d=0, 66 | poisson_mean_vx=0, 67 | poisson_mean_vy=0, 68 | show_predictions=10, 69 | coord_transform=True, 70 | classes_to_track=['all']): 71 | 72 | self.name = config_name 73 | 74 | if motion_model == 'CV' or motion_model == 'CV-Kitti': 75 | self.measurement_model = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) 76 | self.measurement_noise = measurement_var_xy * np.eye(2) 77 | elif motion_model == 'Bicycle': 78 | self.measurement_model = np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, -1, 0, 0]]) 79 | self.measurement_noise = np.eye(3) 80 | self.measurement_noise[0, 0] = measurement_var_xy 81 | self.measurement_noise[1, 1] = measurement_var_xy 82 | self.measurement_noise[2, 2] = measurement_var_psi 83 | elif motion_model == 'CA': 84 | self.measurement_model = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]]) 85 | self.measurement_noise = measurement_var_xy * np.eye(2) 86 | elif motion_model == 'Mixed': 87 | temp_measurement_noise = np.eye(3) 88 | temp_measurement_noise[0, 0] = measurement_var_xy 89 | temp_measurement_noise[1, 1] = measurement_var_xy 90 | temp_measurement_noise[2, 2] = measurement_var_psi 91 | self.measurement_models = {'CV': np.array([[1, 0, 0, 0], [0, 1, 0, 0]]), 92 | 'Bicycle': np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, -1, 0, 0]])} 93 | self.measurement_noises = {'CV': measurement_var_xy * np.eye(2), 94 | 'Bicycle': temp_measurement_noise} 95 | self.measurement_model = None 96 | self.measurement_noise = None 97 | else: 98 | raise ValueError( 99 | 'No measurement model! Measurement model for Motion Model: {} not added '.format(motion_model)) 100 | 101 | self.detection_probability = detection_probability 102 | self.survival_probability = survival_probability 103 | self.prune_threshold_poisson = prune_threshold_poisson 104 | self.prune_threshold_global_hypo = prune_threshold_global_hypo 105 | self.prune_threshold_targets = prune_threshold_targets 106 | self.prune_single_existence = prune_single_existence 107 | self.gating_distance = gating_distance 108 | self.birth_gating_distance = birth_gating_distance 109 | self.birth_weight = birth_weight 110 | self.desired_nof_global_hypos = desired_nof_global_hypos 111 | self.max_nof_global_hypos = max_nof_global_hypos 112 | self.min_new_nof_global_hypos = min_new_nof_global_hypos 113 | self.max_new_nof_global_hypos = max_new_nof_global_hypos 114 | self.global_init_weight = global_init_weight 115 | self.clutter_intensity = clutter_intensity 116 | self.poisson_merge_threshold = poisson_merge_threshold 117 | self.poisson_reduce_factor = poisson_reduce_factor 118 | self.show_predictions = show_predictions 119 | self.dt = dt 120 | self.classes_to_track = classes_to_track 121 | self.motion_model_name = motion_model 122 | self.poisson_states_model_name = poisson_states_model_name 123 | self.filter_name = filter_class 124 | if motion_model == 'CV': 125 | self.state_dims = 4 126 | self.motion_model = motion_models.ConstantVelocityModel(motion_noise=0.5, dt=dt) 127 | self.motion_noise = self.motion_model.Q 128 | self.coord_transform = None 129 | 130 | elif motion_model == 'CV-Kitti': 131 | self.state_dims = 4 132 | self.motion_model = motion_models.ConstantVelocityModel(motion_noise=sigma_cv, dt=dt) 133 | self.motion_noise = self.motion_model.Q 134 | if coord_transform: 135 | self.coord_transform = coordinate_transform_CV 136 | else: 137 | self.coord_transform = None 138 | 139 | # CONSTANT ACCERLERATION 140 | elif motion_model == 'CA': 141 | self.state_dims = 6 142 | self.motion_model = motion_models.ConstantAccelerationModel(motion_noise=sigma_ca, dt=dt) 143 | self.motion_noise = self.motion_model.Q 144 | if coord_transform: 145 | self.coord_transform = coordinate_transform_CA 146 | else: 147 | self.coord_transform = None 148 | 149 | 150 | elif motion_model == 'LW2D': 151 | self.state_dims = 2 152 | self.motion_model = motion_models.LinearWalk2D(motion_noise=0.5) 153 | self.motion_noise = self.motion_model.Q 154 | self.coord_transform = None 155 | 156 | elif motion_model == 'Bicycle': 157 | self.state_dims = 5 158 | self.motion_model = motion_models.BicycleModel(dt=dt, 159 | bike_lr=bike_lr, bike_lf=bike_lf, 160 | car_lr=car_lr, car_lf=car_lf, 161 | pedestrian_lr=pedestrian_lr, pedestrian_lf=pedestrian_lf, 162 | tram_lr=tram_lr, tram_lf=tram_lf, 163 | truck_lr=truck_lr, truck_lf=truck_lf, 164 | van_lr=van_lr, van_lf=van_lf, 165 | sigma_xy_bicycle=sigma_xy_bicycle, 166 | sigma_phi=sigma_phi, 167 | sigma_v=sigma_v, 168 | sigma_d=sigma_d) 169 | self.motion_noise = self.motion_model.Q 170 | if coord_transform: 171 | self.coord_transform = coordinate_transform_bicycle 172 | else: 173 | self.coord_transform = None 174 | 175 | elif motion_model == 'Mixed': 176 | self.state_dims = 5 177 | self.motion_model = motion_models.MixedModel(dt=dt, motion_noise=2, 178 | bike_lr=bike_lr, bike_lf=bike_lf, 179 | car_lr=car_lr, car_lf=car_lf, 180 | pedestrian_lr=pedestrian_lr, pedestrian_lf=pedestrian_lf, 181 | tram_lr=tram_lr, tram_lf=tram_lf, 182 | truck_lr=truck_lr, truck_lf=truck_lf, 183 | van_lr=van_lr, van_lf=van_lf, 184 | sigma_xy_bicycle=sigma_xy_bicycle, 185 | sigma_phi=sigma_phi, 186 | sigma_v=sigma_v, 187 | sigma_d=sigma_d) 188 | if coord_transform: 189 | self.coord_transform = coordinate_transform_mixed 190 | else: 191 | self.coord_transform = None 192 | 193 | 194 | else: 195 | raise ValueError('Choose a motion model that exists you fool!') 196 | 197 | if poisson_states_model_name == 'uniform' or \ 198 | poisson_states_model_name == 'uniform-CV' or \ 199 | poisson_states_model_name == 'uniform-CA' or \ 200 | poisson_states_model_name == 'uniform-mixed': 201 | self.uniform_radius = uniform_radius 202 | self.uniform_weight = uniform_weight 203 | self.uniform_angle = uniform_angle 204 | self.uniform_adjust = uniform_adjust 205 | if motion_model == 'Mixed': 206 | uniform_covariance_cv = np.zeros((np.shape(self.measurement_models['CV'])[1], 207 | np.shape(self.measurement_models['CV'])[1])) 208 | uniform_covariance_bicycle = np.zeros((np.shape(self.measurement_models['Bicycle'])[1], 209 | np.shape(self.measurement_models['Bicycle'])[1])) 210 | 211 | uniform_covariance_cv[0:np.shape(self.measurement_noises['CV'])[0], 212 | 0:np.shape(self.measurement_noises['CV'])[0]] = self.measurement_noises['CV'] 213 | uniform_covariance_cv[-2, -2] = poisson_vx 214 | uniform_covariance_cv[-1, -1] = poisson_vy 215 | 216 | uniform_covariance_bicycle[0:np.shape(self.measurement_noises['Bicycle'])[0], 217 | 0:np.shape(self.measurement_noises['Bicycle'])[0]] = self.measurement_noises['Bicycle'] 218 | uniform_covariance_bicycle[-2, -2] = poisson_v 219 | uniform_covariance_bicycle[-1, -1] = poisson_d 220 | 221 | self.uniform_covariances = {'CV': uniform_covariance_cv, 222 | 'Bicycle': uniform_covariance_bicycle} 223 | self.unmeasurable_state_means = {'CV': np.array([[poisson_mean_vx], [poisson_mean_vy]]), 224 | 'Bicycle': np.array([[poisson_mean_v], [poisson_mean_d]])} 225 | 226 | else: 227 | self.uniform_covariance = np.zeros((self.state_dims, self.state_dims)) 228 | self.uniform_covariance[0:self.measurement_noise.shape[0], 229 | 0:self.measurement_noise.shape[1]] = self.measurement_noise 230 | if motion_model == 'CV' or motion_model == 'CV-Kitti': 231 | self.uniform_covariance[-2, -2] = poisson_vx 232 | self.uniform_covariance[-1, -1] = poisson_vy 233 | self.unmeasurable_state_mean = np.array([[poisson_mean_vx], [poisson_mean_vy]]) 234 | elif motion_model == 'CA': 235 | self.uniform_covariance[-4, -4] = poisson_vx 236 | self.uniform_covariance[-3, -3] = poisson_vy 237 | self.uniform_covariance[-2, -2] = poisson_vx 238 | self.uniform_covariance[-1, -1] = poisson_vy 239 | self.unmeasurable_state_mean = np.array([[poisson_mean_vx], [poisson_mean_vy], 240 | [poisson_mean_vx], [poisson_mean_vy]]) 241 | else: 242 | self.uniform_covariance[-2, -2] = poisson_v 243 | self.uniform_covariance[-1, -1] = poisson_d 244 | self.unmeasurable_state_mean = np.array([[poisson_mean_v], [poisson_mean_d]]) 245 | else: 246 | self.uniform_radius = 0 247 | self.uniform_weight = 0 248 | self.uniform_adjust = 0 249 | self.uniform_angle = (0, 0) 250 | self.uniform_covariance = None 251 | self.unmeasurable_state_mean = None 252 | 253 | self.poisson_birth_state, self.poisson_birth_var = poisson_birth_state_models.get_model( 254 | model_name=poisson_states_model_name) 255 | 256 | if filter_class == 'Linear': 257 | self.filt = LinearFilter() 258 | elif filter_class == 'UKF': 259 | points = MerweScaledSigmaPoints(self.state_dims, alpha=ukf_alpha, beta=ukf_beta, kappa=ukf_kappa) 260 | self.filt = UKF(dt=dt, dim_x=self.state_dims, dim_z=len(self.measurement_model[1]), points=points, 261 | fx=self.motion_model, hx=self.measurement_model) 262 | elif filter_class == 'Mixed': 263 | points = MerweScaledSigmaPoints(self.state_dims, alpha=ukf_alpha, beta=ukf_beta, kappa=ukf_kappa) 264 | self.filt = MixedFilter(dt=dt, dim_x=self.state_dims, dim_z=len(self.measurement_models['Bicycle'][1]), 265 | points=points, fx=self.motion_model, hx=self.measurement_models['Bicycle']) 266 | else: 267 | raise ValueError('Choose a filter that exists you fool!') 268 | 269 | if self.poisson_birth_state: 270 | assert np.shape(self.poisson_birth_state)[1] == self.state_dims, \ 271 | 'State dimension missmatch. Have: {}, Want: {}'.format(np.shape(self.poisson_birth_state)[1], 272 | self.state_dims) 273 | -------------------------------------------------------------------------------- /pmbm/filter.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from utils.UKF import UnscentedKalmanFilter 3 | from scipy.stats import multivariate_normal 4 | 5 | 6 | class LinearFilter: 7 | def __init__(self): 8 | pass 9 | 10 | def predict(self, state, variance, motion_model, motion_noise=None, object_class=None): 11 | assert state.shape[1] == 1, "State is not a column vector" 12 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 13 | 14 | _state, _variance = motion_model(state=state, variance=variance, object_class=object_class) 15 | 16 | # For numerical stability 17 | _variance = 0.5 * (_variance + _variance.transpose()) 18 | return _state, _variance 19 | 20 | def update(self, state, variance, measurement, measurement_model, measurement_noise, object_class=None): 21 | assert state.shape[1] == 1, "State is not a column vector" 22 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 23 | 24 | # Else we update all the single target hypotheses 25 | # S = H * P * H' + R 26 | _S = measurement_model @ variance @ measurement_model.transpose() + measurement_noise 27 | # For numerical stability 28 | _S = 0.5 * (_S + _S.transpose()) 29 | # W = P * H' 30 | _W = variance @ measurement_model.transpose() 31 | # K = W * inv(S) 32 | _K = _W @ np.linalg.inv(_S) 33 | # P = P - K 34 | _P = variance - _K @ _W.transpose() 35 | # For numerical stability 36 | _P = 0.5 * (_P + _P.transpose()) 37 | 38 | _v = measurement - measurement_model @ state 39 | 40 | _state = state + _K @ _v 41 | _variance = _P 42 | 43 | reshaped_meas = measurement.reshape(np.shape(measurement_model)[0], ) 44 | reshaped_mean = (measurement_model @ _state).reshape(np.shape(measurement_model)[0], ) 45 | 46 | meas_likelihood = multivariate_normal.pdf(reshaped_meas, reshaped_mean, _S) 47 | return _state, _variance, meas_likelihood 48 | 49 | 50 | class UKF(UnscentedKalmanFilter): 51 | def __init__(self, dt, dim_x, dim_z, points, fx, hx, residual_z=None, residual_x=None, sqrt_fn=None): 52 | super().__init__(dt, dim_x, dim_z, points, fx, hx, residual_z=residual_z, 53 | residual_x=residual_x, sqrt_fn=sqrt_fn) 54 | 55 | def predict(self, state, variance, motion_model, motion_noise, object_class, dt=None, UT=None): 56 | assert state.shape[0] == self._dim_x, "Mismatch in state cardinality" 57 | assert state.shape[1] == 1, "State is not a column vector" 58 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 59 | _state, _variance = super().predict(state.reshape(len(state),), variance, motion_model, motion_noise, 60 | object_class, dt=dt, UT=UT) 61 | _variance = 0.5 * (_variance + _variance.transpose()) 62 | return _state, _variance 63 | 64 | def update(self, state, variance, measurement, measurement_model, measurement_noise, oject_class=None, UT=None): 65 | assert state.shape[0] == self._dim_x, "Mismatch in state cardinality" 66 | assert state.shape[1] == 1, "State is not a column vector" 67 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 68 | _state, _variance = super().update(state.reshape(len(state),), variance, measurement, measurement_model, 69 | measurement_noise, UT=UT) 70 | reshaped_meas = measurement.reshape(np.shape(measurement_model)[0], ) 71 | reshaped_mean = (measurement_model @ state).reshape(np.shape(measurement_model)[0], ) 72 | _S = measurement_model @ variance @ measurement_model.transpose() + measurement_noise # Input variance here 73 | _S = 0.5 * (_S + _S.transpose()) 74 | meas_likelihood = multivariate_normal.pdf(reshaped_meas, reshaped_mean, _S) 75 | _variance = 0.5 * (_variance + _variance.transpose()) 76 | return _state, _variance, meas_likelihood 77 | 78 | 79 | class MixedFilter(): 80 | def __init__(self, dt, dim_x, dim_z, points, fx, hx, residual_z=None, residual_x=None, sqrt_fn=None): 81 | self.LF = LinearFilter() 82 | self.UKF = UKF(dt=dt, dim_x=dim_x, dim_z=dim_z, points=points, fx=fx, hx=hx, 83 | residual_z=residual_z, residual_x=residual_x, sqrt_fn=sqrt_fn) 84 | 85 | def predict(self, state, variance, motion_model, motion_noise, object_class, dt=None, UT=None): 86 | if object_class == 'Pedestrian' or object_class == 'Misc' or object_class == 'Person': 87 | _state, _variance = self.LF.predict(state=state, variance=variance, motion_model=motion_model, 88 | object_class=object_class) 89 | else: 90 | _state, _variance = self.UKF.predict(state=state, variance=variance, motion_model=motion_model, 91 | motion_noise=motion_noise, object_class=object_class, dt=dt, UT=UT) 92 | return _state, _variance 93 | 94 | def update(self, state, variance, measurement, measurement_model, measurement_noise, object_class, UT=None): 95 | if object_class == 'Pedestrian' or object_class == 'Misc' or object_class == 'Person': 96 | _state, _variance, meas_likelihood = self.LF.update(state=state, variance=variance, 97 | measurement=measurement, 98 | measurement_model=measurement_model, 99 | measurement_noise=measurement_noise) 100 | else: 101 | _state, _variance, meas_likelihood = self.UKF.update(state=state, variance=variance, 102 | measurement=measurement, 103 | measurement_model=measurement_model, 104 | measurement_noise=measurement_noise, UT=UT) 105 | return _state, _variance, meas_likelihood 106 | 107 | -------------------------------------------------------------------------------- /pmbm/global_hypothesis.py: -------------------------------------------------------------------------------- 1 | class GlobalHypothesis: 2 | def __init__(self, weight, hypothesis): 3 | self.weight = weight 4 | self.hypothesis = hypothesis 5 | 6 | def __repr__(self): 7 | return '\n'.format( 8 | self.weight, self.hypothesis) 9 | -------------------------------------------------------------------------------- /pmbm/poisson.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from math import atan2, sqrt 3 | import matplotlib.pyplot as plt 4 | import scipy.stats as stats 5 | from scipy.spatial.distance import cdist 6 | from utils.moment_matching import moment_matching_dists 7 | from utils.constants import XLIM, ZLIM 8 | 9 | 10 | class Distribution: 11 | def __init__(self, state, variance, weight, object_class, motion_model): 12 | assert state.shape[1] == 1, "Input state is not column vector" 13 | assert state.shape[0] == variance.shape[0], "State vector not aligned with Covariance matrix" 14 | self.state_dim = state.shape[0] 15 | self.state = state 16 | self.variance = variance 17 | self.weight = weight 18 | self.object_class = object_class 19 | self.motion_model = motion_model 20 | self.motion_noise = motion_model.get_Q(self.object_class) 21 | 22 | def predict(self, filt, survival_probability): 23 | if self.motion_model.model == 0: 24 | velocity = self.state[3] 25 | elif self.motion_model.model == 1: 26 | velocity = np.linalg.norm(self.state[2:4, :]) 27 | else: 28 | velocity = 0 29 | if velocity != 0: 30 | _state, _variance = filt.predict(self.state, self.variance, self.motion_model, self.motion_noise, self.object_class) 31 | self.state = _state 32 | self.variance = _variance 33 | self.weight *= survival_probability 34 | 35 | def update(self, detection_probability): 36 | self.weight *= detection_probability 37 | 38 | def __repr__(self): 39 | return ' \n'.format(self.weight, self.state, self.variance) 41 | 42 | 43 | class Poisson: 44 | """ 45 | Class to hold all poisson distributions. Methods include birth, prediction, merge, prune, recycle. 46 | 47 | :param birth_state: Where to birth new poisson distributions 48 | :param birth_var: What covariances the new poissons should have. 49 | :param birth_weight_factor: Weight of the new distributions 50 | :param prune_threshold: Which weight threshold for pruning 51 | :param merge_threshold: Which distance threshold for merge of poisson distributions 52 | :param reduce_factor: How much should the weight be reduced for each timestep 53 | :return: 54 | """ 55 | 56 | def __init__(self, 57 | birth_state, 58 | birth_var, 59 | birth_weight, 60 | prune_threshold, 61 | merge_threshold, 62 | reduce_factor, 63 | uniform_weight, 64 | uniform_radius, 65 | uniform_angle, 66 | uniform_adjust, 67 | state_dim): 68 | 69 | self.birth_weight = birth_weight 70 | if birth_state is None: 71 | self.number_of_births = 0 72 | else: 73 | self.number_of_births = len(birth_state) 74 | 75 | self.state_dim = state_dim 76 | self.distributions = [] 77 | 78 | self.birth_state = birth_state 79 | self.birth_var = birth_var 80 | self.prune_threshold = prune_threshold 81 | self.merge_threshold = merge_threshold 82 | self.reduce_factor = reduce_factor 83 | 84 | self.uniform_radius = uniform_radius 85 | self.uniform_angle = uniform_angle # Tuple with min, max in radius 86 | self.uniform_adjust = uniform_adjust 87 | self.uniform_weight = uniform_weight 88 | self.uniform_area = 0.5 * uniform_radius ** 2 * (uniform_angle[1] - uniform_angle[0]) 89 | 90 | # Plot thingies 91 | self.x_range = [-50, 150] 92 | self.y_range = [-50, 150] 93 | self.window_min = np.array([self.x_range[0], self.y_range[0]]) 94 | self.window_size = np.array([self.x_range[1] - self.x_range[0], self.y_range[1] - self.y_range[0]]) 95 | self.window_intensity = np.prod(self.window_size) 96 | 97 | self.grid_length = [200, 200] 98 | self.x_dim = np.linspace(self.x_range[0], self.x_range[1], self.grid_length[0]) 99 | self.y_dim = np.linspace(self.y_range[0], self.y_range[1], self.grid_length[1]) 100 | self.x_mesh, self.y_mesh = np.meshgrid(self.x_dim, self.y_dim) 101 | self.grid = np.dstack((self.x_mesh, self.y_mesh)) 102 | self.intensity = np.zeros((self.grid_length[1], self.grid_length[0])) 103 | 104 | def __repr__(self): 105 | return ''.format(self.distributions) 106 | 107 | def give_birth(self): 108 | for i in range(self.number_of_births): 109 | self.distributions.append(Distribution(self.birth_state[i], self.birth_var, self.birth_weight)) 110 | 111 | def predict(self, filt, survival_probability): 112 | for dist in self.distributions: 113 | dist.predict(filt, survival_probability) 114 | self.give_birth() 115 | 116 | def update(self, detection_probability): 117 | for dist in self.distributions: 118 | dist.update(detection_probability) 119 | 120 | def prune(self): 121 | self.distributions[:] = [distribution for distribution in self.distributions if 122 | distribution.weight > self.prune_threshold] 123 | 124 | def within_uniform(self, point): 125 | angle = atan2(point[1] + self.uniform_adjust, point[0]) 126 | radius = sqrt(point[0]**2 + point[1]**2) 127 | return self.uniform_angle[0] < angle < self.uniform_angle[1] and radius < self.uniform_radius 128 | 129 | def merge(self): 130 | _new_distributions = [] 131 | while self.distributions: 132 | if len(self.distributions) == 1: 133 | _new_distributions.append(self.distributions[0]) 134 | del self.distributions[0] 135 | 136 | else: 137 | self.distributions.sort(key=lambda x: x.weight, reverse=True) 138 | cdistr = self.distributions[0] 139 | states = np.array([distr.state.reshape(cdistr.state_dim, ) 140 | for index, distr in enumerate(self.distributions) 141 | if (index != 0 and distr.object_class == cdistr.object_class)]) 142 | indices = [index 143 | for index, distr in enumerate(self.distributions) 144 | if (index != 0 and distr.object_class == cdistr.object_class)] 145 | 146 | if len(states) > 0: 147 | distance = list(cdist(cdistr.state.reshape(1, cdistr.state_dim), 148 | states.reshape(len(states), cdistr.state_dim), metric='mahalanobis', 149 | VI=np.linalg.inv(cdistr.variance))[0]) 150 | 151 | indices_within_threshold = [index_value for counter, index_value in enumerate(indices) 152 | if distance[counter] < self.merge_threshold] 153 | else: 154 | indices_within_threshold = False 155 | 156 | if indices_within_threshold: 157 | indices_within_threshold.append(0) 158 | merge_list = [distr for index, distr in enumerate(self.distributions) 159 | if index in indices_within_threshold] 160 | merge_weights = [distr.weight for index, distr in enumerate(self.distributions) 161 | if index in indices_within_threshold] 162 | 163 | # Merge with the ones that are inside of threshold 164 | _state, _variance, _weight = moment_matching_dists(merge_list, merge_weights) 165 | _new_distributions.append(Distribution(state=_state, 166 | variance=_variance, 167 | weight=_weight, 168 | object_class=cdistr.object_class, 169 | motion_model=cdistr.motion_model)) 170 | 171 | # Remove the merged ones from the distribution list. 172 | for i in sorted(indices_within_threshold, reverse=True): 173 | del self.distributions[i] 174 | else: 175 | _new_distributions.append(self.distributions[0]) 176 | del self.distributions[0] 177 | 178 | # When the old distribution list is empty, overwrite with new. 179 | self.distributions = _new_distributions 180 | 181 | def recycle(self, bernoulli, motion_model, object_class): 182 | # TODO: Change to existence instead of birth weight factor? 183 | _distr = Distribution(state=bernoulli.state, 184 | variance=bernoulli.variance, 185 | weight=bernoulli.existence, 186 | object_class=object_class, 187 | motion_model=motion_model) 188 | self.distributions.append(_distr) 189 | 190 | def reduce_weight(self, index): 191 | self.distributions[index].weight *= self.reduce_factor 192 | 193 | def plot(self, measurement_model): 194 | self.compute_intensity(measurement_model) 195 | plt.figure() 196 | for counter, distribution in enumerate(self.distributions): 197 | plt.plot(distribution.state[0], distribution.state[1], 'bo', 198 | markersize=self.distributions[counter].weight * 10) 199 | plt.title("Poisson Distribution") 200 | plt.contourf(self.x_dim, self.y_dim, self.intensity, alpha=0.5) 201 | plt.xlim(XLIM[0], XLIM[1]) 202 | plt.ylim(ZLIM[0], ZLIM[1]) 203 | plt.show() 204 | 205 | def compute_intensity(self, measurement_model): 206 | self.reset_intensity() 207 | # increase intensity based on distributions in PPP 208 | for counter, distribution in enumerate(self.distributions): 209 | measurable_states = measurement_model @ distribution.state 210 | measurable_variance = measurement_model @ distribution.variance @ measurement_model.T 211 | _rv = stats.multivariate_normal(mean=measurable_states.reshape(np.shape(measurement_model)[0]), 212 | cov=measurable_variance) 213 | sampled_pdf = _rv.pdf(self.grid) * self.distributions[counter].weight 214 | self.intensity += sampled_pdf 215 | 216 | def reset_intensity(self): 217 | self.intensity = np.zeros((self.grid_length[1], self.grid_length[0])) 218 | -------------------------------------------------------------------------------- /pmbm/single_target_hypothesis.py: -------------------------------------------------------------------------------- 1 | from utils.constants import LARGE 2 | 3 | 4 | class SingleTargetHypothesis: 5 | def __init__(self, measurement_index, state, variance, existence, weight, time_of_birth, single_cost=LARGE): 6 | assert state.shape[1] == 1, "Input state is not column vector" 7 | assert state.shape[0] == variance.shape[0], "State vector not aligned with Covariance matrix" 8 | assert isinstance(weight, float), "Input weight not a float. Current type: {}".format(type(weight)) 9 | 10 | self.state = state 11 | self.variance = variance 12 | self.existence = existence 13 | self.weight = weight 14 | self.single_cost = single_cost 15 | self.measurement_index = measurement_index 16 | self.children = [] 17 | self.time_of_birth = time_of_birth 18 | 19 | def __repr__(self): 20 | if self.state.shape[0] == 5: 21 | return '\t\n'.format( 22 | self.time_of_birth, 23 | round(self.weight, 2), 24 | round(self.single_cost, 2), 25 | round(self.existence, 2), 26 | self.measurement_index, 27 | (round(float(self.state[0]), 2), round(float(self.state[1]), 2), round(float(self.state[2]), 2), 28 | round(float(self.state[3]), 2), round(float(self.state[4]), 2))) 29 | else: 30 | return '\t\n'.format( 31 | self.time_of_birth, 32 | round(self.weight, 2), 33 | round(self.single_cost, 2), 34 | round(self.existence, 2), 35 | self.measurement_index, 36 | (round(float(self.state[0]), 2), round(float(self.state[1]), 2), round(float(self.state[2]), 2), 37 | round(float(self.state[3]), 2))) 38 | -------------------------------------------------------------------------------- /pmbm/target.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from .single_target_hypothesis import SingleTargetHypothesis 3 | from scipy.spatial.distance import cdist 4 | 5 | 6 | class Target: 7 | def __init__(self, 8 | time_of_birth, 9 | state, 10 | variance, 11 | weight, 12 | existence, 13 | measurement_index, 14 | motion_model, 15 | object_class, 16 | survival_probability, 17 | detection_probability, 18 | measurement_model, 19 | measurement_noise, 20 | gating_distance, 21 | verbose=False): 22 | 23 | self.single_target_hypotheses = {0: SingleTargetHypothesis(measurement_index=measurement_index, 24 | state=state, 25 | variance=variance, 26 | weight=weight, 27 | existence=existence, 28 | time_of_birth=time_of_birth)} 29 | self.next_single_id = 1 30 | self.time_of_birth = time_of_birth 31 | self.object_class = object_class 32 | self.motion_model = motion_model 33 | self.motion_noise = motion_model.get_Q(object_class) 34 | self.measurement_model = measurement_model 35 | self.measurement_noise = measurement_noise 36 | self.measurement_dims = np.shape(measurement_model)[0] 37 | self.survival_probability = survival_probability 38 | self.detection_probability = detection_probability 39 | self.gating_distance = gating_distance 40 | 41 | self.target_weight = None 42 | 43 | self.verbose = verbose 44 | 45 | def __repr__(self): 46 | return ' self.gating_distance: 130 | continue 131 | 132 | detected_state, detected_variance, meas_likelihood = filt.update(node.state, node.variance, 133 | measurement, 134 | self.measurement_model, 135 | self.measurement_noise, 136 | self.object_class) 137 | detected_existence = 1 138 | 139 | detected_weight = node.weight + np.log(node.existence * self.detection_probability * meas_likelihood) 140 | single_cost = detected_weight - missed_weight 141 | 142 | detected_node = SingleTargetHypothesis(measurement_index=j, 143 | state=detected_state, 144 | variance=detected_variance, 145 | existence=detected_existence, 146 | weight=detected_weight, 147 | time_of_birth=current_time, 148 | single_cost=single_cost) 149 | 150 | output_nodes[self.next_single_id] = detected_node 151 | _children.append(self.next_single_id) 152 | self.next_single_id += 1 153 | self.single_target_hypotheses[single_id].children = _children 154 | 155 | self.single_target_hypotheses.update(output_nodes) 156 | 157 | def remove_single_hypos(self, single_ids): 158 | for single_id in single_ids: 159 | del self.single_target_hypotheses[single_id] 160 | 161 | -------------------------------------------------------------------------------- /pmbm/test_pmbm.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | from .pmbm import PMBM, Settings 4 | from .target import Target 5 | from utils.constants import LARGE 6 | from .global_hypothesis import GlobalHypothesis 7 | 8 | # Test Target 9 | 10 | target_id = 0 11 | time_of_birth = 0 12 | state = np.array([[-2], [-2]]) 13 | variance = np.eye(2) 14 | measurement_index = 0 15 | motion_model = np.eye(2) 16 | survival_probability = 0.5 17 | detection_probability = 0.5 18 | measurement_model = np.eye(2) 19 | measurement_noise = np.eye(2) 20 | gating_distance = 2 21 | 22 | 23 | class TestPMBM(unittest.TestCase): 24 | def test_possible_new_targets(self): 25 | pass # possible_new_targets(self, measurements): 26 | 27 | def test_cost_new_targets(self): 28 | max_nof_global_hypos = 2 29 | 30 | measurement_model = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) 31 | measurement_noise = 0.1 * np.eye(2) 32 | settings = Settings(detection_probability=0.95, 33 | survival_probability=0.95, 34 | prune_threshold_poisson=0, 35 | prune_threshold_targets=0, 36 | gating_distance=15, 37 | birth_gating_distance=15, 38 | max_nof_global_hypos=max_nof_global_hypos, 39 | motion_model='CV', 40 | motion_noise=1, 41 | measurement_model=measurement_model, 42 | measurement_noise=measurement_noise) 43 | pmbm = PMBM(settings) 44 | measurements = [np.array([[-45], [-45]]), np.array([[-45], [45]])] 45 | nof_measurements = len(measurements) 46 | 47 | # NO NEW TARGETS. 48 | W_nt, new_target_map, new_measurement_map_meas2row, new_measurement_map_row2meas = pmbm.new_targets_cost( 49 | measurements) 50 | assert (W_nt == np.full((nof_measurements, nof_measurements), LARGE)).all() 51 | assert new_target_map == {} 52 | assert new_measurement_map_row2meas == {} 53 | assert new_measurement_map_meas2row == {} 54 | 55 | # NEW TARGET 56 | pmbm.current_time = 10 57 | 58 | jx = 1 59 | _state = np.array([[-45], [45]]) 60 | _variance = np.eye(2) 61 | _weight = 10. 62 | _existence = 1 63 | 64 | target_id = 100 65 | target = Target(measurement_index=jx, time_of_birth=pmbm.current_time, 66 | state=_state, 67 | variance=_variance, 68 | weight=_weight, 69 | existence=_existence, 70 | motion_model=pmbm.motion_model, 71 | survival_probability=pmbm.survival_probability, 72 | detection_probability=pmbm.detection_probability, 73 | measurement_model=pmbm.measurement_model, 74 | measurement_noise=pmbm.measurement_noise, 75 | gating_distance=pmbm.gating_distance) 76 | 77 | pmbm.targets[target_id] = target 78 | pmbm.new_targets = [target_id] 79 | W_nt, new_target_map, new_measurement_map_meas2row, new_measurement_map_row2meas = pmbm.new_targets_cost( 80 | measurements) 81 | assert W_nt[0, 0] == - _weight 82 | assert new_target_map[target_id] == 0 83 | assert new_measurement_map_meas2row[jx] == 0 84 | assert new_measurement_map_row2meas[0] == jx 85 | 86 | def test_cap_global_hypos(self): 87 | max_nof_global_hypos = 2 88 | 89 | measurement_model = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) 90 | measurement_noise = 0.1 * np.eye(2) 91 | 92 | pmbm = PMBM(state_dims=4, 93 | detection_probability=0.95, 94 | survival_probability=0.95, 95 | prune_threshold_poisson=0, 96 | prune_threshold_targets=0, 97 | gating_distance=15, 98 | birth_gating_distance=15, 99 | max_nof_global_hypos=max_nof_global_hypos, 100 | motion_model='CV', 101 | motion_noise=1, 102 | measurement_model=measurement_model, 103 | measurement_noise=measurement_noise) 104 | 105 | pmbm.global_hypotheses[1] = GlobalHypothesis(1.0, []) 106 | pmbm.global_hypotheses[2] = GlobalHypothesis(0.8, []) 107 | pmbm.global_hypotheses[3] = GlobalHypothesis(0.5, []) 108 | pmbm.global_hypotheses[4] = GlobalHypothesis(1.2, []) 109 | 110 | pmbm.cap_global_hypos() 111 | 112 | assert len(pmbm.global_hypotheses) == max_nof_global_hypos, "Capping failed" 113 | assert pmbm.global_hypotheses[1], "Global hypo with key = 1 shouldn't have been removed" 114 | assert pmbm.global_hypotheses[4], "Global hypo with key = 4 shouldn't have been removed" 115 | 116 | def test_prune_global_hypo(self): 117 | pmbm = PMBM(prune_threshold_global_hypo=2, motion_model='CV') 118 | pmbm.global_hypotheses = {0: GlobalHypothesis(weight=1, hypothesis=None), 119 | 1: GlobalHypothesis(weight=3, hypothesis=None)} 120 | assert len(pmbm.global_hypotheses) == 2 121 | pmbm.prune_global() 122 | assert len(pmbm.global_hypotheses) == 1 123 | 124 | def test_prune_targets(self): 125 | pmbm = PMBM(prune_threshold_targets=3, motion_model='CV') 126 | pmbm.targets = {0: Target(time_of_birth=0, 127 | state=np.array([[1], [1]]), 128 | variance=np.eye(2), 129 | weight=1., 130 | existence=1, 131 | measurement_index=1, 132 | motion_model=1, 133 | survival_probability=1, 134 | detection_probability=1, 135 | measurement_model=1, 136 | measurement_noise=1, 137 | gating_distance=1, 138 | verbose=False), 139 | 1: Target(time_of_birth, 140 | state=np.array([[1], [1]]), 141 | variance=np.eye(2), 142 | weight=1., 143 | existence=0.5, 144 | measurement_index=1, 145 | motion_model=1, 146 | survival_probability=1, 147 | detection_probability=1, 148 | measurement_model=1, 149 | measurement_noise=1, 150 | gating_distance=1, 151 | verbose=False)} 152 | pmbm.global_hypotheses = {0: GlobalHypothesis(weight=1, hypothesis=[(0, 0)]), 153 | 1: GlobalHypothesis(weight=10, hypothesis=[(1, 0)]), 154 | 2: GlobalHypothesis(weight=1, hypothesis=[(0, 0), (1, 0)])} 155 | 156 | assert len(pmbm.targets) == 2 157 | pmbm.recycle_targets() 158 | assert len(pmbm.targets) == 1 159 | 160 | def test_recycling(self): 161 | init_state = np.array([[1], [1]]) 162 | init_cov = np.eye(2) 163 | pmbm = PMBM(prune_threshold_targets=3, motion_model='CV') 164 | pmbm.targets = {0: Target(time_of_birth=0, 165 | state=init_state, 166 | variance=init_cov, 167 | weight=1., 168 | existence=1, 169 | measurement_index=1, 170 | motion_model=1, 171 | survival_probability=1, 172 | detection_probability=1, 173 | measurement_model=1, 174 | measurement_noise=1, 175 | gating_distance=1, 176 | verbose=False), 177 | 1: Target(time_of_birth, 178 | state=init_state, 179 | variance=init_cov, 180 | weight=1., 181 | existence=0.5, 182 | measurement_index=1, 183 | motion_model=1, 184 | survival_probability=1, 185 | detection_probability=1, 186 | measurement_model=1, 187 | measurement_noise=1, 188 | gating_distance=1, 189 | verbose=False)} 190 | pmbm.global_hypotheses = {0: GlobalHypothesis(weight=1, hypothesis=[(0, 0)]), 191 | 1: GlobalHypothesis(weight=10, hypothesis=[(1, 0)]), 192 | 2: GlobalHypothesis(weight=1, hypothesis=[(0, 0), (1, 0)])} 193 | 194 | assert len(pmbm.poisson.distributions) == 0 195 | pmbm.recycle_targets() 196 | assert len(pmbm.poisson.distributions) == 1 197 | assert (pmbm.poisson.distributions[0].state == init_state).all() 198 | assert (pmbm.poisson.distributions[0].variance == init_cov).all() 199 | 200 | 201 | if __name__ == '__main__': 202 | unittest.main() 203 | -------------------------------------------------------------------------------- /pmbm/test_poisson.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | from .poisson import Distribution, Poisson 4 | from utils import motion_models 5 | 6 | 7 | class TestDistribution(unittest.TestCase): 8 | def test_predict_2D(self): 9 | init_state = np.random.rand(2, 1) 10 | init_variance = np.random.rand(2, 2) 11 | init_weight = 1 12 | distribution = Distribution(init_state, init_variance, init_weight) 13 | Q = np.matrix('1, 0 ; 0, 1') 14 | motion_model = motion_models.LinearWalk2D(motion_factor=1, motion_noise=Q) 15 | survival_probability = 0.9 16 | distribution.predict(motion_model, survival_probability) 17 | self.assertTrue((init_state == distribution.state).all()) 18 | self.assertTrue((init_variance + Q == distribution.variance).all()) 19 | 20 | 21 | class TestPoisson(unittest.TestCase): 22 | def test_give_birth(self): 23 | poisson = Poisson(birth_var=np.eye(2), birth_state=[np.array([[-2], [-2]]), np.array([[2], [2]]), np.array([[-4], [-4]]), 24 | np.array([[4], [4]])],) 25 | self.assertTrue(len(poisson.distributions) == 0) 26 | poisson.give_birth() 27 | self.assertTrue(len(poisson.distributions) == 4) 28 | 29 | def test_predict(self): 30 | poisson = Poisson(birth_var=np.eye(2), birth_state=[np.array([[-2], [-2]]), np.array([[2], [2]]), np.array([[-4], [-4]]), 31 | np.array([[4], [4]])]) 32 | Q = np.matrix('1, 0 ; 0, 1') 33 | motion_model = motion_models.LinearWalk2D(motion_factor=1, motion_noise=Q) 34 | 35 | survival_probability = 0.8 36 | poisson.predict(motion_model, survival_probability) 37 | self.assertTrue(len(poisson.distributions) == 4) 38 | poisson.predict(motion_model, survival_probability) 39 | self.assertTrue(len(poisson.distributions) == 8) 40 | self.assertTrue((poisson.distributions[1].state == motion_model(poisson.distributions[5].state, Q)[0]).all()) 41 | self.assertTrue((poisson.distributions[0].state == motion_model(poisson.distributions[4].state, Q)[0]).all()) 42 | 43 | def test_merge(self): 44 | state1 = np.array([[-2.], [-2.]]) 45 | state2 = np.array([[2.], [2.]]) 46 | state3 = np.array([[1.], [2.]]) 47 | cov = np.eye(2) 48 | distr1 = Distribution(state1, cov, 1.) 49 | distr2 = Distribution(state2, cov, 2.) 50 | distr3 = Distribution(state3, cov, 3.) 51 | poisson = Poisson(birth_state=np.random.rand(2, 1), 52 | birth_var=np.eye(2), 53 | birth_weight_factor=1, 54 | prune_threshold=0.1, 55 | merge_threshold=2, 56 | reduce_factor=0.1) 57 | poisson.distributions = [distr1, distr2, distr3] 58 | 59 | assert len(poisson.distributions) == 3 60 | poisson.merge() 61 | assert len(poisson.distributions) == 2 62 | poisson.merge() 63 | assert len(poisson.distributions) == 2 64 | poisson.merge_threshold = 10 65 | poisson.merge() 66 | assert len(poisson.distributions) == 1 67 | 68 | 69 | if __name__ == '__main__': 70 | unittest.main() 71 | -------------------------------------------------------------------------------- /pmbm/test_target.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | from .target import Target 4 | 5 | target_id = 0 6 | time_of_birth = 0 7 | state = np.array([[-2], [-2]]) 8 | variance = np.eye(2) 9 | measurement_index = 0 10 | motion_model = np.eye(2) 11 | survival_probability = 0.5 12 | detection_probability = 0.5 13 | measurement_model = np.eye(2) 14 | measurement_noise = np.eye(2) 15 | gating_distance = 2 16 | 17 | 18 | class TestTarget(unittest.TestCase): 19 | def test_hypo_addition(self): 20 | target = Target(target_id, time_of_birth, state, variance, measurement_index, motion_model, 21 | survival_probability, detection_probability, measurement_model, 22 | measurement_noise, gating_distance) 23 | assert(len(target.single_target_hypotheses) == 1) 24 | 25 | def test_len_new_hypos(self): 26 | target = Target(target_id, time_of_birth, state, variance, measurement_index, motion_model, 27 | survival_probability, detection_probability, measurement_model, 28 | measurement_noise, gating_distance) 29 | measurements = [np.array([[-2], [-2]]), np.array([[100], [100]])] 30 | assert len(target.single_target_hypotheses) == 1 31 | target.new_hypos(measurements, 1) 32 | assert len(target.single_target_hypotheses) == 2 33 | target.new_hypos(measurements, 2) 34 | assert len(target.single_target_hypotheses) == 4 35 | -------------------------------------------------------------------------------- /runforrest.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib inline\n", 10 | "from data_utils import kitti_stuff\n", 11 | "from data_utils.fafe_stuff import FafeDetections, FafeMeasurements\n", 12 | "from utils import environments\n", 13 | "from utils import logger\n", 14 | "from utils import plot_stuff\n", 15 | "from pmbm import pmbm\n", 16 | "from pmbm.config import Config\n", 17 | "from utils import constants\n", 18 | "import numpy as np\n", 19 | "import time, datetime\n", 20 | "import os\n", 21 | "import sys\n", 22 | "sys.path.append('/home/mlt/mot/fafe')" 23 | ] 24 | }, 25 | { 26 | "cell_type": "markdown", 27 | "metadata": {}, 28 | "source": [ 29 | "Start pmbm engine" 30 | ] 31 | }, 32 | { 33 | "cell_type": "code", 34 | "execution_count": null, 35 | "metadata": {}, 36 | "outputs": [], 37 | "source": [ 38 | "config_fafe = Config(config_name='CV-Fafe',\n", 39 | " motion_model='CV-Kitti',\n", 40 | " poisson_states_model_name='uniform-CV',\n", 41 | " filter_class='Linear',\n", 42 | " measurement_var_xy=1,\n", 43 | " detection_probability=0.92,\n", 44 | " survival_probability=0.9,\n", 45 | " max_nof_global_hypos=100,\n", 46 | " prune_threshold_global_hypo=-4,\n", 47 | " prune_threshold_targets=-4,\n", 48 | " prune_single_existence=1e-3,\n", 49 | " clutter_intensity=1e-2,\n", 50 | " gating_distance=4,\n", 51 | " birth_gating_distance=4,\n", 52 | " uniform_weight=1,\n", 53 | " poisson_vx=10,\n", 54 | " poisson_vy=10,\n", 55 | " sigma_v=5)\n", 56 | "\n" 57 | ] 58 | }, 59 | { 60 | "cell_type": "code", 61 | "execution_count": null, 62 | "metadata": {}, 63 | "outputs": [], 64 | "source": [ 65 | "# For CV model:\n", 66 | "config_cv = Config(config_name='CV',\n", 67 | " motion_model='CV-Kitti',\n", 68 | " poisson_states_model_name='uniform-CV',\n", 69 | " filter_class='Linear',\n", 70 | " measurement_var_xy=1e-1,\n", 71 | " detection_probability=0.95,\n", 72 | " survival_probability=0.9,\n", 73 | " max_nof_global_hypos=100,\n", 74 | " prune_threshold_global_hypo=-4,\n", 75 | " prune_threshold_targets=-4,\n", 76 | " prune_single_existence=1e-3,\n", 77 | " clutter_intensity=1e-4,\n", 78 | " gating_distance=3,\n", 79 | " birth_gating_distance=3,\n", 80 | " uniform_weight=1,\n", 81 | " poisson_vx=10,\n", 82 | " poisson_vy=10,\n", 83 | " sigma_v=5)\n", 84 | "\n", 85 | "# For CA model:\n", 86 | "config_ca = Config(config_name='CA',\n", 87 | " motion_model='CA',\n", 88 | " poisson_states_model_name='uniform-CA',\n", 89 | " filter_class='Linear',\n", 90 | " measurement_var_xy=1e-1,\n", 91 | " detection_probability=0.95,\n", 92 | " survival_probability=0.9,\n", 93 | " max_nof_global_hypos=100,\n", 94 | " prune_threshold_global_hypo=-4,\n", 95 | " prune_threshold_targets=-4,\n", 96 | " prune_single_existence=1e-3,\n", 97 | " clutter_intensity=1e-4,\n", 98 | " gating_distance=3,\n", 99 | " birth_gating_distance=3,\n", 100 | " uniform_weight=1,\n", 101 | " poisson_vx=10,\n", 102 | " poisson_vy=10,\n", 103 | " sigma_v=5)\n", 104 | "\n", 105 | "# For bicycle models: \n", 106 | "config_bc = Config(config_name='BC',\n", 107 | " motion_model='Bicycle',\n", 108 | " poisson_states_model_name='uniform',\n", 109 | " filter_class='UKF',\n", 110 | " measurement_var_xy=1e-1,\n", 111 | " measurement_var_psi=5e-1,\n", 112 | " detection_probability=0.95,\n", 113 | " survival_probability=0.9,\n", 114 | " max_nof_global_hypos=100,\n", 115 | " prune_threshold_global_hypo=-4,\n", 116 | " prune_threshold_targets=-4,\n", 117 | " prune_single_existence=1e-3,\n", 118 | " clutter_intensity=5e-4,\n", 119 | " gating_distance=3,\n", 120 | " birth_gating_distance=2,\n", 121 | " uniform_weight=1,\n", 122 | " poisson_v=5,\n", 123 | " poisson_d=2,\n", 124 | " sigma_xy_bicycle=0.5,\n", 125 | " sigma_phi=2.5,\n", 126 | " sigma_v=2,\n", 127 | " sigma_d=0.5)\n", 128 | " \n", 129 | "# For mixed models\n", 130 | "config_mix = Config(config_name='Mixed',\n", 131 | " motion_model='Mixed',\n", 132 | " poisson_states_model_name='uniform-mixed',\n", 133 | " filter_class='Mixed',\n", 134 | " measurement_var_xy=1e-1,\n", 135 | " measurement_var_psi=5e-1, #1,\n", 136 | " detection_probability=0.95,\n", 137 | " survival_probability=0.9,\n", 138 | " max_nof_global_hypos=100,\n", 139 | " prune_threshold_global_hypo=-4,\n", 140 | " prune_threshold_targets=-4,\n", 141 | " prune_single_existence=1e-3,\n", 142 | " clutter_intensity=1e-4,\n", 143 | " gating_distance=3,\n", 144 | " birth_gating_distance=3,\n", 145 | " uniform_weight=1,\n", 146 | " poisson_vx=10,#5,\n", 147 | " poisson_vy=10,#5,\n", 148 | " poisson_v=5,#10,\n", 149 | " poisson_d=2,#5,\n", 150 | " sigma_xy_bicycle=0.5,\n", 151 | " sigma_v=2,#1,\n", 152 | " sigma_d=0.5,#0.01,\n", 153 | " sigma_phi=2.5)#1) " 154 | ] 155 | }, 156 | { 157 | "cell_type": "code", 158 | "execution_count": null, 159 | "metadata": {}, 160 | "outputs": [], 161 | "source": [ 162 | "config_cv_car = Config(config_name='Car-CV',\n", 163 | " classes_to_track=['Car', 'Van'],\n", 164 | " motion_model='CV-Kitti',\n", 165 | " poisson_states_model_name='uniform-CV',\n", 166 | " filter_class='Linear',\n", 167 | " measurement_var_xy=1e-1,\n", 168 | " detection_probability=0.95,\n", 169 | " survival_probability=0.9,\n", 170 | " max_nof_global_hypos=100,\n", 171 | " prune_threshold_global_hypo=-4,\n", 172 | " prune_threshold_targets=-4,\n", 173 | " prune_single_existence=1e-3,\n", 174 | " clutter_intensity=1e-4,\n", 175 | " gating_distance=3,\n", 176 | " birth_gating_distance=3,\n", 177 | " uniform_weight=1,\n", 178 | " poisson_vx=10,\n", 179 | " poisson_vy=10,\n", 180 | " sigma_v=5)\n", 181 | "config_cv_ped = Config(config_name='Ped-CV',\n", 182 | " classes_to_track=['Pedestrian'],\n", 183 | " motion_model='CV-Kitti',\n", 184 | " poisson_states_model_name='uniform-CV',\n", 185 | " filter_class='Linear',\n", 186 | " measurement_var_xy=1e-1,\n", 187 | " detection_probability=0.95,\n", 188 | " survival_probability=0.9,\n", 189 | " max_nof_global_hypos=100,\n", 190 | " prune_threshold_global_hypo=-4,\n", 191 | " prune_threshold_targets=-4,\n", 192 | " prune_single_existence=1e-3,\n", 193 | " clutter_intensity=1e-4,\n", 194 | " gating_distance=3,\n", 195 | " birth_gating_distance=3,\n", 196 | " uniform_weight=1,\n", 197 | " poisson_vx=10,\n", 198 | " poisson_vy=10,\n", 199 | " sigma_v=5)\n", 200 | "\n", 201 | "config_bc_car = Config(config_name='Car-BC',\n", 202 | " classes_to_track=['Car', 'Van'],\n", 203 | " motion_model='Bicycle',\n", 204 | " poisson_states_model_name='uniform',\n", 205 | " filter_class='UKF',\n", 206 | " measurement_var_xy=1e-1,\n", 207 | " measurement_var_psi=5e-1,\n", 208 | " detection_probability=0.95,\n", 209 | " survival_probability=0.9,\n", 210 | " max_nof_global_hypos=100,\n", 211 | " prune_threshold_global_hypo=-4,\n", 212 | " prune_threshold_targets=-4,\n", 213 | " prune_single_existence=1e-3,\n", 214 | " clutter_intensity=5e-4,\n", 215 | " gating_distance=3,\n", 216 | " birth_gating_distance=2,\n", 217 | " uniform_weight=1,\n", 218 | " poisson_v=5,\n", 219 | " poisson_d=2,\n", 220 | " sigma_xy_bicycle=0.5,\n", 221 | " sigma_phi=2.5,\n", 222 | " sigma_v=2,\n", 223 | " sigma_d=0.5)\n", 224 | "config_bc_ped = Config(config_name='Ped-BC',\n", 225 | " classes_to_track=['Pedestrian'],\n", 226 | " motion_model='Bicycle',\n", 227 | " poisson_states_model_name='uniform',\n", 228 | " filter_class='UKF',\n", 229 | " measurement_var_xy=1e-1,\n", 230 | " measurement_var_psi=5e-1,\n", 231 | " detection_probability=0.95,\n", 232 | " survival_probability=0.9,\n", 233 | " max_nof_global_hypos=100,\n", 234 | " prune_threshold_global_hypo=-4,\n", 235 | " prune_threshold_targets=-4,\n", 236 | " prune_single_existence=1e-3,\n", 237 | " clutter_intensity=5e-4,\n", 238 | " gating_distance=3,\n", 239 | " birth_gating_distance=2,\n", 240 | " uniform_weight=1,\n", 241 | " poisson_v=5,\n", 242 | " poisson_d=2,\n", 243 | " sigma_xy_bicycle=0.5,\n", 244 | " sigma_phi=2.5,\n", 245 | " sigma_v=2,\n", 246 | " sigma_d=0.5)\n", 247 | "config_cv_fafe = Config(config_name='PMBM-CV',\n", 248 | " classes_to_track=['Car', 'Van'],\n", 249 | " motion_model='CV-Kitti',\n", 250 | " poisson_states_model_name='uniform-CV',\n", 251 | " filter_class='Linear',\n", 252 | " measurement_var_xy=3e-1,\n", 253 | " detection_probability=0.6,\n", 254 | " survival_probability=0.9,\n", 255 | " max_nof_global_hypos=100,\n", 256 | " prune_threshold_global_hypo=-6,\n", 257 | " prune_threshold_targets=-6,\n", 258 | " prune_single_existence=1e-4,\n", 259 | " clutter_intensity=1e-3,\n", 260 | " gating_distance=4,\n", 261 | " birth_gating_distance=4,\n", 262 | " uniform_weight=0.1,\n", 263 | " poisson_vx=10,\n", 264 | " poisson_vy=10,\n", 265 | " sigma_v=5)\n" 266 | ] 267 | }, 268 | { 269 | "cell_type": "code", 270 | "execution_count": null, 271 | "metadata": {}, 272 | "outputs": [], 273 | "source": [ 274 | "config_cv_fafe1 = Config(config_name='PMBM-CV-1',\n", 275 | " classes_to_track=['Car', 'Van'],\n", 276 | " motion_model='CV-Kitti',\n", 277 | " poisson_states_model_name='uniform-CV',\n", 278 | " filter_class='Linear',\n", 279 | " measurement_var_xy=3e-1,\n", 280 | " detection_probability=0.6,\n", 281 | " survival_probability=0.9,\n", 282 | " max_nof_global_hypos=100,\n", 283 | " prune_threshold_global_hypo=-6,\n", 284 | " prune_threshold_targets=-6,\n", 285 | " prune_single_existence=1e-4,\n", 286 | " clutter_intensity=1e-3,\n", 287 | " gating_distance=3,\n", 288 | " birth_gating_distance=4,\n", 289 | " uniform_weight=10,\n", 290 | " poisson_vx=10,\n", 291 | " poisson_vy=10,\n", 292 | " sigma_v=5)\n", 293 | "config_cv_fafe2 = Config(config_name='PMBM-CV-2',\n", 294 | " classes_to_track=['Car', 'Van'],\n", 295 | " motion_model='CV-Kitti',\n", 296 | " poisson_states_model_name='uniform-CV',\n", 297 | " filter_class='Linear',\n", 298 | " measurement_var_xy=3e-1,\n", 299 | " detection_probability=0.4,\n", 300 | " survival_probability=0.9,\n", 301 | " max_nof_global_hypos=100,\n", 302 | " prune_threshold_global_hypo=-6,\n", 303 | " prune_threshold_targets=-6,\n", 304 | " prune_single_existence=1e-4,\n", 305 | " clutter_intensity=1e-3,\n", 306 | " gating_distance=3,\n", 307 | " birth_gating_distance=4,\n", 308 | " uniform_weight=10,\n", 309 | " poisson_vx=10,\n", 310 | " poisson_vy=10,\n", 311 | " sigma_v=5)\n", 312 | "config_cv_fafe3 = Config(config_name='PMBM-CV-3',\n", 313 | " classes_to_track=['Car', 'Van'],\n", 314 | " motion_model='CV-Kitti',\n", 315 | " poisson_states_model_name='uniform-CV',\n", 316 | " filter_class='Linear',\n", 317 | " measurement_var_xy=3e-1,\n", 318 | " detection_probability=0.4,\n", 319 | " survival_probability=0.9,\n", 320 | " max_nof_global_hypos=100,\n", 321 | " prune_threshold_global_hypo=-4,\n", 322 | " prune_threshold_targets=-4,\n", 323 | " prune_single_existence=1e-4,\n", 324 | " clutter_intensity=1e-3,\n", 325 | " gating_distance=3,\n", 326 | " birth_gating_distance=4,\n", 327 | " uniform_weight=10,\n", 328 | " poisson_vx=10,\n", 329 | " poisson_vy=10,\n", 330 | " sigma_v=5,\n", 331 | " sigma_cv=0.5)\n", 332 | "config_cv_fafe4 = Config(config_name='PMBM-CV-4',\n", 333 | " classes_to_track=['Car', 'Van'],\n", 334 | " motion_model='CV-Kitti',\n", 335 | " poisson_states_model_name='uniform-CV',\n", 336 | " filter_class='Linear',\n", 337 | " measurement_var_xy=3e-1,\n", 338 | " detection_probability=0.4,\n", 339 | " survival_probability=0.9,\n", 340 | " max_nof_global_hypos=100,\n", 341 | " prune_threshold_global_hypo=-4,\n", 342 | " prune_threshold_targets=-4,\n", 343 | " prune_single_existence=1e-3,\n", 344 | " clutter_intensity=1e-3,\n", 345 | " gating_distance=3,\n", 346 | " birth_gating_distance=4,\n", 347 | " uniform_weight=10,\n", 348 | " poisson_vx=10,\n", 349 | " poisson_vy=10,\n", 350 | " sigma_v=5,\n", 351 | " sigma_cv=0.5)\n", 352 | "config_cv_fafe5 = Config(config_name='PMBM-CV-5',\n", 353 | " classes_to_track=['Car', 'Van'],\n", 354 | " motion_model='CV-Kitti',\n", 355 | " poisson_states_model_name='uniform-CV',\n", 356 | " filter_class='Linear',\n", 357 | " measurement_var_xy=3e-1,\n", 358 | " detection_probability=0.4,\n", 359 | " survival_probability=0.9,\n", 360 | " max_nof_global_hypos=100,\n", 361 | " prune_threshold_global_hypo=-6,\n", 362 | " prune_threshold_targets=-6,\n", 363 | " prune_single_existence=1e-4,\n", 364 | " clutter_intensity=1e-4,\n", 365 | " gating_distance=3,\n", 366 | " birth_gating_distance=4,\n", 367 | " uniform_weight=10,\n", 368 | " poisson_vx=10,\n", 369 | " poisson_vy=10,\n", 370 | " sigma_v=5,\n", 371 | " sigma_cv=0.5)\n", 372 | "config_cv_fafe6 = Config(config_name='PMBM-CV-6',\n", 373 | " classes_to_track=['Car', 'Van'],\n", 374 | " motion_model='CV-Kitti',\n", 375 | " poisson_states_model_name='uniform-CV',\n", 376 | " filter_class='Linear',\n", 377 | " measurement_var_xy=3e-1,\n", 378 | " detection_probability=0.4,\n", 379 | " survival_probability=0.9,\n", 380 | " max_nof_global_hypos=100,\n", 381 | " prune_threshold_global_hypo=-6,\n", 382 | " prune_threshold_targets=-6,\n", 383 | " prune_single_existence=1e-4,\n", 384 | " clutter_intensity=1e-3,\n", 385 | " gating_distance=3,\n", 386 | " birth_gating_distance=4,\n", 387 | " uniform_weight=10,\n", 388 | " poisson_vx=10,\n", 389 | " poisson_vy=10,\n", 390 | " sigma_v=5,\n", 391 | " sigma_cv=1)\n", 392 | "config_cv_fafe7 = Config(config_name='PMBM-CV-7',\n", 393 | " classes_to_track=['Car', 'Van'],\n", 394 | " motion_model='CV-Kitti',\n", 395 | " poisson_states_model_name='uniform-CV',\n", 396 | " filter_class='Linear',\n", 397 | " measurement_var_xy=3e-1,\n", 398 | " detection_probability=0.4,\n", 399 | " survival_probability=0.9,\n", 400 | " max_nof_global_hypos=100,\n", 401 | " prune_threshold_global_hypo=-6,\n", 402 | " prune_threshold_targets=-6,\n", 403 | " prune_single_existence=1e-4,\n", 404 | " clutter_intensity=1e-3,\n", 405 | " gating_distance=3,\n", 406 | " birth_gating_distance=4,\n", 407 | " uniform_weight=10,\n", 408 | " poisson_vx=10,\n", 409 | " poisson_vy=10,\n", 410 | " sigma_v=5,\n", 411 | " sigma_cv=1.5)" 412 | ] 413 | }, 414 | { 415 | "cell_type": "code", 416 | "execution_count": null, 417 | "metadata": {}, 418 | "outputs": [], 419 | "source": [ 420 | "max_number_of_timesteps = constants.LARGE\n", 421 | "#max_number_of_timesteps = 19\n", 422 | "sequence_indeces = 0 #np.arange(0,21) #,12,14,16,20] # [12] #np.arange(0,21) #[0,1,12,14,16,20] # np.arange(0,21) ##, 5] #,1,2,3,4,5]\n", 423 | "\n", 424 | "showroom_path = '/home/mlt/mot/fafe/showroom/weights_2019-05-03_14-01_epoch_110_fafe'\n", 425 | "\n", 426 | "# 0 for CV, 1 for Bicycle, 2 for mixed, 'all' for running all\n", 427 | "model = 'compare-pmbm-fafe' #'all'\n", 428 | "\n", 429 | "save_plots = False\n", 430 | "show_stuff = False\n", 431 | "\n", 432 | "plot_gospa_comparison = False # For comparing PMBM w. FaFeNet\n", 433 | "use_fafenet_detections = False\n", 434 | "use_fafenet_measurements = False\n", 435 | "track_only_cars_vans = False # If only use car, van, misc as measurements to pmbm" 436 | ] 437 | }, 438 | { 439 | "cell_type": "code", 440 | "execution_count": null, 441 | "metadata": {}, 442 | "outputs": [], 443 | "source": [ 444 | "if model == 0: \n", 445 | " measurement_dimss = [2]\n", 446 | " configs = [config_cv]\n", 447 | "if model == 1: \n", 448 | " measurement_dimss = [3]\n", 449 | " configs = [config_bc]\n", 450 | "elif model == 2:\n", 451 | " measurement_dimss = [3]\n", 452 | " configs = [config_mix]\n", 453 | "elif model == 'all':\n", 454 | " measurement_dimss = [2,2,3,3]\n", 455 | " configs = [config_cv, config_ca, config_bc, config_mix]\n", 456 | "elif model == 'kg-plot':\n", 457 | " measurement_dimss = [2,3]\n", 458 | " configs = [config_cv, config_mix]\n", 459 | "elif model == 'diff-mix':\n", 460 | " measurement_dimss = [3,3,3]\n", 461 | " configs = [config_mix, config_mix2, config_mix3]\n", 462 | "elif model == 'fafe':\n", 463 | " measurement_dimss = [2]\n", 464 | " configs = [config_fafe]\n", 465 | " use_fafenet_detections = True\n", 466 | "elif model == 'ca':\n", 467 | " measurement_dimss = [2]\n", 468 | " configs = [config_ca]\n", 469 | "elif model == 'ped-car':\n", 470 | " measurement_dimss = [2,2,3,3]\n", 471 | " configs = [config_cv_car, config_cv_ped, config_bc_car, config_bc_ped]\n", 472 | "elif model == 'compare-pmbm-fafe':\n", 473 | " measurement_dimss = [2,2,2,2,2,2,2,2] # [2] #\n", 474 | " configs = [config_cv_fafe, config_cv_fafe1, config_cv_fafe2, config_cv_fafe3, config_cv_fafe4,\n", 475 | " config_cv_fafe5, config_cv_fafe6, config_cv_fafe7] # [config_cv_fafe4] #\n", 476 | " use_fafenet_measurements = True\n", 477 | " log_root = '/home/mlt/mot/fafe/showroom/showroom_all/showroom_bev_NN/logs/log-seq'\n", 478 | " sequence_indeces = [0, 1, 2, 3, 5, 6, 8, 9, 10, 11, 12, 13, 14, 18, 19, 20]" 479 | ] 480 | }, 481 | { 482 | "cell_type": "code", 483 | "execution_count": null, 484 | "metadata": {}, 485 | "outputs": [], 486 | "source": [ 487 | "time_str = datetime.datetime.now().strftime('%m-%d_%H%M')\n", 488 | "for sequence_idx in sequence_indeces: \n", 489 | " log_path = log_root + str(sequence_idx).zfill(4)\n", 490 | " print('Sequence: {}'.format(sequence_idx))\n", 491 | " try:\n", 492 | " root = '/home/mlt/data'\n", 493 | " kitti= kitti_stuff.Kitti(ROOT=root, split='training')\n", 494 | " kitti.lbls = kitti.load_labels(sequence_idx)\n", 495 | " kitti.imus = kitti.load_imu(sequence_idx)\n", 496 | " kitti.load_measurements(p_missed=0.05, p_clutter=0.02, sigma_xy=0.1, sigma_psi=0.1)\n", 497 | " if use_fafenet_detections:\n", 498 | " fd = FafeDetections(showroom_path, sequence=sequence_idx)\n", 499 | " if use_fafenet_measurements:\n", 500 | " fm = FafeMeasurements(log_path)\n", 501 | " except AssertionError:\n", 502 | " kitti = kitti_stuff.Kitti(ROOT='/Users/erikbohnsack/data', split='training')\n", 503 | " kitti.lbls = kitti.load_labels(sequence_idx)\n", 504 | " kitti.imus = kitti.load_imu(sequence_idx)\n", 505 | " kitti.load_measurements(p_missed=0.05, p_clutter=0.02, sigma_xy=0.1, sigma_psi=0.1)\n", 506 | " root = '/Users/erikbohnsack/data' \n", 507 | " imud = kitti.load_imu(sequence_idx)\n", 508 | " \n", 509 | " for ix_config, config in enumerate(configs):\n", 510 | " print('\\tConfig: {} ({})'.format(ix_config, config.name))\n", 511 | " measurement_dims = measurement_dimss[ix_config]\n", 512 | " \n", 513 | " if use_fafenet_detections:\n", 514 | " number_of_timesteps = min(fd.max_frame_idx, max_number_of_timesteps)\n", 515 | " fafe_gospa_filename = os.path.join(showroom_path, 'gospa_scores_' + str(sequence_idx).zfill(4) + '.txt')\n", 516 | " else:\n", 517 | " number_of_timesteps = min(kitti.max_frame_idx, max_number_of_timesteps)\n", 518 | " \n", 519 | " _pmbm = pmbm.PMBM(config)\n", 520 | " L = logger.Logger(sequence_idx=sequence_idx, config_idx=ix_config, filename=config.name)\n", 521 | "\n", 522 | " toc_list = []\n", 523 | " for frame_idx in range(number_of_timesteps):\n", 524 | " #if show_stuff: print('\\t\\tFrame: {}'.format(frame_idx))\n", 525 | " tic = time.time()\n", 526 | " \n", 527 | " if use_fafenet_detections:\n", 528 | " measurements, classes = fd.get_fafe_detections(frame_idx, measurement_dims)\n", 529 | " elif use_fafenet_measurements:\n", 530 | " measurements, classes = fm.get_fafe_measurements(frame_idx, measurement_dims)\n", 531 | " else:\n", 532 | " measurements, classes = kitti.get_measurements(frame_idx, measurement_dims, classes_to_track=config.classes_to_track)\n", 533 | " \n", 534 | " _pmbm.run(measurements, classes, imud[frame_idx], frame_idx, verbose=False, verbose_time=False)\n", 535 | " \n", 536 | " toc= time.time()-tic\n", 537 | " toc_list.append(toc)\n", 538 | "\n", 539 | " L.log_data(_pmbm, frame_id=frame_idx, total_time=toc, measurements=measurements, true_states=kitti.get_bev_states(frame_idx, classes_to_track=config.classes_to_track), verbose=False)\n", 540 | " \n", 541 | " data = logger.load_logging_data(filename=L.filename)\n", 542 | " gospa_sl = logger.calculate_GOSPA_score(data=data, gt_dims=2)\n", 543 | " mot_summary = logger.calculate_MOT(sequence_idx, root, data=data, classes_to_track=config.classes_to_track)\n", 544 | " predictions_gospa_scores, predictions_average_gospa = logger.prediction_stats(sequence_idx, config, kitti, data=data)\n", 545 | " \n", 546 | " L.log_stats(toc_list, gospa_sl, mot_summary, config, predictions_average_gospa)\n", 547 | " if show_stuff:\n", 548 | " plot_stuff.plot_time_score(data=data, score_list=gospa_sl, config_name=config.name+'_seq_'+str(sequence_idx).zfill(4)) \n", 549 | " plot_stuff.plot_hypos_over_time(data=data, config_name=config.name+'_seq_'+str(sequence_idx).zfill(4))\n", 550 | " plot_stuff.plot_target_life(data=data, config_name=config.name+'_seq_'+str(sequence_idx).zfill(4))\n", 551 | " if plot_gospa_comparison:\n", 552 | " plot_stuff.compare_pmbm_fafe_gospas(pmbm_sl=gospa_sl, fafe_filename=fafe_gospa_filename, pmbm_pred_gospa=predictions_average_gospa)\n", 553 | "\n", 554 | " if save_plots:\n", 555 | " print('Saving {} tracking history plots...'.format(min(kitti.max_frame_idx, number_of_timesteps)))\n", 556 | " dir_name = config.name\n", 557 | " in_dir = '_'.join(('showroom/output_tracks', dir_name, time_str))\n", 558 | " if not os.path.exists(in_dir):\n", 559 | " os.mkdir(in_dir)\n", 560 | " path = os.path.join(in_dir, str(sequence_idx).zfill(4))\n", 561 | " for frame_idx in range(min(kitti.max_frame_idx, number_of_timesteps)):\n", 562 | " print('{},'.format(frame_idx),end='')\n", 563 | " plot_stuff.plot_tracking_history(path, sequence_idx=sequence_idx, data=data, kitti=kitti, final_frame_idx=frame_idx, disp='save', only_alive=True, show_cov=True, show_predictions=config.show_predictions,\n", 564 | " config_name=config.name, car_van_flag=track_only_cars_vans, fafe=False, num_conseq_frames=None)\n", 565 | " print('Saved successfully!')\n", 566 | " print('Making movie in path: {}]'.format(path))\n", 567 | " plot_stuff.make_movie_from_images(path)\n", 568 | " print('{}.mov complete'.format(path))\n", 569 | " \n", 570 | "\n", 571 | "df, avg_df = plot_stuff.sequence_analysis(sortby='CfgName')\n", 572 | "if avg_df is not None: print(avg_df.to_string())\n", 573 | "df\n", 574 | "\n", 575 | "# Save as latex tables\n", 576 | "df2 = df.drop(columns=['Filter', 'PoissonModel', 'MotionModel', 'MostlyLost', '#Fragmentations', 'PredGOSPA'])\n", 577 | "file = open(\"showroom/stats_table.txt\", \"w\")\n", 578 | "\n", 579 | "for seq in range(0, max(df['SeqId'].values) + 1):\n", 580 | " df3 = df2.loc[df2['SeqId'] == seq]\n", 581 | " df3 = df3.sort_values(by ='CfgName', ascending=True)\n", 582 | " _str = '\\n\\\\begin{table}[] \\n\\centering'\n", 583 | " file.write(_str)\n", 584 | " file.write(df3.to_latex(index=False)) \n", 585 | " _str = ' \\caption{Results for sequence ' + str(seq) + '} \\n \\label{tab:avg-pmbm-df}\\n\\end{table}\\n'\n", 586 | " file.write(_str)\n", 587 | "file.close()\n", 588 | "\n", 589 | "if avg_df is not None:\n", 590 | " file = open(\"showroom/average_stats_table.txt\", \"w\")\n", 591 | " file.write(avg_df.to_latex(index=True)) \n", 592 | " file.close()" 593 | ] 594 | }, 595 | { 596 | "cell_type": "code", 597 | "execution_count": null, 598 | "metadata": {}, 599 | "outputs": [], 600 | "source": [] 601 | }, 602 | { 603 | "cell_type": "code", 604 | "execution_count": null, 605 | "metadata": {}, 606 | "outputs": [], 607 | "source": [ 608 | "if True:\n", 609 | " from utils import plot_stuff\n", 610 | " stats_path = '/home/mlt/mot/conventional-MOT/showroom/all_sequences/cv-ca-bc-mx/logs' + '/stats'\n", 611 | " df, avg_df = plot_stuff.sequence_analysis(filenames_prefix=stats_path, sortby='CfgName')" 612 | ] 613 | }, 614 | { 615 | "cell_type": "code", 616 | "execution_count": null, 617 | "metadata": {}, 618 | "outputs": [], 619 | "source": [] 620 | } 621 | ], 622 | "metadata": { 623 | "kernelspec": { 624 | "display_name": "Python 3", 625 | "language": "python", 626 | "name": "python3" 627 | }, 628 | "language_info": { 629 | "codemirror_mode": { 630 | "name": "ipython", 631 | "version": 3 632 | }, 633 | "file_extension": ".py", 634 | "mimetype": "text/x-python", 635 | "name": "python", 636 | "nbconvert_exporter": "python", 637 | "pygments_lexer": "ipython3", 638 | "version": "3.7.2" 639 | } 640 | }, 641 | "nbformat": 4, 642 | "nbformat_minor": 2 643 | } 644 | -------------------------------------------------------------------------------- /utils/GA.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "from gen_hyperparam_search import ga" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "ga()" 19 | ] 20 | }, 21 | { 22 | "cell_type": "code", 23 | "execution_count": null, 24 | "metadata": {}, 25 | "outputs": [], 26 | "source": [] 27 | } 28 | ], 29 | "metadata": { 30 | "kernelspec": { 31 | "display_name": "Python 3", 32 | "language": "python", 33 | "name": "python3" 34 | }, 35 | "language_info": { 36 | "codemirror_mode": { 37 | "name": "ipython", 38 | "version": 3 39 | }, 40 | "file_extension": ".py", 41 | "mimetype": "text/x-python", 42 | "name": "python", 43 | "nbconvert_exporter": "python", 44 | "pygments_lexer": "ipython3", 45 | "version": "3.7.2" 46 | } 47 | }, 48 | "nbformat": 4, 49 | "nbformat_minor": 2 50 | } 51 | -------------------------------------------------------------------------------- /utils/UKF.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from filterpy.kalman import unscented_transform 3 | from scipy.linalg import cholesky 4 | from utils.matrix_stuff import switch_state_direction 5 | 6 | 7 | class UnscentedKalmanFilter: 8 | def __init__(self, dt, dim_x, dim_z, points, motion_model, measurement_model, 9 | residual_z=None, residual_x=None, sqrt_fn=None): 10 | 11 | self.points_fn = points 12 | self._num_sigmas = points.num_sigmas() 13 | self._dim_x = dim_x 14 | self._dim_z = dim_z 15 | self.measurement_model = measurement_model 16 | self.motion_model = motion_model 17 | self._dt = dt 18 | self._num_sigmas = points.num_sigmas() 19 | 20 | # weights for the means and covariances. 21 | self.Wm, self.Wc = points.Wm, points.Wc 22 | 23 | if residual_x is None: 24 | self.residual_x = np.subtract 25 | else: 26 | self.residual_x = residual_x 27 | 28 | if residual_z is None: 29 | self.residual_z = np.subtract 30 | else: 31 | self.residual_z = residual_z 32 | 33 | if sqrt_fn is None: 34 | self.msqrt = cholesky 35 | else: 36 | self.msqrt = sqrt_fn 37 | 38 | def __repr__(self): 39 | pass 40 | 41 | def predict(self, state, variance, motion_model, motion_noise, object_class, dt=None, UT=None): 42 | """ 43 | Performs the predict step of the UKF. On return, self.x and 44 | self.P contain the predicted state (x) and covariance (P). ' 45 | Important: this MUST be called before update() is called for the first 46 | time. 47 | Parameters 48 | ---------- 49 | state : Prior state estimate (x_{k-1|k-1}) 50 | variance : Prior state covariance matrix (P_{k-1|k-1}) 51 | dt : double, optional 52 | If specified, the time step to be used for this prediction. 53 | self._dt is used if this is not provided. 54 | motion_model : callable f(x, **fx_args), optional 55 | State transition function. If not provided, the default 56 | function passed in during construction will be used. 57 | UT : function(sigmas, Wm, Wc, noise_cov), optional 58 | Optional function to compute the unscented transform for the sigma 59 | points passed through hx. Typically the default function will 60 | work - you can use x_mean_fn and z_mean_fn to alter the behavior 61 | of the unscented transform. 62 | object_class : Either Car or Bicycle. 63 | """ 64 | assert state.shape == (self._dim_x, ), "State shape not 1D array, required by UKF" 65 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 66 | if dt is None: 67 | dt = self._dt 68 | 69 | if UT is None: 70 | UT = unscented_transform 71 | 72 | # calculate sigma points for given mean and covariance 73 | points = self.compute_process_sigmas(x=state, P=variance, dt=dt, motion_model=motion_model, object_class=object_class) 74 | 75 | # and pass sigmas through the unscented transform to compute prior 76 | x, P = UT(points, self.Wm, self.Wc, motion_noise, mean_fn=None, residual_fn=self.residual_x) 77 | return x.reshape(-1, 1), P 78 | 79 | def update(self, state, variance, measurement, measurement_model, measurement_noise, UT=None): 80 | """ 81 | Update the UKF with the given measurements. On return, 82 | self.x and self.P contain the new mean and covariance of the filter. 83 | Parameters 84 | ---------- 85 | state : Predicted state estimate (x_{k|k-1}) 86 | variance : Predicted state covariance matrix (P_{k|k-1}) 87 | measurement : numpy.array of shape (dim_z) 88 | measurement vector 89 | measurement_noise : numpy.array((dim_z, dim_z)), optional 90 | Measurement noise. If provided, overrides self.R for 91 | this function call. 92 | measurement_model: 93 | UT : function(sigmas, Wm, Wc, noise_cov), optional 94 | Optional function to compute the unscented transform for the sigma 95 | points passed through hx. Typically the default function will 96 | work - you can use x_mean_fn and z_mean_fn to alter the behavior 97 | of the unscented transform. 98 | 99 | """ 100 | assert state.shape == (self._dim_x,), "State shape not 1D array, required by UKF" 101 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 102 | if measurement is None: 103 | return state, variance 104 | 105 | if measurement_model is None: 106 | measurement_model = self.measurement_model 107 | 108 | if UT is None: 109 | UT = unscented_transform 110 | 111 | if measurement_noise is None: 112 | measurement_noise = self.R 113 | elif np.isscalar(measurement_noise): 114 | measurement_noise = np.eye(self._dim_z) * measurement_noise 115 | 116 | sigmas_f = self.points_fn.sigma_points(state, variance) 117 | 118 | # pass prior sigmas through h(x) to get measurement sigmas 119 | # the shape of sigmas_h will vary if the shape of z varies, so 120 | # recreate each time 121 | sigmas_h = [] 122 | for s in sigmas_f: 123 | sigmas_h.append(measurement_model @ s) 124 | 125 | sigmas_h = np.atleast_2d(sigmas_h) 126 | 127 | # mean and covariance of prediction passed through unscented transform 128 | zp, S = UT(sigmas_h, self.Wm, self.Wc, measurement_noise, mean_fn=None, residual_fn=self.residual_z) 129 | SI = np.linalg.inv(S) 130 | Pxz = self.cross_variance(state, zp, sigmas_f, sigmas_h) 131 | 132 | K = Pxz @ SI # Kalman gain 133 | y = self.residual_z(measurement[:, 0], zp) # residual 134 | 135 | state += K @ y 136 | #if state[3] < 0: 137 | # print("------------ ERROR -------------") 138 | 139 | # print("State: {}".format(state)) 140 | # print("Old State: {}".format(state - K @ y)) 141 | # print("Measurement: {}".format(measurement)) 142 | # print("K: {}".format(K)) 143 | # print("Pxz: {}".format(Pxz)) 144 | # print("SI: {}".format(SI)) 145 | # print("y: {}".format(y)) 146 | # raise ValueError("Negative speed") 147 | 148 | variance -= K @ S @ K.T 149 | #output_state = switch_state_direction(state) 150 | return state.reshape(-1, 1), variance 151 | 152 | def compute_process_sigmas(self, x, P, dt, motion_model=None, object_class='Car'): 153 | """ 154 | computes the values of sigmas_f. Normally a user would not call 155 | this, but it is useful if you need to call update more than once 156 | between calls to predict (to update for multiple simultaneous 157 | measurements), so the sigmas correctly reflect the updated state 158 | x, P. 159 | """ 160 | if motion_model is None: 161 | motion_model = self.motion_model 162 | # calculate sigma points for given mean and covariance 163 | sigmas = self.points_fn.sigma_points(x, P) 164 | sigmas_f = np.zeros((self._num_sigmas, self._dim_x)) 165 | for i, s in enumerate(sigmas): 166 | if motion_model.model: 167 | sigmas_f[i] = motion_model(state=s, variance=P, object_class=object_class)[0] 168 | else: 169 | sigmas_f[i] = motion_model(state=s, variance=P, object_class=object_class) 170 | return sigmas_f 171 | 172 | def cross_variance(self, x, z, sigmas_f, sigmas_h): 173 | """ 174 | Compute cross variance of the state `x` and measurement `z`. 175 | """ 176 | 177 | Pxz = np.zeros((sigmas_f.shape[1], sigmas_h.shape[1])) 178 | N = sigmas_f.shape[0] 179 | for i in range(N): 180 | dx = self.residual_x(sigmas_f[i], x) 181 | dz = self.residual_z(sigmas_h[i], z) 182 | Pxz += self.Wc[i] * np.outer(dx, dz) 183 | return Pxz 184 | 185 | -------------------------------------------------------------------------------- /utils/Uniform.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erikbohnsack/pmbm/b6e48462cca6c12f9fb425002515dc199658da14/utils/Uniform.png -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /utils/constants.py: -------------------------------------------------------------------------------- 1 | PossibleClasses = ['Car', 'Pedestrian', 'Van', 'Cyclist'] 2 | 3 | LARGE = 10000.0 4 | 5 | XLIM = (-30, 30) 6 | ZLIM = (-5, 55) 7 | 8 | -------------------------------------------------------------------------------- /utils/coord_transf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from math import atan2, sqrt 4 | 5 | 6 | def coordinate_transform_bicycle(state, imu_data, dt, object_class=None): 7 | assert state.shape == (5, 1) 8 | # Euler integrate angle of ego vehicle 9 | angle = - imu_data.ru * dt 10 | 11 | # minus angle holds here as well 12 | rotation_matrix = np.array([[math.cos(angle), - math.sin(angle)], 13 | [math.sin(angle), math.cos(angle)]]) 14 | translation = np.array([[- imu_data.vl * dt], 15 | [imu_data.vf * dt]]) 16 | 17 | output_state = np.copy(state) 18 | output_state[0:2] = rotation_matrix @ state[0:2] - translation 19 | output_state[2] += angle 20 | 21 | # same argument for minus translation here as above. 22 | return output_state 23 | 24 | 25 | def coordinate_transform_CV(state, imu_data, dt, object_class=None): 26 | assert state.shape == (4, 1) 27 | # Minus as the imu_data is for the ego vehicle 28 | angle = - imu_data.ru * dt 29 | 30 | # minus angle holds here as well 31 | rotation_matrix = np.array([[math.cos(angle), - math.sin(angle)], 32 | [math.sin(angle), math.cos(angle)]]) 33 | translation = np.array([[- imu_data.vl * dt], 34 | [imu_data.vf * dt]]) 35 | 36 | output_state = np.copy(state) 37 | output_state[0:2] = rotation_matrix @ state[0:2] - translation 38 | output_state[2:4] = rotation_matrix @ state[2:4] 39 | # same argument for minus translation here as above. 40 | return output_state 41 | 42 | 43 | def coordinate_transform_CA(state, imu_data, dt, object_class=None): 44 | assert state.shape == (6, 1) 45 | # Minus as the imu_data is for the ego vehicle 46 | angle = - imu_data.ru * dt 47 | 48 | # minus angle holds here as well 49 | rotation_matrix = np.array([[math.cos(angle), - math.sin(angle)], 50 | [math.sin(angle), math.cos(angle)]]) 51 | translation = np.array([[- imu_data.vl * dt], 52 | [imu_data.vf * dt]]) 53 | 54 | output_state = np.copy(state) 55 | output_state[0:2] = rotation_matrix @ state[0:2] - translation 56 | output_state[2:4] = rotation_matrix @ state[2:4] 57 | output_state[4:6] = rotation_matrix @ state[4:6] 58 | # same argument for minus translation here as above. 59 | return output_state 60 | 61 | 62 | 63 | def coordinate_transform_mixed(state, imu_data, dt, object_class): 64 | if object_class == 'Pedestrian' or object_class == 'Misc' or object_class == 'Person': 65 | output_state = coordinate_transform_CV(state, imu_data, dt) 66 | else: 67 | output_state = coordinate_transform_bicycle(state, imu_data, dt) 68 | 69 | return output_state 70 | 71 | def within_fov(point, min_angle=0.78, max_angle=2.45, max_radius=100): 72 | angle = atan2(point[1], point[0]) 73 | radius = sqrt(point[0] ** 2 + point[1] ** 2) 74 | return min_angle < angle < max_angle and radius < max_radius 75 | -------------------------------------------------------------------------------- /utils/environments.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.patches as patches 4 | import matplotlib as mpl 5 | import cv2 6 | 7 | 8 | def plot_bev_gt(labels, frame_idx): 9 | fig = plt.figure() 10 | ax = fig.add_subplot(111) 11 | 12 | ego_vehicle = patches.Rectangle((-0.5, -2), 1, 4, color="blue", alpha=0.50) 13 | ax.add_patch(ego_vehicle) 14 | 15 | 16 | for l in labels[frame_idx]: 17 | if l.type[0] == 'DontCare': 18 | continue 19 | 20 | x_pos = l.location[0] 21 | z_pos = l.location[2] 22 | width = l.dimensions[1] 23 | length = l.dimensions[2] 24 | 25 | rot_y = l.rotation_y 26 | _x = x_pos - width / 2 27 | _z = z_pos - length / 2 28 | r = patches.Rectangle((_x, _z), width, length, color="red", alpha=0.2) 29 | # TODO: fix the rotation of bboxes 30 | #t = mpl.transforms.Affine2D().rotate_around(x_pos, z_pos, -rot_y) + ax.transData 31 | #r.set_transform(t) 32 | ax.add_patch(r) 33 | 34 | plt.text(x_pos-width,z_pos+length, str(l.track_id),color='black') 35 | plt.plot(x_pos, z_pos, 'r*') 36 | 37 | 38 | plt.xlim(-xlim, xlim) 39 | plt.ylim(-0.25*zlim, 1.75*zlim) 40 | plt.grid(True) 41 | 42 | plt.show() 43 | 44 | 45 | def plot_image(image): 46 | plt.figure(figsize=(16, 9)) 47 | plt.imshow(cv2.cvtColor(image, cv2.COLOR_BGR2RGB)) 48 | plt.xticks([]) 49 | plt.yticks([]) 50 | plt.show() 51 | 52 | 53 | def plot_gt_et(labels, frame_idx, estimated_targets): 54 | fig = plt.figure() 55 | ax = fig.add_subplot(111) 56 | 57 | ego_vehicle = patches.Rectangle((-0.5, -2), 1, 4, color="blue", alpha=0.50) 58 | ax.add_patch(ego_vehicle) 59 | 60 | for l in labels[frame_idx]: 61 | if l.type[0] == 'DontCare': 62 | continue 63 | 64 | x_pos = l.location[0] 65 | z_pos = l.location[2] 66 | width = l.dimensions[1] 67 | length = l.dimensions[2] 68 | 69 | rot_y = l.rotation_y 70 | _x = x_pos - width / 2 71 | _z = z_pos - length / 2 72 | r = patches.Rectangle((_x, _z), width, length, color="red", alpha=0.2) 73 | # TODO: fix the rotation of bboxes 74 | #t = mpl.transforms.Affine2D().rotate_around(x_pos, z_pos, -rot_y) + ax.transData 75 | #r.set_transform(t) 76 | ax.add_patch(r) 77 | 78 | plt.text(x_pos-width,z_pos+length, str(l.track_id),color='black') 79 | plt.plot(x_pos, z_pos, 'r*') 80 | if not len(estimated_targets) == 0: 81 | for est in estimated_targets: 82 | _x = est['single_target'].state[0] 83 | _z = est['single_target'].state[1] 84 | ax.plot(_x, _z, 'go', lw=2, ms=2) 85 | 86 | plt.xlim(-xlim, xlim) 87 | plt.ylim(-0.25*zlim, 1.75*zlim) 88 | plt.grid(True) 89 | 90 | plt.show() 91 | 92 | -------------------------------------------------------------------------------- /utils/eval_metrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.optimize as spopt 3 | from scipy.spatial.distance import cdist 4 | 5 | 6 | # "Generalized optimal sub-pattern assignment metric" 7 | # Rahmathullah et al 2017 8 | def GOSPA(ground_truths, estimates, p=1, c=100, alpha=2., state_dim=2): 9 | assert ground_truths.shape[1] == state_dim, 'Shape = {}, state_dim = {}'.format(ground_truths.shape[1], state_dim) 10 | assert estimates.shape[1] == state_dim 11 | 12 | m = len(ground_truths) 13 | n = len(estimates) 14 | if m > n: 15 | return GOSPA(estimates, ground_truths, p, c, alpha, state_dim) 16 | if m == 0: 17 | return c ** p / alpha * n 18 | costs = cdist(ground_truths, estimates) 19 | costs = np.minimum(costs, c) ** p 20 | row_ind, col_ind = spopt.linear_sum_assignment(costs) 21 | return np.sum(costs[row_ind, col_ind]) + c ** p / alpha * (n - m) 22 | 23 | 24 | # did this myself - like RMS over time 25 | def normalizeGOSPA(ospas, ns, smoothval, p=1): 26 | ospas = np.convolve(ospas, [1] * smoothval, 'valid')[::smoothval] 27 | counts = np.convolve(ns, [1] * smoothval, 'valid')[::smoothval] 28 | return (ospas / counts) ** (1. / p) 29 | 30 | 31 | def isPSD(matrix): return np.sign(np.linalg.eigvals(matrix)) 32 | 33 | 34 | def isPosDef(matrix): return np.all(np.real(np.linalg.eigvals(matrix)) > 0) 35 | 36 | 37 | def normpdf(x, prec): 38 | dev = prec.dot(x).dot(x) 39 | 40 | return (np.linalg.det(prec) / np.exp(dev)) ** .5 / (2 * np.pi) ** 1.5 41 | -------------------------------------------------------------------------------- /utils/gen_hyperparam_search.py: -------------------------------------------------------------------------------- 1 | from deap import base, creator, tools 2 | from deap import algorithms 3 | import numpy as np 4 | from operator import attrgetter 5 | from pmbm.pmbm import PMBM 6 | from pmbm.config import Config 7 | from utils.mot_metrics import MotCalculator 8 | import motmetrics as mm 9 | from utils.eval_metrics import GOSPA 10 | from data_utils import kitti_stuff 11 | import platform 12 | import random 13 | import time 14 | 15 | 16 | class Params: 17 | def __init__(self): 18 | self.measurement_var_xy = (0.01, 5) 19 | self.measurement_var_psi = (1, 5) 20 | 21 | self.poisson_vx = (1, 10) 22 | self.poisson_vy = (1, 10) 23 | self.poisson_v = (1, 10) 24 | self.poisson_d = (1, 15) 25 | 26 | self.sigma_v = (1, 15) 27 | self.sigma_d = (1, 10) 28 | self.sigma_phi = (1, 10) 29 | 30 | 31 | def ga(): 32 | ngen = 50 33 | n_ind = 20 34 | print("Running Genetic Algorithm. # Gen: {}, # Individuals: {}".format(ngen, n_ind)) 35 | toolbox = base.Toolbox() 36 | # CONSTRAINT_PENALTY = 10000 37 | p = Params() 38 | 39 | creator.create("FitnessMax", base.Fitness, weights=(1.0,)) 40 | creator.create("Individual", list, fitness=creator.FitnessMax) 41 | 42 | toolbox.register("mate", tools.cxTwoPoint) 43 | toolbox.register("mutate", tools.mutGaussian, mu=0, sigma=0.5, indpb=0.2) 44 | toolbox.register("select", tools.selTournament, tournsize=3) 45 | 46 | toolbox.decorate("mate", checkBounds(p)) 47 | toolbox.decorate("mutate", checkBounds(p)) 48 | 49 | toolbox.register("evaluate", evalFct) 50 | # toolbox.decorate("evaluate", tools.DeltaPenalty(feasible, CONSTRAINT_PENALTY)) 51 | 52 | toolbox.register("attr_meas_var_xy", random.uniform, p.measurement_var_xy[0], p.measurement_var_xy[1]) 53 | toolbox.register("attr_meas_var_psi", random.uniform, p.measurement_var_psi[0], p.measurement_var_psi[1]) 54 | toolbox.register("attr_poisson_vx", random.uniform, p.poisson_vx[0], p.poisson_vx[1]) 55 | toolbox.register("attr_poisson_vy", random.uniform, p.poisson_vy[0], p.poisson_vy[1]) 56 | toolbox.register("attr_poisson_v", random.uniform, p.poisson_v[0], 57 | p.poisson_v[1]) 58 | toolbox.register("attr_poisson_d", random.uniform, p.poisson_d[0], 59 | p.poisson_d[1]) 60 | toolbox.register("attr_sigma_v", random.uniform, p.sigma_v[0], 61 | p.sigma_v[1]) 62 | toolbox.register("attr_sigma_d", random.uniform, p.sigma_d[0], p.sigma_d[1]) 63 | toolbox.register("attr_sigma_phi", random.uniform, p.sigma_phi[0], p.sigma_phi[1]) 64 | 65 | toolbox.register("individual", tools.initCycle, creator.Individual, 66 | (toolbox.attr_meas_var_xy, toolbox.attr_meas_var_psi, 67 | toolbox.attr_poisson_vx, toolbox.attr_poisson_vy, 68 | toolbox.attr_poisson_v, toolbox.attr_poisson_d, 69 | toolbox.attr_sigma_v, toolbox.attr_sigma_d, 70 | toolbox.attr_sigma_phi), n=1) 71 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 72 | 73 | pop = toolbox.population(n=n_ind) 74 | pop, log = algorithms.eaSimple(pop, toolbox, cxpb=0.5, mutpb=0.2, ngen=ngen) 75 | best = max(pop, key=attrgetter("fitness")) 76 | print(best, best.fitness) 77 | 78 | 79 | def evalFct(individual): 80 | """ Evaluation function. Should return .""" 81 | try: 82 | result = get_score_4_ind(individual) 83 | except: 84 | return 0., 85 | return result, 86 | 87 | 88 | def feasible(individual): 89 | """Feasibility function for the individual. Returns True if feasible False 90 | otherwise.""" 91 | boolie = [False] * len(individual) 92 | p = Params() 93 | i = 0 94 | 95 | for key, value in p.__dict__.items(): 96 | if value[0] <= individual[i] <= value[1]: 97 | boolie[i] = True 98 | i += 1 99 | 100 | if boolie: 101 | return True 102 | return False 103 | 104 | 105 | def checkBounds(params): 106 | def decorator(func): 107 | def wrapper(*args, **kargs): 108 | offspring = func(*args, **kargs) 109 | for child in offspring: 110 | i = 0 111 | for key, value in params.__dict__.items(): 112 | if child[i] > value[1]: 113 | child[i] = value[1] 114 | elif child[i] < value[0]: 115 | child[i] = value[0] 116 | if i >= 13 and isinstance(child[i], float): 117 | child[i] = int(round(child[i])) 118 | i += 1 119 | assert i == len(child) 120 | return offspring 121 | 122 | return wrapper 123 | 124 | return decorator 125 | 126 | 127 | def get_score_4_ind(individual): 128 | tic = time.time() 129 | if platform.system() == 'Darwin': 130 | root = '/Users/erikbohnsack/data/' 131 | else: 132 | root = '/home/mlt/data' 133 | kitti = kitti_stuff.Kitti(ROOT=root, split='training') 134 | 135 | config = ind_2_config(individual) 136 | 137 | state_dims = 2 138 | tot_score = 0 139 | # sequence_ids = random.sample(range(0, 20), 2) 140 | sequence_ids = [3, 5, 7] 141 | max_frames = 100 142 | for seq_id in sequence_ids: 143 | pmbm = PMBM(config) 144 | acc = MotCalculator(seq_id, path_to_data=root) 145 | kitti.lbls = kitti.load_labels(seq_id) 146 | imud = kitti.load_imu(seq_id) 147 | gospa_score = 0 148 | n_unique_id = 0 149 | for frame_idx in range(min(len(kitti.lbls), max_frames)): 150 | max_track_id = max([lbl.track_id for lbl in kitti.lbls[frame_idx]]) 151 | if max_track_id > n_unique_id: 152 | n_unique_id = max_track_id 153 | measurements, classes = kitti.get_measurements(frame_idx, measurement_dims=3, p_missed=0.08, p_clutter=0.02, p_mutate=0.7) 154 | pmbm.run(measurements, classes, imud[frame_idx], frame_idx, verbose=False) 155 | 156 | ground_truths = np.array([], dtype=np.float).reshape(0, 2) 157 | for l in kitti.lbls[frame_idx]: 158 | if l.type[0] == 'DontCare': 159 | continue 160 | 161 | x_pos = l.location[0] 162 | z_pos = l.location[2] 163 | ground_truths = np.vstack((ground_truths, np.array([[x_pos, z_pos]]))) 164 | 165 | estimated_states = np.array([], dtype=np.float).reshape(0, 2) 166 | for et in pmbm.estimated_targets: 167 | _state = et['single_target'].state 168 | estimated_states = np.vstack((estimated_states, _state[0:state_dims].reshape(1, state_dims))) 169 | acc.calculate(pmbm.estimated_targets, frame_idx) 170 | gospa_score += GOSPA(ground_truths, estimated_states, p=1, c=100, alpha=2., state_dim=state_dims) 171 | mh = mm.metrics.create() 172 | gospa_score /= frame_idx 173 | summary = mh.compute(acc, metrics=['num_frames', 'mota', 'motp', 'mostly_tracked', 'mostly_lost', 174 | 'num_false_positives', 'num_switches', 'num_fragmentations'], name=str(seq_id)) 175 | summary.at[str(seq_id), 'mostly_tracked'] /= n_unique_id 176 | summary.at[str(seq_id), 'mostly_lost'] /= n_unique_id 177 | summary.at[str(seq_id), 'num_false_positives'] /= n_unique_id 178 | summary.at[str(seq_id), 'num_switches'] /= n_unique_id 179 | summary.at[str(seq_id), 'num_fragmentations'] /= n_unique_id 180 | mota = summary['mota'].values 181 | motp = summary['motp'].values 182 | num_switches = summary['num_switches'].values 183 | num_false_positives = summary['num_false_positives'].values 184 | num_fragmentations = summary['num_fragmentations'].values 185 | 186 | if num_false_positives == 0: 187 | num_false_positives = 1 188 | if num_switches == 0: 189 | num_switches = 1 190 | if num_fragmentations == 0: 191 | num_fragmentations = 1 192 | tot_score += 1 / gospa_score + mota + 1 - motp + 0.5 / num_switches + 0.5 / num_false_positives + 0.5 / num_fragmentations 193 | toc = time.time() - tic 194 | print("Individual evaluated, score: {}. Time taken: {}\nIndivudial:{}".format(tot_score, toc, individual)) 195 | return tot_score 196 | 197 | 198 | def ind_2_config(individual): 199 | config = Config(config_name='Mixed-GA', 200 | motion_model='Mixed', 201 | poisson_states_model_name='uniform-mixed', 202 | filter_class='Mixed', 203 | measurement_var_xy=individual[0], 204 | measurement_var_psi=individual[1], 205 | poisson_vx=individual[2], 206 | poisson_vy=individual[3], 207 | poisson_v=individual[4], 208 | poisson_d=individual[5], 209 | sigma_phi=individual[6], 210 | sigma_v=individual[7], 211 | sigma_d=individual[8]) 212 | return config 213 | -------------------------------------------------------------------------------- /utils/logger.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import os 3 | from utils.eval_metrics import GOSPA 4 | import numpy as np 5 | import glob 6 | from .mot_metrics import MotCalculator 7 | import motmetrics as mm 8 | import pandas as pd 9 | import time 10 | from utils.coord_transf import within_fov 11 | import math 12 | 13 | class Logger: 14 | def __init__(self, sequence_idx, config_idx, filename=''): 15 | if not os.path.exists('logs'): 16 | os.mkdir('logs') 17 | 18 | self.sequence_idx = sequence_idx 19 | timestr = time.strftime("%Y%m%d-%H%M%S") 20 | 21 | self.filename = 'logs/log-' + filename + '-' + timestr 22 | if os.path.exists(self.filename): 23 | os.remove(self.filename) 24 | 25 | self.statsname = 'logs/' + 'stats_cfg_' + str(config_idx).zfill(2) + '_' + filename + '_seq' + str(sequence_idx).zfill(4) + '-' + timestr 26 | if os.path.exists(self.statsname): 27 | os.remove(self.statsname) 28 | 29 | def log_data(self, PMBM, frame_id, total_time=-1, measurements=None, true_states=None, verbose=False): 30 | data = self.encode_data(PMBM, frame_id, total_time, measurements, true_states) 31 | 32 | if verbose: 33 | self.display_data(data) 34 | 35 | with open(self.filename, 'ab') as fp: 36 | pickle.dump(data, fp) 37 | 38 | def encode_data(self, PMBM, frame_id, total_time, measurements, true_states): 39 | data = { 40 | 'state_dims': PMBM.state_dims, 41 | 'current_time': PMBM.current_time, 42 | 'measurements': measurements, 43 | 'true_states': true_states, 44 | 'estimated_targets': PMBM.estimated_targets, 45 | 'global_hypos': PMBM.global_hypotheses, 46 | 'targets': PMBM.targets, 47 | 'nof_global_hypos': len(PMBM.global_hypotheses), 48 | 'nof_targets': len(PMBM.targets), 49 | 'nof_STH': count_nof_STH(PMBM), 50 | 'nof_ET': len(PMBM.estimated_targets), 51 | 'total_time': total_time, 52 | 'frame_id': frame_id 53 | } 54 | return data 55 | 56 | def display_data(self, data): 57 | print('\t#targets: \t {} \n\t#STH: \t {} \n\t#global hypos: \t {} \n\t#est.targets \t {}'.format(data['nof_targets'], 58 | data['nof_STH'], 59 | data['nof_global_hypos'], 60 | len(data['estimated_targets']))) 61 | 62 | def log_stats(self, total_time_per_iteration, gospa_score_list, mot_summary, config, predictions_average_gospa, 63 | verbose=False): 64 | data = { 65 | 'config_name' : config.name, 66 | 'sequence_idx': self.sequence_idx, 67 | 'time_per_iter': total_time_per_iteration, 68 | 'gospa_sl': gospa_score_list, 69 | 'mot_summary' : mot_summary, 70 | 'motion_model' : config.motion_model_name, 71 | 'poisson_states_model_name' : config.poisson_states_model_name, 72 | 'filter_name' : config.filter_name, 73 | 'predictions_average_gospa' : predictions_average_gospa 74 | } 75 | if verbose: 76 | self.display_data(data) 77 | 78 | with open(self.statsname, 'wb') as fp: 79 | pickle.dump(data, fp) 80 | 81 | def count_nof_STH(PMBM): 82 | counter = 0 83 | for target in PMBM.targets.values(): 84 | for single in target.single_target_hypotheses: 85 | counter += 1 86 | return counter 87 | 88 | 89 | def load_logging_data(filename): 90 | data = [] 91 | with open(filename, 'rb') as fr: 92 | try: 93 | while True: 94 | data.append(pickle.load(fr)) 95 | except EOFError: 96 | pass 97 | return data 98 | 99 | def create_estimated_trajectory(data=False, filename=False): 100 | if filename: 101 | data = load_logging_data(filename) 102 | target_indeces = [] 103 | estimated_time = {} 104 | estimated_states = {} 105 | estimated_variances = {} 106 | estimated_predicted_states = {} 107 | estimated_predicted_variances = {} 108 | 109 | for d in data: 110 | current_time = d['current_time'] 111 | for est in d['estimated_targets']: 112 | target_idx = est['target_idx'] 113 | target_state = est['single_target'].state 114 | target_variance = est['single_target'].variance 115 | predicted_states = est['state_predictions'] 116 | predicted_variances = est['var_predictions'] 117 | if target_idx in target_indeces: 118 | estimated_states[target_idx].append(target_state) 119 | estimated_variances[target_idx].append(target_variance) 120 | estimated_time[target_idx].append(current_time) 121 | estimated_predicted_states[target_idx].append(predicted_states) 122 | estimated_predicted_variances[target_idx].append(predicted_variances) 123 | else: 124 | estimated_states[target_idx] = [target_state] 125 | estimated_variances[target_idx] = [target_variance] 126 | estimated_time[target_idx]= [current_time] 127 | estimated_predicted_states[target_idx] = [predicted_states] 128 | estimated_predicted_variances[target_idx] = [predicted_variances] 129 | target_indeces.append(target_idx) 130 | return estimated_states, estimated_variances, estimated_time, estimated_predicted_states, estimated_predicted_variances 131 | 132 | def create_hypos_over_time(data=False, filename=False): 133 | if filename: 134 | data = load_logging_data(filename) 135 | time = [] 136 | nof_globals = [] 137 | nof_targets = [] 138 | nof_sths = [] 139 | nof_ets = [] 140 | nof_gts = [] 141 | for d in data: 142 | time.append(d['current_time']) 143 | nof_globals.append(d['nof_global_hypos']) 144 | nof_targets.append(d['nof_targets']) 145 | nof_sths.append(d['nof_STH']) 146 | nof_ets.append(d['nof_ET']) 147 | nof_gts.append(len(d['true_states'])) 148 | return time, nof_globals, nof_targets, nof_sths, nof_ets, nof_gts 149 | 150 | def create_total_time_over_iteration(data=False, filename=False): 151 | if filename: 152 | data = load_logging_data(filename) 153 | total_times = [] 154 | iterations = [] 155 | for d in data: 156 | total_times.append(d['total_time']) 157 | iterations.append(d['current_time']) 158 | 159 | return iterations, total_times 160 | 161 | def data_states_2_gospa(d, gt_dims): 162 | state_dims = d['state_dims'] 163 | true_states = np.array([], dtype=np.float).reshape(0, gt_dims) 164 | for ts in d['true_states']: 165 | true_states = np.vstack((true_states, ts.reshape(1, gt_dims))) 166 | 167 | if gt_dims == state_dims: 168 | estimated_targets = d['estimated_targets'] 169 | estimated_states = np.array([], dtype=np.float).reshape(0,4) 170 | for et in estimated_targets: 171 | _state = et['single_target'].state 172 | estimated_states = np.vstack((estimated_states, _state.reshape(1,state_dims))) 173 | elif gt_dims < state_dims: 174 | estimated_targets = d['estimated_targets'] 175 | estimated_states = np.array([], dtype=np.float).reshape(0, gt_dims) 176 | for et in estimated_targets: 177 | _state = et['single_target'].state[0:gt_dims] 178 | estimated_states = np.vstack((estimated_states, _state.reshape(1, gt_dims))) 179 | else: 180 | assert 'Estimated states not as big as ground truth states, do smth about it and then come back' 181 | 182 | return true_states, estimated_states, state_dims 183 | 184 | 185 | def calculate_GOSPA_score(data=False, filename=False, gt_dims=4): 186 | if filename: 187 | data = load_logging_data(filename) 188 | total_score = 0 189 | score_list = [] 190 | for d in data: 191 | true_states, estimated_states, state_dims = data_states_2_gospa(d, gt_dims) 192 | score = GOSPA(true_states, estimated_states, p=1, c=100, alpha=2., state_dim=gt_dims) 193 | total_score += score 194 | score_list.append(score) 195 | mean_score = np.mean(score_list) 196 | print('\n==========================\n' 197 | 'Total GOSPA score: {}\n' 198 | 'Average GOSPA score: {}' 199 | '\n=========================='.format(round(total_score, 2), round(mean_score, 2))) 200 | return score_list 201 | 202 | 203 | def calculate_MOT(sequence_idx, root, data=False, filename=False, classes_to_track=['all']): 204 | if not data and not filename: 205 | raise ValueError("Neither data nor filename provided.") 206 | 207 | if filename and not data: 208 | data = load_logging_data(filename) 209 | acc = MotCalculator(sequence_idx, path_to_data=root, classes_to_track=classes_to_track) 210 | #n_unique_id = 0 211 | for d in data: 212 | frame_id = d['frame_id'] 213 | estimated_targets = d['estimated_targets'] 214 | #try: 215 | # max_track_id = max([lbl.track_id for lbl in acc.kitti.lbls[frame_id]]) 216 | #except: 217 | # max_track_id = n_unique_id 218 | #if max_track_id > n_unique_id: 219 | # n_unique_id = max_track_id 220 | acc.calculate(estimated_targets, frame_id) 221 | 222 | mh = mm.metrics.create() 223 | summary = mh.compute(acc, metrics=['num_frames', 'mota', 'motp', 'mostly_tracked', 'mostly_lost', 224 | 'num_false_positives', 'num_switches', 'num_fragmentations'], name=str(sequence_idx)) 225 | 226 | # Make MOTP higher = better 227 | summary['motp'] = 1 - summary['motp'] 228 | 229 | # If weird stuff due to no ground truth for current class_to_track happens, fix it 230 | if np.isnan(summary['motp'].values[0]): 231 | summary['motp'] = 1. 232 | if np.isnan(summary['mota'].values[0]): 233 | summary['mota'] = 1. 234 | if np.isposinf(summary['motp'].values[0]) or np.isneginf(summary['motp'].values[0]): 235 | summary['motp'] = 0. 236 | if np.isposinf(summary['mota'].values[0]) or np.isneginf(summary['mota'].values[0]): 237 | summary['mota'] = 0. 238 | 239 | #summary.at[str(sequence_idx), 'mostly_tracked'] /= n_unique_id 240 | #summary.at[str(sequence_idx), 'mostly_lost'] /= n_unique_id 241 | #summary.at[str(sequence_idx), 'num_false_positives'] /= n_unique_id 242 | #summary.at[str(sequence_idx), 'num_switches'] /= n_unique_id 243 | #summary.at[str(sequence_idx), 'num_fragmentations'] /= n_unique_id 244 | print(summary) 245 | return summary 246 | 247 | 248 | def prediction_stats(sequence_idx, config, kitti, data=False, filename=False): 249 | if not data and not filename: 250 | raise ValueError("Neither data nor filename provided.") 251 | if filename and not data: 252 | data = load_logging_data(filename) 253 | 254 | imu_dict = kitti.imus 255 | 256 | dt = config.dt 257 | gt_dims = 2 258 | print('Sequence: {}'.format(sequence_idx)) 259 | all_true_states = [] 260 | for d in data: 261 | all_true_states.append(d['true_states']) 262 | 263 | gospa_scores = np.zeros(config.show_predictions) 264 | for d in data: 265 | #print('\tCurrent time: {}'.format(d['current_time'])) 266 | if d['current_time'] > len(data) - config.show_predictions: 267 | #print('*** No more ground truths to work with... Aborting ***') 268 | break 269 | 270 | future_ego_position = np.array([0, 0], dtype=np.float).reshape(gt_dims, 1) 271 | track_indeces = [] 272 | 273 | for t in range(config.show_predictions): 274 | state_dims = d['state_dims'] 275 | imu_data = imu_dict[d['current_time'] + t] 276 | angle = imu_data.ru * dt 277 | rotation_matrix = np.array([[math.cos(angle), - math.sin(angle)], 278 | [math.sin(angle), math.cos(angle)]]) 279 | translation = np.array([[-imu_data.vl * dt], 280 | [imu_data.vf * dt]]) 281 | future_ego_position = rotation_matrix @ future_ego_position + translation 282 | 283 | true_states = np.array([], dtype=np.float).reshape(0, gt_dims) 284 | for lbl in kitti.lbls[d['current_time'] + t]: 285 | if t == 0: 286 | track_indeces.append(lbl.track_id) 287 | if not lbl.track_id in track_indeces: 288 | continue 289 | 290 | ts = np.array([[lbl.location[0]], [lbl.location[2]]]) 291 | _ts = ts + future_ego_position 292 | true_states = np.vstack((true_states, _ts.reshape(1, gt_dims))) 293 | 294 | estimated_states = np.array([], dtype=np.float).reshape(0, gt_dims) 295 | for et in d['estimated_targets']: 296 | _state = et['state_predictions'][t][0:gt_dims] 297 | 298 | if within_fov(_state, min_angle=config.uniform_angle[0], 299 | max_angle=config.uniform_angle[1], 300 | max_radius=config.uniform_radius): 301 | estimated_states = np.vstack((estimated_states, _state.reshape(1, gt_dims))) 302 | 303 | score = GOSPA(true_states, estimated_states, p=1, c=100, alpha=2., state_dim=gt_dims) 304 | gospa_scores[t] += score 305 | 306 | mean_gospas = gospa_scores / (len(data) - config.show_predictions) 307 | 308 | return gospa_scores, mean_gospas 309 | #print('\tTotal GOSPAs: \n\t{}\n\tAverage GOSPAs: \n\t{}'.format(gospa_scores, mean_gospas)) 310 | 311 | 312 | def fafe_prediction_stats(sequence_idx, kitti, data=False, filename=False, num_conseq_frames=5): 313 | if not data and not filename: 314 | raise ValueError("Neither data nor filename provided.") 315 | if filename and not data: 316 | data = load_logging_data(filename) 317 | 318 | imu_dict = kitti.imus 319 | dt = 0.1 320 | gt_dims = 2 321 | print('Sequence: {}'.format(sequence_idx)) 322 | all_true_states = [] 323 | for d in data: 324 | all_true_states.append(d['true_states']) 325 | 326 | gospa_scores = np.zeros(num_conseq_frames) 327 | for d in data: 328 | future_ego_position = np.array([0, 0], dtype=np.float).reshape(gt_dims, 1) 329 | track_indeces = [] 330 | 331 | for t in range(num_conseq_frames): 332 | state_dims = d['state_dims'] 333 | imu_data = imu_dict[d['current_time'] + t] 334 | angle = imu_data.ru * dt 335 | rotation_matrix = np.array([[math.cos(angle), - math.sin(angle)], 336 | [math.sin(angle), math.cos(angle)]]) 337 | translation = np.array([[-imu_data.vl * dt], 338 | [imu_data.vf * dt]]) 339 | future_ego_position = rotation_matrix @ future_ego_position + translation 340 | 341 | true_states = np.array([], dtype=np.float).reshape(0, gt_dims) 342 | for lbl in kitti.lbls[d['current_time'] + t]: 343 | if t == 0: 344 | track_indeces.append(lbl.track_id) 345 | if not lbl.track_id in track_indeces: 346 | continue 347 | 348 | ts = np.array([[lbl.location[0]], [lbl.location[2]]]) 349 | _ts = ts + future_ego_position 350 | true_states = np.vstack((true_states, _ts.reshape(1, gt_dims))) 351 | 352 | estimated_states = np.array([], dtype=np.float).reshape(0, gt_dims) 353 | for et in d['estimated_targets']: 354 | if t >= len(et['state_predictions']): 355 | continue 356 | 357 | _state = et['state_predictions'][t][0:gt_dims] 358 | 359 | if within_fov(_state, min_angle=0.78, 360 | max_angle=2.35, 361 | max_radius=100): 362 | estimated_states = np.vstack((estimated_states, _state.reshape(1, gt_dims))) 363 | 364 | score = GOSPA(true_states, estimated_states, p=1, c=100, alpha=2., state_dim=gt_dims) 365 | gospa_scores[t] += score 366 | 367 | mean_gospas = gospa_scores / len(data) 368 | 369 | return gospa_scores, mean_gospas 370 | 371 | 372 | def create_sequences_stats(filenames_prefix='logs/stats_seq'): 373 | filenames = glob.glob(filenames_prefix + '*') 374 | sequences = [] 375 | avg_times = [] 376 | avg_gospa = [] 377 | mota = [] 378 | motp = [] 379 | columns = ['SeqId', 'CfgName', 'MotionModel', 'Filter', 'PoissonModel', '#Frames', 'Pase', 'MOTA', 'MOTP', 'GOSPA', 380 | 'MostlyTracked', 'MostlyLost', '#FalsePos', '#Switches', '#Fragmentations', 'PredGOSPA'] 381 | rows = [] 382 | for ix, filename in enumerate(filenames): 383 | data = load_logging_data(filename) 384 | 385 | d = data[0] 386 | sequences.append(d['sequence_idx']) 387 | avg_times.append(np.mean(d['time_per_iter'])) 388 | avg_gospa.append(np.mean(d['gospa_sl'])) 389 | mota.append(float(d['mot_summary']['mota'].values)) 390 | motp.append(float(d['mot_summary']['motp'].values)) 391 | df = d['mot_summary'] 392 | df['gospa'] = np.mean(d['gospa_sl']) 393 | df['avg_times'] =np.mean(d['time_per_iter']) 394 | df['motion_model'] = d['motion_model'] 395 | df['poisson_model'] = d['poisson_states_model_name'] 396 | df['filter'] = d['filter_name'] 397 | 398 | _seqid = d['sequence_idx'] 399 | _name = d['config_name'] 400 | _motionmodel = d['motion_model'] 401 | _filter = d['filter_name'] 402 | _poissonmodel = d['poisson_states_model_name'] 403 | _nframes = int(df['num_frames'].values) 404 | _pase = round(float(np.mean(d['time_per_iter'])), 3) 405 | _mota = round(float(d['mot_summary']['mota'].values), 2) 406 | _motp = round(float(d['mot_summary']['motp'].values), 2) 407 | _gospa = round(float(np.mean(d['gospa_sl'])), 2) 408 | _pred_gospa = d['predictions_average_gospa'] 409 | _mostly_tracked = round(float(df['mostly_tracked'].values), 2) 410 | _mostly_lost = round(float(df['mostly_lost'].values), 2) 411 | _nfalsepos = round(float(df['num_false_positives'].values), 2) 412 | _nswitches = round(float(df['num_switches'].values), 2) 413 | _nfrags = round(float(df['num_fragmentations'].values), 2) 414 | 415 | row = [_seqid, _name, _motionmodel, _filter, _poissonmodel, _nframes, _pase, _mota, _motp, _gospa, _mostly_tracked, 416 | _mostly_lost, _nfalsepos, _nswitches, _nfrags, _pred_gospa] 417 | 418 | rows.append(row) 419 | stats_df = pd.DataFrame(np.array(rows), 420 | columns=columns) 421 | 422 | return sequences, avg_times, avg_gospa, mota, motp, stats_df 423 | 424 | def create_sequence_dataframe(filenames_prefix='logs/stats_seq'): 425 | filenames = glob.glob(filenames_prefix + '*') 426 | columns = ['SeqId', 'CfgName', 'MotionModel', 'Filter', 'PoissonModel', '#Frames', 'Pase', 'MOTA', 'MOTP', 'GOSPA', 427 | 'MostlyTracked', 'MostlyLost', '#FalsePos', '#Switches', '#Fragmentations', 'PredGOSPA'] 428 | rows = [] 429 | for ix, filename in enumerate(filenames): 430 | data = load_logging_data(filename) 431 | d = data[0] 432 | df = d['mot_summary'] 433 | _seqid = d['sequence_idx'] 434 | _name = d['config_name'] 435 | _motionmodel = d['motion_model'] 436 | _filter = d['filter_name'] 437 | _poissonmodel = d['poisson_states_model_name'] 438 | _nframes = int(df['num_frames'].values) 439 | _pase = round(float(np.mean(d['time_per_iter'])), 3) 440 | _mota = round(float(d['mot_summary']['mota'].values), 2) 441 | _motp = round(float(d['mot_summary']['motp'].values), 2) 442 | _gospa = round(float(np.mean(d['gospa_sl'])), 2) 443 | _pred_gospa = d['predictions_average_gospa'] 444 | _mostly_tracked = round(float(df['mostly_tracked'].values), 2) 445 | _mostly_lost = round(float(df['mostly_lost'].values), 2) 446 | _nfalsepos = round(float(df['num_false_positives'].values), 2) 447 | _nswitches = round(float(df['num_switches'].values), 2) 448 | _nfrags = round(float(df['num_fragmentations'].values), 2) 449 | row = [_seqid, _name, _motionmodel, _filter, _poissonmodel, _nframes, _pase, _mota, _motp, _gospa, _mostly_tracked, 450 | _mostly_lost, _nfalsepos, _nswitches, _nfrags, _pred_gospa] 451 | rows.append(row) 452 | stats_df = pd.DataFrame(np.array(rows), 453 | columns=columns) 454 | return stats_df 455 | 456 | 457 | def measurements_from_log(data, frame_idx): 458 | return data[frame_idx]['measurements'] 459 | 460 | 461 | 462 | 463 | -------------------------------------------------------------------------------- /utils/matrix_stuff.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from math import pi 3 | EPSILON = 1e-6 4 | 5 | 6 | def nearPSD(A, epsilon=0): 7 | n = A.shape[0] 8 | eigval, eigvec = np.linalg.eig(A) 9 | val = np.matrix(np.maximum(eigval,epsilon)) 10 | vec = np.matrix(eigvec) 11 | T = 1/(np.multiply(vec,vec) * val.T) 12 | T = np.matrix(np.sqrt(np.diag(np.array(T).reshape((n)) ))) 13 | B = T * vec * np.diag(np.array(np.sqrt(val)).reshape((n))) 14 | out = B*B.T 15 | return(out) 16 | 17 | 18 | def is_pos_def(x): 19 | return np.all(np.linalg.eigvals(x) > 0) 20 | 21 | 22 | def log_sum(weight_array): 23 | ''' 24 | weight_sum = log_sum(weight_array) 25 | 26 | Sum of logarithmic components 27 | w_sum = w_smallest + log( 1 + sum(exp(w_rest - w_smallest)) ) 28 | ''' 29 | weight_array.sort() 30 | _w0 = weight_array[0] 31 | _wr = weight_array[1:] 32 | _wdelta = _wr - _w0 33 | _exp = np.exp(_wdelta) 34 | _sum = np.sum(_exp) 35 | _weight = _w0 + np.log(1 + _sum) 36 | 37 | return _weight 38 | 39 | 40 | def switch_state_direction(state): 41 | output_state = np.copy(state) 42 | if state[3] < 0: 43 | if output_state[2] > 0: 44 | output_state[2] -= pi 45 | else: 46 | output_state[2] += pi 47 | output_state[3] = abs(output_state[3]) 48 | return output_state 49 | 50 | -------------------------------------------------------------------------------- /utils/moment_matching.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def moment_matching_dists(list_of_distributions, list_of_weights): 5 | 6 | normalized_weights = [float(weight)/sum(list_of_weights) for weight in list_of_weights] 7 | 8 | _state = np.zeros(np.shape(list_of_distributions[0].state)) 9 | _variance = np.zeros(np.shape(list_of_distributions[0].variance)) 10 | _weight = sum(list_of_weights) 11 | 12 | for index, distribution in enumerate(list_of_distributions): 13 | _state += distribution.state * normalized_weights[index] 14 | for index, distribution in enumerate(list_of_distributions): 15 | _delta_state = distribution.state - _state 16 | _variance += normalized_weights[index] * ( distribution.variance + _delta_state @ _delta_state.transpose() ) 17 | # For numerical stability 18 | _variance = 0.5 * (_variance + _variance.transpose()) 19 | 20 | return _state, _variance, _weight 21 | 22 | 23 | def moment_matching(states_within_gate, variances_within_gate, weight_within_gate): 24 | normalized_weights = weight_within_gate / np.sum(weight_within_gate) 25 | _state = np.zeros((np.shape(states_within_gate[0]))) 26 | _variance = np.zeros((np.shape(variances_within_gate[0]))) 27 | _weight = sum(weight_within_gate) 28 | for _i in range(len(states_within_gate)): 29 | _state += states_within_gate[_i] * normalized_weights[_i] 30 | for _i in range(len(states_within_gate)): 31 | _delta_state = states_within_gate[_i] - _state 32 | _variance += normalized_weights[_i] * (variances_within_gate[_i] + _delta_state @ _delta_state.transpose()) 33 | 34 | # For numerical stability 35 | _variance = 0.5 * (_variance + _variance.transpose()) 36 | 37 | return _state, _variance, _weight 38 | -------------------------------------------------------------------------------- /utils/mot_metrics.py: -------------------------------------------------------------------------------- 1 | import motmetrics as mm 2 | import numpy as np 3 | from data_utils import kitti_stuff 4 | 5 | 6 | class MotCalculator(mm.MOTAccumulator): 7 | def __init__(self, sequence_idx, auto_id=False, path_to_data='/Users/erikbohnsack/data', split='training', 8 | classes_to_track=['all']): 9 | super().__init__(auto_id=auto_id) 10 | 11 | self.kitti = kitti_stuff.Kitti(ROOT=path_to_data, split=split) 12 | self.kitti.lbls = self.kitti.load_labels(sequence_idx) 13 | self.classes_to_track = classes_to_track 14 | 15 | def calculate(self, estimated_targets, frameid): 16 | """ 17 | Calculates 18 | :param estimated_targets: list of dicts. [{'single_target':, 'single_hypo_idx':, 'target_idx':},...] 19 | :param frameid: 20 | :return: 21 | """ 22 | 23 | # Rearrange ground truths to be able to input to PyMotMetrics 24 | ground_truth = self.kitti.lbls[frameid] 25 | gt_ids = [] 26 | o = np.array([], dtype=np.float).reshape(0, 2) 27 | h = np.array([], dtype=np.float).reshape(0, 2) 28 | for gt_label in ground_truth: 29 | if gt_label.type[0] == 'DontCare': 30 | continue 31 | if 'all' not in self.classes_to_track and gt_label.type[0] not in self.classes_to_track: 32 | continue 33 | 34 | gt_ids.append(gt_label.track_id) 35 | 36 | o = np.vstack((o, np.array([[gt_label.location[0], gt_label.location[2]]]))) 37 | 38 | # Rearrange estimated targets to be able to input to PyMotMetrics 39 | est_ids = [] 40 | for target in estimated_targets: 41 | est_ids.append(target['target_idx']) 42 | state = target['single_target'].state.T 43 | if state.shape[1] > 2: 44 | state = state[:, 0:2] 45 | assert state.shape == (1, 2), 'State not row vector. ' 46 | h = np.vstack((h, state)) 47 | 48 | # Calculate dists 49 | dists = mm.distances.norm2squared_matrix(o, h, max_d2=5.) 50 | 51 | self.update(oids=gt_ids, hids=est_ids, dists=dists, frameid=frameid) 52 | 53 | return 54 | 55 | 56 | #mh = mm.metrics.create() 57 | 58 | #summary = mh.compute(acc, metrics=['num_frames', 'mota', 'motp'], name='acc') 59 | 60 | -------------------------------------------------------------------------------- /utils/motion_models.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | 5 | class BicycleModel: 6 | """ 7 | State vector: 8 | [ x, 9 | y, 10 | psi, 11 | V, 12 | df 13 | 14 | """ 15 | 16 | def __init__(self, dt, bike_lr, bike_lf, car_lr, car_lf, pedestrian_lr, pedestrian_lf, tram_lr, tram_lf, 17 | truck_lr, truck_lf, van_lr, van_lf, 18 | sigma_xy_bicycle, sigma_phi, sigma_v, sigma_d): 19 | self.dt = dt 20 | self.bike_lr = bike_lr 21 | self.bike_lf = bike_lf 22 | self.car_lr = car_lr 23 | self.car_lf = car_lf 24 | self.pedestrian_lr = pedestrian_lr 25 | self.pedestrian_lf = pedestrian_lf 26 | self.tram_lr = tram_lr 27 | self.tram_lf = tram_lf 28 | self.truck_lr = truck_lr 29 | self.truck_lf = truck_lf 30 | self.van_lr = van_lr 31 | self.van_lf = van_lf 32 | 33 | self.Q = ([[sigma_xy_bicycle, 0.19450606, 0.054374531, 0.033518521, 0.0814456], 34 | [0.19450606, sigma_xy_bicycle, 0.054374531, 0.033518521, 0.0814456], 35 | [0.054374531, 0.054374531, sigma_phi, 0.027877464, 0.003820711], 36 | [0.033518521, 0.033518521, 0.027877464, sigma_v, 0.0005], 37 | [0.0814456, 0.0814456, 0.003820711, 0.0005, sigma_d]]) 38 | # self.Q[2, 2] = sigma_phi 39 | # self.Q[3, 3] = sigma_v 40 | # self.Q[4, 4] = sigma_d 41 | self.model = 0 42 | 43 | def __call__(self, state, variance, dt=None, object_class='Car'): 44 | assert state.shape[0] == variance.shape[0], "State vector and covariance matrix is misaligned" 45 | if dt is None: 46 | dt = self.dt 47 | if object_class == 'Car': 48 | lr = self.car_lr 49 | lf = self.car_lf 50 | elif object_class == 'Cyclist': 51 | lr = self.bike_lr 52 | lf = self.bike_lf 53 | elif object_class == 'Van': 54 | lr = self.van_lr 55 | lf = self.van_lf 56 | elif object_class == 'Pedestrian' or object_class == 'Person': 57 | lr = self.pedestrian_lr 58 | lf = self.pedestrian_lf 59 | elif object_class == 'Truck': 60 | lr = self.truck_lr 61 | lf = self.truck_lf 62 | elif object_class == 'Tram': 63 | lr = self.tram_lr 64 | lf = self.tram_lf 65 | elif object_class == 'Misc': # TODO: see what is the best lr/lf here 66 | lr = self.bike_lr 67 | lf = self.bike_lf 68 | else: 69 | raise ValueError("Input a valid object you fool. {} is certainly not one of them.".format(object_class)) 70 | 71 | beta = math.atan2(lr * math.tan(state[4]), (lr + lf)) 72 | x = state[0] + state[3] * math.cos(state[2] + beta) * dt 73 | y = state[1] + state[3] * math.sin(state[2] + beta) * dt 74 | psi = state[2] + state[3] / lr * math.sin(beta) * dt 75 | return np.array([x, y, psi, state[3], state[4]]) 76 | 77 | def get_Q(self, object_class=None): 78 | return self.Q 79 | 80 | 81 | class ConstantVelocityModel: 82 | """Nearly constant velocity motion model.""" 83 | """ States = [x y v_x v_y] """ 84 | 85 | def __init__(self, motion_noise, dt): 86 | """Init.""" 87 | self.q = motion_noise 88 | self.dim = 4 # Dimension of this model... 89 | self.model = 1 90 | 91 | self.F = np.array([[1, 0, dt, 0], 92 | [0, 1, 0, dt], 93 | [0, 0, 1, 0], 94 | [0, 0, 0, 1]]) 95 | 96 | self.Q = np.array([[dt ** 3 / 3, 0, dt ** 2 / 2, 0], 97 | [0, dt ** 3 / 3, 0, dt ** 2 / 2], 98 | [dt ** 2 / 2, 0, dt, 0], 99 | [0, dt ** 2 / 2, 0, dt]]) * self.q 100 | 101 | def __call__(self, state, variance, dt=None, object_class='Car'): 102 | """Step model.""" 103 | # Check if the state vector is correctly represented so that matrix multiplication will work as intended... 104 | try: 105 | if not np.shape(state)[0] == self.dim: 106 | raise ValueError( 107 | 'Faulty state dimensions in CV Model! \n Need: dim{} \n Got: dim{} \n Current state: \n {}'.format( 108 | self.dim, np.shape(state)[0], state)) 109 | 110 | if not (np.shape(variance)[0] == self.dim and np.shape(variance)[1] == self.dim): 111 | raise ValueError( 112 | 'Faulty variance dimensions in CV Model! \n Need: dim{},{} \n Got: dim{},{} \n Current variance: \n {}'.format( 113 | self.dim, self.dim, np.shape(variance)[0], np.shape(variance)[1], variance)) 114 | except IndexError as e: 115 | print('State: {}'.format(state)) 116 | print('Variance: {}'.format(variance)) 117 | raise e 118 | if dt is None: 119 | F = self.F 120 | Q = self.Q 121 | else: 122 | F = np.array([[1, 0, dt, 0], 123 | [0, 1, 0, dt], 124 | [0, 0, 1, 0], 125 | [0, 0, 0, 1]]) 126 | Q = np.array([[dt ** 3 / 3, 0, dt ** 2 / 2, 0], 127 | [0, dt ** 3 / 3, 0, dt ** 2 / 2], 128 | [dt ** 2 / 2, 0, dt, 0], 129 | [0, dt ** 2 / 2, 0, dt]]) * self.q 130 | new_state = F @ state 131 | new_variance = F @ variance @ F.transpose() + Q 132 | return new_state, new_variance 133 | 134 | def get_Q(self, object_class=None): 135 | return self.Q 136 | 137 | def __repr__(self): 138 | return ''.format(self.q) 139 | 140 | 141 | class LinearWalk2D: 142 | def __init__(self, motion_noise): 143 | """Init.""" 144 | self.dim = 2 # Dimension of this model... 145 | 146 | self.F = np.eye(self.dim) 147 | self.Q = motion_noise 148 | self.model = 2 149 | 150 | def __call__(self, state, variance, dt=None, object_class='Car'): 151 | # Check if the state vector is correctly represented so that matrix multiplication will work as intended... 152 | if not np.shape(state)[0] == self.dim: 153 | raise ValueError( 154 | 'Faulty state dimensions in LW2D Model! \n Need: {} \n Got: {}'.format(self.dim, np.shape(state)[0])) 155 | 156 | new_state = self.F @ state 157 | new_variance = self.F @ variance @ self.F.transpose() + self.Q 158 | return new_state, new_variance 159 | 160 | 161 | class MixedModel: 162 | def __init__(self, dt, motion_noise, bike_lr, bike_lf, car_lr, car_lf, pedestrian_lr, pedestrian_lf, tram_lr, 163 | tram_lf, 164 | truck_lr, truck_lf, van_lr, van_lf, 165 | sigma_xy_bicycle, sigma_phi, sigma_v, sigma_d): 166 | self.BM = BicycleModel(dt=dt, 167 | bike_lr=bike_lr, bike_lf=bike_lf, 168 | car_lr=car_lr, car_lf=car_lf, 169 | pedestrian_lr=pedestrian_lr, pedestrian_lf=pedestrian_lf, 170 | tram_lr=tram_lr, tram_lf=tram_lf, 171 | truck_lr=truck_lr, truck_lf=truck_lf, 172 | van_lr=van_lr, van_lf=van_lf, 173 | sigma_xy_bicycle=sigma_xy_bicycle, 174 | sigma_phi=sigma_phi, 175 | sigma_v=sigma_v, 176 | sigma_d=sigma_d) 177 | 178 | self.CV = ConstantVelocityModel(motion_noise=motion_noise, dt=dt) 179 | self.model = 1 180 | 181 | def __call__(self, state, variance, object_class): 182 | if object_class == 'Pedestrian' or object_class == 'Misc' or object_class == 'Person': 183 | new_state, new_variance = self.CV(state=state, variance=variance, object_class=object_class) 184 | else: 185 | new_state = self.BM(state=state, variance=variance, object_class=object_class) 186 | new_variance = [None] 187 | 188 | return new_state, new_variance 189 | 190 | def get_Q(self, object_class): 191 | if object_class == 'Pedestrian' or object_class == 'Misc' or object_class == 'Person': 192 | return self.CV.Q 193 | else: 194 | return self.BM.Q 195 | 196 | 197 | class ConstantAccelerationModel: 198 | """Nearly constant velocity motion model.""" 199 | """ States = [x y dx dy ddx ddy] """ 200 | 201 | def __init__(self, motion_noise, dt): 202 | """Init.""" 203 | self.q = motion_noise 204 | self.dim = 6 # Dimension of this model... 205 | self.model = 1 206 | 207 | self.F = np.array([[1, 0, dt, 0, dt ** 2 / 2, 0], 208 | [0, 1, 0, dt, 0, dt ** 2 / 2], 209 | [0, 0, 1, 0, dt, 0], 210 | [0, 0, 0, 1, 0, dt], 211 | [0, 0, 0, 0, 1, 0], 212 | [0, 0, 0, 0, 0, 1]]) 213 | 214 | self.Q = np.array([[dt ** 5 / 20, 0, dt ** 4 / 8, 0, dt ** 3 / 6, 0], 215 | [0, dt ** 5 / 20, 0, dt ** 4 / 8, 0, dt ** 3 / 6], 216 | [dt ** 4 / 8, 0, dt ** 3 / 3, 0, dt ** 2 / 2, 0], 217 | [0, dt ** 4 / 8, 0, dt ** 3 / 3, 0, dt ** 2 / 2], 218 | [dt ** 3 / 6, 0, dt ** 2 / 2, 0, dt, 0], 219 | [0, dt ** 3 / 6, 0, dt ** 2 / 2, 0, dt]]) * self.q 220 | 221 | def __call__(self, state, variance, dt=None, object_class='Car'): 222 | """Step model.""" 223 | # Check if the state vector is correctly represented so that matrix multiplication will work as intended... 224 | try: 225 | if not np.shape(state)[0] == self.dim: 226 | raise ValueError( 227 | 'Faulty state dimensions in CV Model! \n Need: dim{} \n Got: dim{} \n Current state: \n {}'.format( 228 | self.dim, np.shape(state)[0], state)) 229 | 230 | if not (np.shape(variance)[0] == self.dim and np.shape(variance)[1] == self.dim): 231 | raise ValueError( 232 | 'Faulty variance dimensions in CV Model! \n Need: dim{},{} \n Got: dim{},{} \n Current variance: \n {}'.format( 233 | self.dim, self.dim, np.shape(variance)[0], np.shape(variance)[1], variance)) 234 | except IndexError as e: 235 | print('State: {}'.format(state)) 236 | print('Variance: {}'.format(variance)) 237 | raise e 238 | if dt is None: 239 | F = self.F 240 | Q = self.Q 241 | else: 242 | F = np.array([[1, 0, dt, 0, dt ** 2 / 2, 0], 243 | [0, 1, 0, dt, 0, dt ** 2 / 2], 244 | [0, 0, 1, 0, dt, 0], 245 | [0, 0, 0, 1, 0, dt], 246 | [0, 0, 0, 0, 1, 0], 247 | [0, 0, 0, 0, 0, 1]]) 248 | 249 | Q = np.array([[dt ** 5 / 20, 0, dt ** 4 / 8, 0, dt ** 3 / 6, 0], 250 | [0, dt ** 5 / 20, 0, dt ** 4 / 8, 0, dt ** 3 / 6], 251 | [dt ** 4 / 8, 0, dt ** 3 / 3, 0, dt ** 2 / 2, 0], 252 | [0, dt ** 4 / 8, 0, dt ** 3 / 3, 0, dt ** 2 / 2], 253 | [dt ** 3 / 6, 0, dt ** 2 / 2, 0, dt, 0], 254 | [0, dt ** 3 / 6, 0, dt ** 2 / 2, 0, dt]]) * self.q 255 | new_state = F @ state 256 | new_variance = F @ variance @ F.transpose() + Q 257 | return new_state, new_variance 258 | 259 | def get_Q(self, object_class=None): 260 | return self.Q 261 | 262 | def __repr__(self): 263 | return ''.format(self.q) -------------------------------------------------------------------------------- /utils/plot_stuff.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib.patches import Ellipse 3 | from .logger import load_logging_data, create_estimated_trajectory, create_hypos_over_time 4 | from .logger import measurements_from_log, create_total_time_over_iteration, create_sequences_stats 5 | from .logger import create_sequence_dataframe 6 | import matplotlib.pyplot as plt 7 | from data_utils.kitti_stuff import Kitti 8 | import matplotlib.pyplot as plt 9 | import matplotlib.patches as patches 10 | import utils.constants 11 | from matplotlib import gridspec 12 | import matplotlib as mpl 13 | import os, datetime 14 | import pandas as pd 15 | import cv2 16 | import glob 17 | 18 | XLIM = utils.constants.XLIM 19 | ZLIM = utils.constants.ZLIM 20 | 21 | 22 | def plot_estimated_targets(data=None, filename=None, measurements=None, true_trajectories=None, save=False): 23 | estimated_states, estimated_variances, estimated_time = create_estimated_trajectory(data, filename) 24 | fig, ax = plt.subplots(1, 3) 25 | fig.set_size_inches(20, 20 / 3, forward=True) 26 | 27 | for key, est in estimated_states.items(): 28 | _x = [e[0] for e in est] 29 | _y = [e[1] for e in est] 30 | _vx = [e[2] for e in est] 31 | _vy = [e[3] for e in est] 32 | _t = estimated_time[key] 33 | 34 | ax[0].plot(_x, _y, 'o-', lw=2, ms=2) 35 | ax[1].plot(_t, _vx, 'o-', lw=2, ms=2) 36 | ax[2].plot(_t, _vy, 'o-', lw=2, ms=2) 37 | 38 | if not true_trajectories is None: 39 | for tt in true_trajectories: 40 | t_x = [x[0] for x in tt] 41 | t_y = [x[1] for x in tt] 42 | ax[0].plot(t_x, t_y, 'ko-', lw=0.5, ms=2) 43 | v_x = [x[2] for x in tt] 44 | v_y = [x[3] for x in tt] 45 | t = np.arange(0, len(v_x), 1) 46 | ax[1].plot(t, v_x, 'ko-', lw=0.5, ms=2) 47 | ax[2].plot(t, v_y, 'ko-', lw=0.5, ms=2) 48 | 49 | if not measurements is None: 50 | for current in measurements: 51 | for meas in current: 52 | try: 53 | m_x = meas[0] 54 | m_y = meas[1] 55 | except: 56 | continue 57 | ax[0].plot(m_x, m_y, 'k+', ms=4, mfc='none') 58 | 59 | ax[0].set_xlabel('x') 60 | ax[0].set_ylabel('y') 61 | ax[0].set_title('Position') 62 | ax[1].set_xlabel('t') 63 | ax[1].set_ylabel('vx') 64 | ax[1].set_title('x-Velocity') 65 | ax[2].set_xlabel('t') 66 | ax[2].set_ylabel('vy') 67 | ax[2].set_title('y-Velocity') 68 | 69 | 70 | def plot_hypos_over_time(data=None, filename=None, save=False, config_name=None): 71 | time, nof_globals, nof_targets, nof_sths, nof_ets, nof_gts = create_hypos_over_time(data, filename) 72 | fig, ax = plt.subplots(1, 4) 73 | fig.set_size_inches(20, 20 / 4, forward=True) 74 | ax[0].plot(time, nof_globals, 'o-', lw=1, ms=2) 75 | ax[1].plot(time, nof_targets, 'o-', lw=1, ms=2) 76 | ax[2].plot(time, nof_sths, 'o-', lw=1, ms=2) 77 | ax[3].plot(time, nof_ets, 'bo-', lw=1, ms=2, label='et') 78 | ax[3].plot(time, nof_gts, 'ro-', lw=1, ms=2, label='gt') 79 | 80 | ax[0].set_xlabel('Time step') 81 | ax[0].set_ylabel('#') 82 | ax[0].set_title('#Global hypotheses') 83 | ax[1].set_xlabel('Time step') 84 | ax[1].set_ylabel('#') 85 | ax[1].set_title('#Tracks') 86 | ax[2].set_xlabel('Time step') 87 | ax[2].set_ylabel('#') 88 | ax[2].set_title('#STHs') 89 | ax[3].set_xlabel('Time step') 90 | ax[3].set_ylabel('#') 91 | ax[3].set_title('#Estimated targets') 92 | ax[3].legend() 93 | 94 | if config_name is not None: 95 | if not os.path.exists('showroom'): 96 | os.mkdir('showroom') 97 | fig.savefig('showroom/hypos_over_time_' + config_name + '.pdf') 98 | plt.show(fig) 99 | 100 | 101 | def plot_total_time_per_iteration(data=None, filename=None, save=False, config_name=None): 102 | iterations, total_times = create_total_time_over_iteration(data, filename) 103 | fig, ax = plt.subplots(1, 1) 104 | fig.set_size_inches(5, 5, forward=True) 105 | ax.plot(iterations, total_times, 'o-', lw=1, ms=2) 106 | ax.set_xlabel('iteration k') 107 | ax.set_ylabel('time [s]') 108 | ax.set_title('Time to run one iteration') 109 | 110 | if config_name is not None: 111 | if not os.path.exists('showroom'): 112 | os.mkdir('showroom') 113 | fig.savefig('showroom/total_time_' + config_name + '.pdf') 114 | plt.show(fig) 115 | 116 | 117 | def plot_time_score(data=None, filename=None, save=False, score_list=None, config_name=None): 118 | iterations, total_times = create_total_time_over_iteration(data, filename) 119 | fig, ax = plt.subplots(1, 2) 120 | fig.set_size_inches(10, 5, forward=True) 121 | ax[0].plot(iterations, total_times, 'o-', lw=1, ms=2) 122 | ax[0].set_xlabel('Time step') 123 | ax[0].set_ylabel('time [s]') 124 | ax[0].set_title('Time to run one iteration') 125 | 126 | ax[1].plot(iterations, score_list, 'o-', lw=1, ms=2) 127 | ax[1].set_xlabel('Time step') 128 | ax[1].set_ylabel('score [-]') 129 | ax[1].set_title('GOSPA Score') 130 | 131 | if config_name is not None: 132 | if not os.path.exists('showroom'): 133 | os.mkdir('showroom') 134 | fig.savefig('showroom/time_score_' + config_name + '.pdf') 135 | plt.show(fig) 136 | 137 | 138 | def plot_target_life(data=None, filename=None, save=False, config_name=None): 139 | estimated_states, estimated_variances, estimated_time, _, _ = create_estimated_trajectory(data, filename) 140 | fig, ax = plt.subplots(1, 1) 141 | fig.set_size_inches(5, 15, forward=True) 142 | for key, et_time in estimated_time.items(): 143 | key_vector = np.ones((len(et_time, ))) * key 144 | ax.plot(et_time, key_vector, '-', lw=2) 145 | ax.set_xlabel('Time step') 146 | ax.set_ylabel('Track ID') 147 | ax.set_title('Life span of each track') 148 | 149 | if config_name is not None: 150 | if not os.path.exists('showroom'): 151 | os.mkdir('showroom') 152 | fig.savefig('showroom/target_life_' + config_name + '.pdf') 153 | plt.show(fig) 154 | 155 | 156 | def plot_tracking_history(path, sequence_idx, data=False, filename_log=False, kitti=Kitti, 157 | final_frame_idx=None, disp='show', only_alive=False, show_cov=False, show_predictions=None, 158 | config_name='', car_van_flag=False, fafe=False, num_conseq_frames=None): 159 | 160 | if fafe and num_conseq_frames is None: 161 | raise ValueError("Fafe needs num conseq frames") 162 | if not data and not filename_log: 163 | raise ValueError("Neither data or filename to log file specified. ") 164 | 165 | if filename_log and not data: 166 | data = load_logging_data(filename_log) 167 | 168 | if not os.path.exists(path): 169 | os.mkdir(path) 170 | 171 | fig, ax = plt.subplots(2, 1) 172 | gs = gridspec.GridSpec(2, 1, height_ratios=[1, 4]) 173 | fig.set_size_inches(10, 15, forward=True) 174 | 175 | img = kitti.load_image(sequence_idx=sequence_idx, frame_idx=final_frame_idx) 176 | ax[0].imshow(img) 177 | ax[0].grid(False) 178 | 179 | ego_vehicle = patches.Rectangle((-0.5, -2), 1, 4, color="blue", alpha=0.50) 180 | ax[1].add_patch(ego_vehicle) 181 | 182 | for l in kitti.lbls[final_frame_idx]: 183 | 184 | if car_van_flag: 185 | if l.type[0] not in ['Car', 'Van']: 186 | continue 187 | else: 188 | if l.type[0] == 'DontCare': 189 | continue 190 | 191 | x_pos = l.location[0] 192 | z_pos = l.location[2] 193 | pos = np.array([x_pos, z_pos]) 194 | width = l.dimensions[1] 195 | length = l.dimensions[2] 196 | 197 | if x_pos <= XLIM[0] or x_pos >= XLIM[1] or z_pos <= ZLIM[0] or z_pos >= ZLIM[1]: 198 | continue 199 | 200 | rot_y = l.rotation_y 201 | _xm = - width / 2 202 | _zm = - length / 2 203 | _xp = width / 2 204 | _zp = length / 2 205 | 206 | _bbox = np.matrix([[_xm, _zm], [_xm, _zp], [_xp, _zp], [_xp, _zm]]) 207 | _phi = np.pi / 2 - rot_y 208 | _rotm = np.matrix([[np.cos(_phi), -np.sin(_phi)], [np.sin(_phi), np.cos(_phi)]]) 209 | _rotated_bbox = (_rotm * _bbox.T).T + pos 210 | r = patches.Polygon(_rotated_bbox, color="red", alpha=0.2) 211 | ax[1].add_patch(r) 212 | ax[1].text(x_pos + 0.5 * width, z_pos + 0.5 * length, str(l.track_id), color='black') 213 | ax[1].plot(x_pos, z_pos, 'r.', ms=0.5) 214 | 215 | # Plot current measurements. If fafe the measurements is lined up differently. 216 | if fafe: 217 | meas_frame_idx = final_frame_idx - num_conseq_frames + 1 218 | else: 219 | meas_frame_idx = final_frame_idx 220 | measurements = measurements_from_log(data=data, frame_idx=meas_frame_idx) 221 | for meas in measurements: 222 | ax[1].plot(meas[0], meas[1], 'rs', markerfacecolor='none') 223 | 224 | es, ev, et, eps, epv = create_estimated_trajectory(data=data) 225 | 226 | for tid, state in es.items(): 227 | frame_indeces = et[tid] 228 | if only_alive: 229 | if final_frame_idx not in frame_indeces: 230 | continue 231 | last_idx_to_plot = frame_indeces.index(final_frame_idx) 232 | states_to_plot = [] 233 | for idx, frame_idx in enumerate(frame_indeces[0:last_idx_to_plot + 1]): 234 | current_state = state[idx][0:2] 235 | for i in range(frame_idx, final_frame_idx): 236 | current_velocity = kitti.get_ego_bev_velocity(frame_idx=i) 237 | current_rotation = kitti.get_ext_bev_rotation(frame_idx=i) 238 | current_state = current_rotation @ (current_state - kitti.dT * current_velocity) 239 | states_to_plot.append(current_state) 240 | _ex = [x[0, 0] for x in states_to_plot] 241 | _ez = [x[1, 0] for x in states_to_plot] 242 | _c = cnames[tid % len(cnames)] 243 | ax[1].plot(_ex, _ez, color=_c, linewidth=2) 244 | ax[1].plot(_ex[last_idx_to_plot], _ez[last_idx_to_plot], color=_c, marker='o', markerfacecolor='none', ms=5) 245 | if not (_ex[-1] <= XLIM[0] or _ex[-1] >= XLIM[1] or _ez[-1] <= ZLIM[0] or _ez[-1] >= ZLIM[1]): 246 | ax[1].text(_ex[last_idx_to_plot] - 1, _ez[last_idx_to_plot] + 1, str(tid), color=_c) 247 | 248 | if show_cov: 249 | _cov = ev[tid][last_idx_to_plot][0:2, 0:2] 250 | _cnt = np.array([[_ex[last_idx_to_plot]], [_ez[last_idx_to_plot]]]) 251 | _ = plot_cov_ellipse(_cov, _cnt, nstd=3, ax=ax[1], alpha=0.5, color=_c) 252 | 253 | if show_predictions is not None: 254 | _pex = [x[0, 0] for x in eps[tid][last_idx_to_plot]] 255 | _pez = [x[1, 0] for x in eps[tid][last_idx_to_plot]] 256 | ax[1].plot(_pex, _pez, linestyle='--', marker='^', color=_c, linewidth=0.5, ms=4) 257 | 258 | ax[1].set_xlim(XLIM[0], XLIM[1]) 259 | ax[1].set_ylim(ZLIM[0], ZLIM[1]) 260 | ax[1].grid(True) 261 | plt.tight_layout() 262 | 263 | if disp == 'show': 264 | plt.show() 265 | elif disp == 'save': 266 | fig.savefig( 267 | path + '/' + config_name + '_track_seq_' + str(sequence_idx).zfill(4) + '_frame_' + str(final_frame_idx).zfill( 268 | 4) + '.png') 269 | plt.close(fig) 270 | else: 271 | assert 'Noob' 272 | 273 | # return es, ev, et, eps, epv 274 | 275 | 276 | def plot_sequence_stats(filenames_prefix='logs/stats', disp='show'): 277 | sequences, avg_times, avg_gospa, mota, motp, df = create_sequences_stats(filenames_prefix=filenames_prefix) 278 | fig, ax = plt.subplots(1, 4) 279 | fig.set_size_inches(20, 5, forward=True) 280 | ax[0].bar(sequences, avg_times) 281 | ax[1].bar(sequences, avg_gospa) 282 | ax[2].bar(sequences, mota) 283 | ax[3].bar(sequences, motp) 284 | 285 | ax[0].set_xlabel('Sequence [idx]') 286 | ax[0].set_ylabel('Time [s]') 287 | ax[0].set_title('Average time per time step') 288 | ax[1].set_xlabel('Sequence [idx]') 289 | ax[1].set_ylabel('Mean GOSPA score') 290 | ax[1].set_title('Mean GOSPA') 291 | ax[2].set_xlabel('Sequence [idx]') 292 | ax[2].set_ylabel('MOTA score') 293 | ax[2].set_title('MOTA') 294 | ax[3].set_xlabel('Sequence [idx]') 295 | ax[3].set_ylabel('MOTP score') 296 | ax[3].set_title('MOTP') 297 | 298 | # For pred gospas (prego) 299 | fig_prego, ax_prego = plt.subplots(1, 1) 300 | fig_prego.set_size_inches(10, 10, forward=True) 301 | ax_prego.set_xlabel('Timesteps ahead') 302 | ax_prego.set_ylabel('Average GOSPA') 303 | time_steps_ahead = np.array(range(1, len(df.PredGOSPA[0])+1)) 304 | for a in df.PredGOSPA: 305 | ax_prego.plot(time_steps_ahead, a, '-o') 306 | 307 | if disp == 'show': 308 | plt.show() 309 | elif disp == 'save': 310 | if not os.path.exists('showroom'): 311 | os.mkdir('showroom') 312 | fig.savefig('showroom/sequence_stats' + '.pdf') 313 | fig_prego.savefig('showroom/prego_stats' + '.pdf') 314 | plt.close(fig) 315 | plt.close(fig_prego) 316 | elif disp == 'all': 317 | if not os.path.exists('showroom'): 318 | os.mkdir('showroom') 319 | fig.savefig('showroom/sequence_stats' + '.pdf') 320 | fig_prego.savefig('showroom/prego_stats' + '.pdf') 321 | plt.show() 322 | else: 323 | assert 'Noob' 324 | 325 | 326 | return df 327 | 328 | 329 | def sequence_analysis(filenames_prefix='logs/stats', sortby='MotionModel'): 330 | df = create_sequence_dataframe(filenames_prefix) 331 | 332 | unique_values = np.unique(df.filter(items=[sortby]).values) 333 | n_unique_sequences = len(np.unique(df.filter(items=['SeqId']).values)) 334 | 335 | if len(unique_values) < 2: 336 | print('Found 1 unique setting. Plotting sequence stats...') 337 | return plot_sequence_stats(filenames_prefix=filenames_prefix, disp='save'), None 338 | 339 | nof_uv = len(unique_values) 340 | print('Found {} unique settings and {} unique sequences. Plotting sequence stats...'.format(nof_uv, n_unique_sequences)) 341 | 342 | row_names = ['AvgPase', 'AvgMOTA', 'AvgMOTP', 'AvgGOSPA', 'AvgMT', 'AvgSwitches', 'AvgML'] 343 | 344 | #fig, ax = plt.subplots(7, nof_uv, sharey='row') 345 | fig, ax = plt.subplots(6, nof_uv, sharey='row') 346 | inch_per_ax = 2 347 | fig.set_size_inches(int(inch_per_ax * nof_uv), inch_per_ax * len(row_names), forward=True) 348 | 349 | # For pred gospas (prego) 350 | fig_prego, ax_prego = plt.subplots(1, 1) 351 | plt.xticks(fontsize=30) 352 | plt.yticks(fontsize=30) 353 | fig_prego.set_size_inches(10, 10, forward=True) 354 | #time_steps_ahead = np.array(range(1, len(df.PredGOSPA[0]) + 1)) 355 | 356 | # For pred gospas (prego) 357 | fig_sub_prego, ax_sub_prego = plt.subplots(n_unique_sequences, 1) 358 | fig_sub_prego.set_size_inches(7, n_unique_sequences*7, forward=True) 359 | 360 | rows = [] 361 | max_avg_id_switches = 0 362 | for i, uv in enumerate(unique_values): 363 | row = [] 364 | a = df.loc[df[sortby] == uv] 365 | a = a.sort_values('SeqId') 366 | 367 | seqIds = [int(b) for b in a.SeqId.values] 368 | pases = [float(b) for b in a.Pase.values] 369 | motas = [float(b) for b in a.MOTA.values] 370 | motps = [float(b) for b in a.MOTP.values] 371 | gospas = [float(b) for b in a.GOSPA.values] 372 | predicted_gospas = a.PredGOSPA.values 373 | mts = [float(b) for b in a.MostlyTracked.values] 374 | mls = [float(b) for b in a.MostlyLost.values] 375 | switches = [float(b) for b in a['#Switches'].values] 376 | 377 | time_steps_ahead = np.array(range(1, len(predicted_gospas[0]) + 1)) 378 | 379 | ax[0, i].bar(seqIds, pases) 380 | _mean = np.mean(pases) 381 | ax[0, i].axhline(y=_mean, color='r') 382 | row.append(_mean) 383 | 384 | ax[1, i].bar(seqIds, motas) 385 | _mean = np.mean(motas) 386 | ax[1, i].axhline(y=_mean, color='r') 387 | row.append(_mean) 388 | 389 | _mean = np.mean(motps) 390 | ax[2, i].bar(seqIds, motps) 391 | ax[2, i].axhline(y=_mean, color='r') 392 | row.append(_mean) 393 | 394 | _mean = np.mean(gospas) 395 | ax[3, i].bar(seqIds, gospas) 396 | ax[3, i].axhline(y=_mean, color='r') 397 | row.append(_mean) 398 | 399 | _mean = np.mean(mts) 400 | ax[4, i].bar(seqIds, mts) 401 | ax[4, i].axhline(y=_mean, color='r') 402 | row.append(_mean) 403 | 404 | _mean = np.mean(switches) 405 | ax[5, i].bar(seqIds, switches) 406 | ax[5, i].axhline(y=_mean, color='r') 407 | row.append(_mean) 408 | if _mean > max_avg_id_switches: 409 | max_avg_id_switches = _mean 410 | 411 | _mean = np.mean(mls) 412 | #ax[5, i].bar(seqIds, mls) 413 | #ax[5, i].axhline(y=_mean, color='r') 414 | row.append(_mean) 415 | 416 | rows.append(row) 417 | 418 | ax[0, i].set_xlabel('Sequence [idx]') 419 | ax[0, i].set_title(uv + '\nAvg time/iter') 420 | 421 | ax[1, i].set_xlabel('Sequence [idx]') 422 | ax[1, i].set_title(uv + '\nMOTA') 423 | 424 | ax[2, i].set_xlabel('Sequence [idx]') 425 | ax[2, i].set_title(uv + '\nMOTP') 426 | 427 | ax[3, i].set_xlabel('Sequence [idx]') 428 | ax[3, i].set_title(uv + '\nMean GOSPA') 429 | 430 | ax[4, i].set_xlabel('Sequence [idx]') 431 | ax[4, i].set_title(uv + '\nMostly Tracked') 432 | 433 | ax[5, i].set_xlabel('Sequence [idx]') 434 | ax[5, i].set_title(uv + '\nID Switches') 435 | 436 | #ax[6, i].set_xlabel('Sequence [idx]') 437 | #ax[6, i].set_title(uv + '\nMostly Lost') 438 | 439 | if uv == 'CV': 440 | style = '-o' 441 | elif uv == 'Mixed': 442 | style = ':s' 443 | elif uv == 'BC': 444 | style = '--*' 445 | elif uv == 'CA' : 446 | style = '-.+' 447 | elif uv == 'Car-CV': 448 | style = '-o' 449 | elif uv == 'Ped-CV': 450 | style = ':s' 451 | elif uv == 'Car-BC': 452 | style = '--*' 453 | elif uv == 'Ped-BC': 454 | style = '-.+' 455 | else: 456 | style = '-x' 457 | mean_pg = np.zeros(len(predicted_gospas[0]) , dtype=float) 458 | for ix, pg in enumerate(predicted_gospas): 459 | if n_unique_sequences != 1: 460 | ax_sub_prego[ix].plot(time_steps_ahead, pg, style, label=uv + '-seq' + str(seqIds[ix])) 461 | ax_sub_prego[ix].set_xlabel('Time steps ahead') 462 | ax_sub_prego[ix].set_ylabel('Average prediction GOSPA') 463 | else: 464 | ax_sub_prego.plot(time_steps_ahead, pg, style, label=uv + '-seq' + str(seqIds[ix])) 465 | ax_sub_prego.set_xlabel('Time steps ahead') 466 | ax_sub_prego.set_ylabel('Average prediction GOSPA') 467 | mean_pg = np.add(mean_pg, pg) 468 | mean_pg = mean_pg / n_unique_sequences 469 | ax_prego.plot(time_steps_ahead, mean_pg, style, label=uv) 470 | 471 | ax[0, 0].set_ylabel('Time [s]') 472 | ax[1, 0].set_ylabel('MOTA score') 473 | ax[2, 0].set_ylabel('MOTP score') 474 | ax[3, 0].set_ylabel('Mean GOSPA score') 475 | ax[4, 0].set_ylabel('Mostly Tracked') 476 | ax[5, 0].set_ylabel('nof ID Switches') 477 | #ax[6, 0].set_ylabel('Mostly Lost') 478 | 479 | ax[1, 0].set_ylim([0, 1]) 480 | ax[2, 0].set_ylim([0, 1]) 481 | 482 | ax[5, 0].set_ylim([0, max_avg_id_switches*1.2]) 483 | 484 | ax_prego.set_xlabel('Time steps ahead', fontsize=30) 485 | ax_prego.set_ylabel('Average GOSPA score', fontsize=30) 486 | ax_prego.set_title('Average Prediction GOSPA over sequences: \n {}'.format(seqIds)) 487 | ax_prego.legend(fontsize=30) 488 | 489 | if n_unique_sequences != 1: 490 | for ix in range(n_unique_sequences): 491 | ax_sub_prego[ix].legend() 492 | ax_sub_prego[ix].set_title('Sequence ' + str(seqIds[ix])) 493 | else: 494 | ax_sub_prego.legend() 495 | ax_sub_prego.set_title('Sequence ' + str(seqIds[ix])) 496 | 497 | fig.tight_layout() 498 | fig_prego.tight_layout() 499 | fig_sub_prego.tight_layout() 500 | 501 | if not os.path.exists('showroom'): 502 | os.mkdir('showroom') 503 | fig.savefig('showroom/sequence_stats' + '.pdf') 504 | fig_prego.savefig('showroom/prego_stats' + '.pdf') 505 | fig_sub_prego.savefig('showroom/prego_sub_stats' + '.pdf') 506 | 507 | #plt.show() 508 | 509 | I = pd.Index(row_names, name="Metric") 510 | C = pd.Index(unique_values, name=sortby) 511 | 512 | avg_df = pd.DataFrame(data=rows, index=C, columns=I) 513 | 514 | # PLOT AVERAGE STUFF for pase, mota, motp and gospa 515 | to_plot = ['AvgPase', 'AvgMOTA', 'AvgMOTP', 'AvgGOSPA'] 516 | titles = ['Average Time per iteration', 'Average MOTA', 'Average MOTP', 'Average GOSPA'] 517 | for mi, metric in enumerate(to_plot): 518 | avg_fig, avg_ax = plt.subplots(1, 1, sharey='row') 519 | avg_fig.set_size_inches(10, 10, forward=True) 520 | styles = [] 521 | names = [] 522 | values = [] 523 | for name in avg_df[metric].keys(): 524 | # For comparing motion models 525 | if name == 'BC': 526 | style = '#1f77b4' 527 | elif name == 'CA': 528 | style = '#ff7f0e' 529 | elif name == 'CV': 530 | style = '#2ca02c' 531 | elif name == 'Mixed': 532 | style = '#d62728' 533 | # For car vs pedestrian tracking 534 | elif name == 'Car-BC': 535 | style = '#1f77b4' 536 | elif name == 'Car-CV': 537 | style = '#ff7f0e' 538 | elif name == 'Ped-BC': 539 | style = '#2ca02c' 540 | elif name == 'Ped-CV': 541 | style = '#d62728' 542 | # For FAFE variants 543 | elif name == 'bev_NN': 544 | style = '#1f77b4' 545 | elif name == 'bev_nn': 546 | style = '#ff7f0e' 547 | elif name == 'pp_NN': 548 | style = '#2ca02c' 549 | elif name == 'pp_nn': 550 | style = '#d62728' 551 | # For fafe vs pmbm 552 | elif name == 'PMBM-CV-4': 553 | style = '#1f77b4' 554 | else: 555 | style = 'k' 556 | styles.append(style) 557 | names.append(name) 558 | values.append(avg_df[metric][name]) 559 | avg_ax.bar(names, values, color=styles, alpha=1) 560 | for i, v in enumerate(values): 561 | avg_ax.text(names[i], v, str(round(v,3)), horizontalalignment='center', fontsize=40) 562 | 563 | avg_ax.set_title(titles[mi], fontsize=40) 564 | plt.xticks(fontsize=45) 565 | plt.yticks(fontsize=45) 566 | avg_fig.tight_layout() 567 | avg_fig.savefig('showroom/average_' + metric + '.pdf') 568 | #plt.show() 569 | avg_fig.clear() 570 | 571 | return df, avg_df 572 | 573 | 574 | def compare_pmbm_fafe_gospas(pmbm_sl, fafe_filename, pmbm_pred_gospa): 575 | 576 | pmbm_iterations = np.arange(0, len(pmbm_sl)) 577 | 578 | fig, ax = plt.subplots(2, 1) 579 | fig.set_size_inches(10, 10, forward=True) 580 | 581 | with open(fafe_filename, 'r') as f: 582 | lines = f.read().splitlines() 583 | 584 | gospa_scores = [] 585 | 586 | max_frame = 0 587 | for line in lines: 588 | l = line.split(' ') 589 | row = [] 590 | for i in l[1:]: 591 | row.append(float(i)) 592 | gospa_scores.append(row) 593 | 594 | if int(l[0]) > max_frame: 595 | max_frame = int(l[0]) 596 | 597 | min_frame = int(lines[0].split(' ')[0]) 598 | gospa_scores = np.array(gospa_scores) 599 | 600 | fafe_iterations = np.arange(min_frame, max_frame+1) 601 | # Prediction average gospa... 602 | fafe_pred_gospa = np.mean(gospa_scores, axis=0) 603 | 604 | fafe_sl = gospa_scores[:,0] 605 | 606 | ax[0].plot(pmbm_iterations, pmbm_sl, 'o-', lw=1, ms=2, label='PMBM') 607 | ax[0].plot(fafe_iterations, fafe_sl, 's-', lw=1, ms=2, label='FaFe') 608 | 609 | ax[0].set_xlabel('iteration k') 610 | ax[0].set_ylabel('score [-]') 611 | ax[0].set_title('GOSPA Score') 612 | ax[0].legend() 613 | 614 | pmbm_time_steps_ahead = np.array(range(1, len(pmbm_pred_gospa) + 1)) 615 | fafe_timesteps_ahead = np.array(range(1, min_frame + 2)) 616 | 617 | ax[1].plot(pmbm_time_steps_ahead, pmbm_pred_gospa, 'o-', lw=2, ms=3, label='PMBM') 618 | ax[1].plot(fafe_timesteps_ahead, fafe_pred_gospa, 's-', lw=2, ms=3, label='FaFe') 619 | ax[1].set_xlabel('Timesteps ahead') 620 | ax[1].set_ylabel('score [-]') 621 | ax[1].set_title('GOSPA Prediction Score') 622 | ax[1].legend() 623 | 624 | fig.tight_layout() 625 | fig.savefig('showroom/gospa_comparison' + '.pdf') 626 | plt.show() 627 | 628 | def get_cov_ellipse(cov, centre, nstd, **kwargs): 629 | """ 630 | Return a matplotlib Ellipse patch representing the covariance matrix 631 | cov centred at centre and scaled by the factor nstd. 632 | 633 | USAGE: 634 | fig, ax = plt.subplots() 635 | e = get_cov_ellipse(.) 636 | ax.add_artist(e) 637 | plt.show() 638 | """ 639 | 640 | # Find and sort eigenvalues and eigenvectors into descending order 641 | eigvals, eigvecs = np.linalg.eigh(cov) 642 | order = eigvals.argsort()[::-1] 643 | eigvals, eigvecs = eigvals[order], eigvecs[:, order] 644 | 645 | # The anti-clockwise angle to rotate our ellipse by 646 | vx, vy = eigvecs[:, 0][0], eigvecs[:, 0][1] 647 | theta = np.arctan2(vy, vx) 648 | 649 | # Width and height of ellipse to draw 650 | width, height = 2 * nstd * np.sqrt(eigvals) 651 | return Ellipse(xy=centre, width=width, height=height, 652 | angle=np.degrees(theta), **kwargs) 653 | 654 | 655 | def make_movie_from_images(path): 656 | video_name = "".join((path, ".mov")) 657 | images = [img for img in os.listdir(path) if img.endswith(".png")] 658 | images.sort() 659 | frame = cv2.imread(os.path.join(path, images[0])) 660 | height, width, layers = frame.shape 661 | video = cv2.VideoWriter(video_name, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 10, (width, height)) 662 | for image in images: 663 | video.write(cv2.imread(os.path.join(path, image))) 664 | 665 | cv2.destroyAllWindows() 666 | video.release() 667 | 668 | 669 | def plot_bernoulli(local_bernoulli=None, measurements=None): 670 | fig, ax = plt.subplots() 671 | 672 | # Plot Prior state 673 | ax.plot(local_bernoulli.state[0], local_bernoulli.state[1], 'ko', label='Prior State') 674 | 675 | # Plot all measurements 676 | for meas in measurements: 677 | ax.plot(meas[0], meas[1], 'rs', label='meas') 678 | 679 | # Plot output states as long as they are not None 680 | # (if None they have been gated away in Bernoulli Class) 681 | for ix, st in enumerate(local_bernoulli.output_states): 682 | if st is None: 683 | continue 684 | ax.plot(st[0], st[1], 'g.', label='posterior', linewidth=2) 685 | 686 | _e = get_cov_ellipse(local_bernoulli.output_variance[ix], (st[0], st[1]), 3, 687 | alpha=local_bernoulli.output_weights[ix]) 688 | ax.add_artist(_e) 689 | 690 | # TODO: make this plotting great again 691 | ax.set(xlabel='x', ylabel='y', title='hejhopp') 692 | 693 | ax.grid() 694 | # ax.set_xlim() 695 | # ax.set_ylim() 696 | plt.show() 697 | 698 | 699 | def plot_poissons(poisson): 700 | fig = plt.figure() 701 | ax = fig.add_subplot(111) 702 | 703 | xlim = 15 704 | ylim = 30 705 | 706 | for dist in poisson.distributions: 707 | plot_cov_ellipse(dist.variance[0:2, 0:2], dist.state[0:2], nstd=2, ax=ax) 708 | 709 | plt.xlim(-xlim, xlim) 710 | plt.ylim(-0.25 * ylim, 1.75 * ylim) 711 | plt.grid(True) 712 | 713 | plt.show() 714 | 715 | 716 | def plot_cov_ellipse(cov, pos, nstd=2, ax=None, alpha=1, color='black', **kwargs): 717 | """ 718 | Plots an `nstd` sigma error ellipse based on the specified covariance 719 | matrix (`cov`). Additional keyword arguments are passed on to the 720 | ellipse patch artist. 721 | 722 | Parameters 723 | ---------- 724 | cov : The 2x2 covariance matrix to base the ellipse on 725 | pos : The location of the center of the ellipse. Expects a 2-element 726 | sequence of [x0, y0]. 727 | nstd : The radius of the ellipse in numbers of standard deviations. 728 | Defaults to 2 standard deviations. 729 | ax : The axis that the ellipse will be plotted on. Defaults to the 730 | current axis. 731 | Additional keyword arguments are pass on to the ellipse patch. 732 | 733 | Returns 734 | ------- 735 | A matplotlib ellipse artist 736 | """ 737 | 738 | def eigsorted(cov): 739 | vals, vecs = np.linalg.eigh(cov) 740 | order = vals.argsort()[::-1] 741 | return vals[order], vecs[:, order] 742 | 743 | if ax is None: 744 | ax = plt.gca() 745 | 746 | vals, vecs = eigsorted(cov) 747 | theta = np.degrees(np.arctan2(*vecs[:, 0][::-1])) 748 | 749 | # Width and height are "full" widths, not radius 750 | width, height = 2 * nstd * np.sqrt(vals) 751 | ellip = Ellipse(xy=pos, width=width, height=height, angle=theta, alpha=alpha, color=color) 752 | 753 | ax.add_artist(ellip) 754 | return ellip 755 | 756 | 757 | cnames = ['black' 758 | , 'blue' 759 | , 'blueviolet' 760 | , 'brown' 761 | , 'cadetblue' 762 | , 'chartreuse' 763 | , 'chocolate' 764 | , 'coral' 765 | , 'cornflowerblue' 766 | , 'crimson' 767 | , 'darkblue' 768 | , 'darkcyan' 769 | , 'darkgoldenrod' 770 | , 'darkgray' 771 | , 'darkgreen' 772 | , 'darkkhaki' 773 | , 'darkmagenta' 774 | , 'darkolivegreen' 775 | , 'darkorange' 776 | , 'darkorchid' 777 | , 'darkred' 778 | , 'darksalmon' 779 | , 'darkseagreen' 780 | , 'darkslateblue' 781 | , 'darkslategray' 782 | , 'darkturquoise' 783 | , 'darkviolet' 784 | , 'deeppink' 785 | , 'deepskyblue' 786 | , 'dimgray' 787 | , 'dodgerblue' 788 | , 'firebrick' 789 | , 'forestgreen' 790 | , 'fuchsia' 791 | , 'gainsboro' 792 | , 'gold' 793 | , 'goldenrod' 794 | , 'gray' 795 | , 'green' 796 | , 'greenyellow' 797 | , 'hotpink' 798 | , 'indianred' 799 | , 'indigo' 800 | , 'lawngreen' 801 | , 'lightgreen' 802 | , 'lime' 803 | , 'limegreen' 804 | , 'magenta' 805 | , 'maroon' 806 | , 'mediumaquamarine' 807 | , 'mediumblue' 808 | , 'mediumorchid' 809 | , 'mediumpurple' 810 | , 'mediumseagreen' 811 | , 'mediumslateblue' 812 | , 'mediumspringgreen' 813 | , 'mediumturquoise' 814 | , 'mediumvioletred' 815 | , 'midnightblue' 816 | , 'navy' 817 | , 'olive' 818 | , 'olivedrab' 819 | , 'orange' 820 | , 'orangered' 821 | , 'orchid' 822 | , 'palegoldenrod' 823 | , 'palegreen' 824 | , 'palevioletred' 825 | , 'peru' 826 | , 'purple' 827 | , 'red' 828 | , 'rosybrown' 829 | , 'royalblue' 830 | , 'saddlebrown' 831 | , 'salmon' 832 | , 'sandybrown' 833 | , 'seagreen' 834 | , 'sienna' 835 | , 'slateblue' 836 | , 'slategray' 837 | , 'snow' 838 | , 'springgreen' 839 | , 'steelblue' 840 | , 'teal' 841 | , 'tomato' 842 | , 'turquoise' 843 | , 'violet' 844 | , 'yellowgreen'] 845 | -------------------------------------------------------------------------------- /utils/poisson_birth_state_models.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from itertools import product 3 | 4 | 5 | def get_model(model_name): 6 | if model_name == 'noob': 7 | _states = [np.array([[-45], [-45], [10], [5]]), 8 | np.array([[-45], [45], [10], [-5]]), 9 | np.array([[-45], [-45], [2.5], [2.5]]), 10 | np.array([[-45], [45], [5], [-5]]), 11 | np.array([[-45], [-45], [15], [10]]), 12 | np.array([[-45], [45], [7], [-7]]), 13 | np.array([[-45], [-45], [10], [5]]), 14 | np.array([[-45], [-25], [10], [5]]), 15 | np.array([[-38], [-22], [5], [5]]), 16 | np.array([[-40], [-25], [1], [1]]), 17 | np.array([[-45], [45], [15], [-10]])] 18 | _variance = 1 * np.eye(4) 19 | return _states, _variance 20 | 21 | elif model_name == 'noob2': 22 | _states = [np.array([[-45], [45]]), 23 | np.array([[-40], [35]]), 24 | np.array([[-42], [40]]), 25 | np.array([[-45], [-30]]), 26 | np.array([[-45], [-35]]), 27 | np.array([[-45], [-45]]), 28 | np.array([[-30], [30]]), 29 | np.array([[-20], [20]]), 30 | np.array([[-35], [35]]), 31 | np.array([[-30], [-30]]), 32 | np.array([[-20], [-20]]), 33 | np.array([[-35], [-35]])] 34 | _variance = np.eye(2) 35 | return _states, _variance 36 | 37 | elif model_name == 'datadriven': 38 | r_one = [np.array([[3], [y], [0], [0]]) for y in range(5, 80, 5)] 39 | r_two = [np.array([[8], [y], [0], [0]]) for y in range(5, 70, 5)] 40 | r_three = [np.array([[13], [y], [0], [0]]) for y in range(25, 60, 5)] 41 | l_one = [np.array([[-3], [y], [0], [0]]) for y in range(5, 85, 5)] 42 | l_two = [np.array([[-8], [y], [0], [0]]) for y in range(5, 85, 5)] 43 | l_three = [np.array([[-13], [y], [0], [0]]) for y in range(20, 80, 5)] 44 | l_four = [np.array([[-18], [y], [0], [0]]) for y in range(25, 70, 5)] 45 | l_five = [np.array([[-23], [y], [0], [0]]) for y in range(40, 70, 5)] 46 | 47 | _left = [np.array([[-12], [12], [0], [0]]), 48 | np.array([[-16], [20], [0], [0]]), 49 | np.array([[-20], [28], [0], [0]]), 50 | np.array([[-25], [30], [0], [0]]), 51 | np.array([[-24], [36], [0], [0]])] 52 | 53 | _right = [np.array([[12], [12], [0], [0]]), 54 | np.array([[16], [20], [0], [0]]), 55 | np.array([[20], [28], [0], [0]]), 56 | np.array([[25], [30], [0], [0]]), 57 | np.array([[24], [36], [0], [0]])] 58 | 59 | _states = r_one + r_two + r_three + l_one + l_two + _left + _right + l_three + l_four + l_five 60 | _variance = 3 * np.eye(4) 61 | _variance[2,2] = _variance[2,2] * 2 62 | _variance[3,3] = _variance[3,3] * 2 63 | 64 | return _states, _variance 65 | elif model_name == 'bicycle': 66 | r_one = [np.array([[3], [y], [0], [0], [0]]) for y in range(5, 80, 5)] 67 | r_two = [np.array([[8], [y], [0], [0], [0]]) for y in range(5, 70, 5)] 68 | r_three = [np.array([[13], [y], [0], [0], [0]]) for y in range(25, 60, 5)] 69 | l_one = [np.array([[-3], [y], [0], [0], [0]]) for y in range(5, 85, 5)] 70 | l_two = [np.array([[-8], [y], [0], [0], [0]]) for y in range(5, 85, 5)] 71 | l_three = [np.array([[-13], [y], [0], [0], [0]]) for y in range(20, 80, 5)] 72 | l_four = [np.array([[-18], [y], [0], [0], [0]]) for y in range(25, 70, 5)] 73 | l_five = [np.array([[-23], [y], [0], [0], [0]]) for y in range(40, 70, 5)] 74 | 75 | _left = [np.array([[-12], [12], [0], [0], [0]]), 76 | np.array([[-16], [20], [0], [0], [0]]), 77 | np.array([[-20], [28], [0], [0], [0]]), 78 | np.array([[-24], [36], [0], [0], [0]])] 79 | 80 | _right = [np.array([[12], [12], [0], [0], [0]]), 81 | np.array([[16], [20], [0], [0], [0]]), 82 | np.array([[20], [28], [0], [0], [0]]), 83 | np.array([[24], [36], [0], [0], [0]])] 84 | 85 | _states = r_one + r_two + r_three + l_one + l_two + _left + _right + l_three + l_four + l_five 86 | _variance = 0.5 * np.eye(5) 87 | _variance[2, 2] = 10 88 | _variance[3, 3] = 3 89 | _variance[4, 4] = 3 90 | return _states, _variance 91 | 92 | elif model_name == 'kitti': 93 | # TODO: Make as inputs (maybe) 94 | grid_size = [-30, 30, -5, 45] 95 | nx = 30 96 | nz = 30 97 | x = np.linspace(grid_size[0], grid_size[1], nx) 98 | z = np.linspace(grid_size[2], grid_size[3], nz) 99 | 100 | t = [0, -10, 10] 101 | velocity_combinations = set(product(set(t), repeat=2)) 102 | 103 | _states = [] 104 | for i in range(len(x)): 105 | for j in range(len(z)): 106 | for vel in velocity_combinations: 107 | _s = np.array([[x[i]], [z[j]], [vel[0]], [vel[1]]]) 108 | _states.append(_s) 109 | 110 | _variance = 2 * np.eye(4) 111 | _variance[2, 2] = _variance[2, 2] * 3 112 | _variance[3, 3] = _variance[2, 2] * 3 113 | 114 | return _states, _variance 115 | 116 | else: 117 | return None, None 118 | -------------------------------------------------------------------------------- /utils/test_UKF.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import unittest 3 | from .UKF import UnscentedKalmanFilter as UKF 4 | from .motion_models import BicycleModel 5 | from filterpy.kalman import MerweScaledSigmaPoints 6 | from math import pi 7 | 8 | 9 | class TestUKF(unittest.TestCase): 10 | def test_propagate(self): 11 | dt = 1 12 | bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d = (1., 1., 1., 1., 1., 1., 1.) 13 | model = BicycleModel(dt, bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d) 14 | 15 | 16 | dim_x = 5 17 | dim_z = 2 18 | points = MerweScaledSigmaPoints(dim_x, alpha=1, beta=2., kappa=2) 19 | self.assertAlmostEqual(sum(points.Wm), 1) 20 | print() 21 | print(points.Wm) 22 | dt = 1 23 | measurement_model = np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]]) 24 | measurement_noise = 1 * np.eye(2) 25 | state = np.array([1., 1., pi/2, 1., 0]) 26 | variance = np.eye(dim_x) * 0.2 # initial uncertainty\ 27 | kf = UKF(dim_x=dim_x, dim_z=dim_z, dt=dt, points=points, motion_model=model, measurement_model=measurement_model) 28 | 29 | z_std = 0.1 30 | kf.R = np.diag([z_std ** 2, z_std ** 2]) # 1 standard 31 | kf.Q = np.eye(dim_x) 32 | zs = [[i + np.random.randn() * z_std, i + np.random.randn() * z_std] for i in range(50)] # measurements 33 | 34 | _state, _var = kf.predict(state, variance, model, motion_noise=kf.Q, object_class='Car') 35 | print("State: {}".format(_state)) 36 | print("Var: {}".format(_var)) 37 | #hoppsan = kf.update(x, P, measurement) 38 | 39 | 40 | -------------------------------------------------------------------------------- /utils/test_coord_transf.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import platform 3 | from data_utils.kitti_stuff import Kitti, IMU 4 | from .coord_transf import coordinate_transform_bicycle 5 | import numpy as np 6 | from math import pi, sqrt 7 | 8 | if platform.system() == 'Darwin': 9 | root = '/Users/erikbohnsack/data' 10 | else: 11 | root = '/home/mlt/data' 12 | 13 | 14 | class TestCoordTransf(unittest.TestCase): 15 | def test_coord_transf(self): 16 | state = np.ones((5, 1)) 17 | imud = IMU(0, 1, -1, 0) 18 | dt = 1 19 | transformed_state = coordinate_transform_bicycle(state, imud, dt) 20 | assert (transformed_state[0:2] == np.zeros((2, 1))).all() 21 | 22 | def test_coord_transf_angle(self): 23 | state = np.array([[2.], [2.], [0.], [0.], [0.]]) 24 | imud = IMU(0, 0., 0., pi / 2) 25 | dt = 1 26 | transformed_state = coordinate_transform_bicycle(state, imud, dt) 27 | np.testing.assert_allclose(transformed_state[0:2], np.array([[2.], [-2.]])) 28 | 29 | def test_coord_trans_kitti(self): 30 | sequence_id = 0 31 | kitti = Kitti(root) 32 | imud = kitti.load_imu(sequence_id) 33 | kitti.lbls = kitti.load_labels(sequence_id) 34 | 35 | timesteps = 2 36 | TID = 0 37 | dt = 0.1 38 | for frame_idx in range(timesteps): 39 | labels = kitti.lbls[frame_idx] 40 | next_labels = kitti.lbls[frame_idx + 1] 41 | for label in labels: 42 | for next_label in next_labels: 43 | if label.track_id == next_label.track_id == TID: 44 | 45 | x = label.location[0] 46 | z = label.location[2] 47 | psi = - label.rotation_y 48 | next_x = next_label.location[0] 49 | next_z = next_label.location[0] 50 | next_psi = - next_label.rotation_y 51 | 52 | state = np.array([[x], [z], [psi], [0], [0]]) 53 | transformed_state = coordinate_transform_bicycle(state, imud[frame_idx], dt) 54 | print('------------------') 55 | print('Time: {}, state:\n {}'.format(frame_idx, state)) 56 | print('Time: {}, transformed state:\n {}'.format(frame_idx, transformed_state)) 57 | else: 58 | continue 59 | 60 | #print("Frame: {} , rotation: {}".format(frame_idx, -label.rotation_y)) 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /utils/test_eval_metrics.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from utils.eval_metrics import GOSPA 3 | from data_utils.generate_data import GenerateData 4 | import numpy as np 5 | 6 | 7 | class TestEvalMetrics(unittest.TestCase): 8 | def test_gospa(self): 9 | true_trajectories, measurements = GenerateData.generate_2D_data( 10 | mutation_probability=0.4, 11 | measurement_noise=0.4, 12 | missed_detection_probability=0.05, 13 | clutter_probability=0.1) 14 | #GenerateData.plot_generated_data(true_trajectories, measurements) 15 | for t in range(len(measurements)): 16 | # Vill vstacka 17 | ground_truths = np.array([], dtype=np.float).reshape(0, 2) 18 | estimates = np.array([], dtype=np.float).reshape(0, 2) 19 | for i in range(len(true_trajectories)): 20 | ground_truths = np.vstack((ground_truths, true_trajectories[i][t][0:2].reshape(1, 2))) 21 | for m in measurements[t]: 22 | if m.size: 23 | estimates = np.vstack((estimates, m.reshape(1, 2))) 24 | 25 | 26 | score = GOSPA(ground_truths, estimates, p=1, c=100, alpha=2.) 27 | print(score) 28 | 29 | -------------------------------------------------------------------------------- /utils/test_motion_models.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import unittest 3 | from .motion_models import BicycleModel 4 | 5 | 6 | class TestBicycle(unittest.TestCase): 7 | def test_propagate(self): 8 | dt = 1 9 | bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d = (1., 1., 1., 1., 1., 1., 1.) 10 | model = BicycleModel(dt, bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d) 11 | state = np.array([1., 1., 0, 1., 0]).reshape(5, 1) 12 | variance = np.eye(5) 13 | _state = model(state, variance) 14 | assert state[0, 0] + 1 == _state[0, 0] 15 | 16 | sate = np.array([1., 1., 0, -1., 0]).reshape(5, 1) 17 | _sate = model(sate, variance) 18 | assert state[0, 0] - 1 == _sate[0, 0] 19 | 20 | -------------------------------------------------------------------------------- /utils/test_motmetrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import unittest 3 | from .mot_metrics import MotCalculator 4 | from pmbm.single_target_hypothesis import SingleTargetHypothesis 5 | 6 | 7 | class TestMotMetrics(unittest.TestCase): 8 | def testie(self): 9 | acc = MotCalculator(5) 10 | target_id = 0 11 | single_target_id = 0 12 | test_state = np.array([[0], [35]]) 13 | test_var = np.eye(2) 14 | single_target = SingleTargetHypothesis(measurement_index=1, 15 | state=test_state, 16 | variance=test_var, 17 | existence=1., 18 | weight=1., 19 | time_of_birth=1) 20 | estimated_target = {} 21 | estimated_target['target_idx'] = target_id 22 | estimated_target['single_hypo_idx'] = single_target_id 23 | estimated_target['single_target'] = single_target 24 | estimated_targets = [estimated_target] 25 | acc.calculate(estimated_targets, 3) 26 | acc.calculate(estimated_targets, 4) 27 | 28 | #print(acc.events) 29 | #mh = mm.metrics.create() 30 | 31 | #summary = mh.compute(acc, metrics=['num_frames', 'mota', 'motp'], name='acc') -------------------------------------------------------------------------------- /utils/test_pmbm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pmbm.pmbm import PMBM 3 | from pmbm.config import Config 4 | import time 5 | 6 | 7 | def test_pmbm(logger, true_trajectories, measurements, timesteps=20, verbose=False): 8 | measurement_model = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) 9 | measurement_noise = 0.1 * np.eye(2) 10 | 11 | settings = Config(detection_probability=0.9, 12 | survival_probability=0.9, 13 | prune_threshold_poisson=0.01, 14 | prune_threshold_global_hypo=-10, 15 | prune_threshold_targets=-30, 16 | gating_distance=15, 17 | birth_gating_distance=10, 18 | motion_model='CV', 19 | poisson_states_model_name='noob', 20 | measurement_model=measurement_model, 21 | measurement_noise=measurement_noise, 22 | birth_weight=0.9, 23 | max_nof_global_hypos=200, 24 | min_new_nof_global_hypos=2, 25 | max_new_nof_global_hypos=50) 26 | 27 | pmbm = PMBM(settings) 28 | 29 | for timestep in range(min(timesteps, len(measurements))): 30 | if not verbose: print('k = {}/{} | '.format(timestep + 1, len(measurements)), end='') 31 | if verbose: print('---\nIteration k = {}'.format(timestep + 1)) 32 | start_total_time = time.time() 33 | 34 | # PREDICTION 35 | # print("------------------ PREDICTION: {}------------------".format(timestep)) 36 | start_time = time.time() 37 | pmbm.predict() 38 | # pmbm.plot_targets(true_trajectories, measurements, timestep, 'prediction') 39 | prediction_time = round((time.time() - start_time) / (1e-6), 1) 40 | 41 | # UPDATE 42 | # print("------------------ UPDATE: {}------------------".format(timestep)) 43 | start_time = time.time() 44 | pmbm.update(measurements[timestep]) 45 | update_time = round((time.time() - start_time) / (1e-6), 1) 46 | 47 | start_time = time.time() 48 | pmbm.target_estimation() 49 | estimation_time = round((time.time() - start_time) / (1e-6), 1) 50 | 51 | start_time = time.time() 52 | pmbm.reduction() 53 | prune_sth_time = round((time.time() - start_time) / (1e-6), 1) 54 | 55 | total_time = round((time.time() - start_total_time), 4) 56 | 57 | if verbose: print( 58 | '\tTotal time:\t {}\ts \n\t\t \tPrediction:\t {}\t\u03BCs \n\t\t \tUpdate:\t\t {}\t\u03BCs \n\t\t \tEstim.:\t\t {}\t\u03BCs \n\t\t \tPrune:\t\t {}\t\u03BCs \n\t\t \t'.format( 59 | total_time, prediction_time, update_time, estimation_time, prune_sth_time)) 60 | logger.log_data(pmbm, total_time=total_time, measurements=measurements[timestep], 61 | true_states=np.array(true_trajectories)[:, timestep], verbose=verbose) 62 | 63 | return pmbm 64 | 65 | 66 | --------------------------------------------------------------------------------