81 | https://youtu.be/d9EZSFHPsio 82 |
83 |
84 | The tracker (EKF) follows the car smoothly.
85 |
86 | **RMSE through time (Step 1)**87 |
88 |
89 | And the mean RMSE is 0.31, below 0.35 as the rubric requested.
90 |
91 | **[SDCE ND] Sensor Fusion and Object Tracking (Step 2, Resnet) [Submission 2]**92 | https://youtu.be/Gc0H8hfHD-c 93 |
94 |
95 | EKF follows this fast car without problems. At the end, the tracker deletes the track once it falls outside the field-of-view. However, the tracker takes some time until it finally confirms the track. Why? Because I programmed the tracker to be skeptical enough to reject the ghosts tracks in Step 4.
96 |
97 | **RMSE through time (Step 2)**98 |
99 |
100 | The mean RMSE of this track is 0.03, which is very good!
101 |
102 | **RMSE through time (Step 3)**103 |
104 |
105 | Step 3 is the same experiment in Step 4. So, I didn't upload a video of Step 3. And I will explain the RMSEs of Step 3 in Step 4.
106 |
107 | **[SDCE ND] Sensor Fusion and Object Tracking (Step 4, Resnet) [Submission 2]**108 | https://youtu.be/k-RrG9VT9Kk 109 |
110 |
111 | EKF follows the 2 important cars smoothly without problems. But in the time period from second 10th to second 13th, where there are no lidar measurements in 1 car, the mean RMSE increases slightly. In this part, EKF predicts the car movements in an imperfect way, given there are no lidar measurements. After that, the car recovers its lidar measurements and its tracking. Notice that in the video I draw the camera measurements in yellow. In spite of the lack of lidar measurements in that time period, EKF updates the track with camera measurements, preventing the track from being deleted. However, camera measurements are ambiguous in the lines of ray tracing. Throughout these lines, any depth is valid and ambiguous. And that's why, without lidar measurements, the error followed the line of ray tracing. Watch the video.
112 |
113 | This error is not my fault because the precomputed results of `fpn_resnet` that Udacity gave us lack many lidar measurements in big gaps. Fortunately, I did my best to follow the 2 important cars from the beginning to the end, without losing the track. And I obtained a decent mean RMSE for the 2 important cars: 0.40 and 0.14.
114 |
115 | I debugged extensively this part. I thought I was not using camera measurements properly. But I tracked the flow of measurements throughout all the functions and I noticed that at most 1 lidar measurement and 1 camera measurement are being matched to each track in each frame. Moreover, I changed the calculations of camera measurements slightly, changing signs and adding some quantities, producing disastrous errors. It means that my formulas without slight modifications are correct.
116 |
117 | **RMSE through time (Step 4)**118 |
119 |
120 | Track 0 (blue) and track 1 (orange) are the 2 important cars whose tracking was never lost. Track 0 (blue) increases its RMSEs slightly from second 10th to second 13th, when there are no lidar measurements. Track 10 (green) is the black car whose precomputed detections shown no lidar measurements until second 18th.
121 |
122 | **[SDCE ND] Sensor Fusion and Object Tracking (Step 4, Ground Truth) [Submission 2]**123 | https://youtu.be/Dl25MHOwtD8 124 |
125 |
126 | When I use ground-truth labels, my tracker works perfectly and follows the 2 important cars, Track 0 (blue) and track 1 (orange), from the begining to the end with mean RMSEs of 0.13 and 0.06, as the rubric requested. Track 10 (green) is the black car that appeared in the middle of the video and it was not fully confirmed until both lidar measurements and camera measurements intersected the fields of view. Track 15 (red) is a car parked in the right side at the end of the video.
127 |
128 | **RMSE through time (Step 4 with ground-truth labels)**129 |
130 |
131 | **[SDCE ND] Sensor Fusion and Object Tracking (Step 4, Darknet) [Submission 2]**132 | https://youtu.be/AiZo0Lup4XI 133 |
134 |
135 | When I used the precomputed detections of `darknet`, lidar measurements have no big gaps of false negatives. And I obtained almost perfect results as the rubric requested. My tracker works perfectly and follows the 2 important cars, Track 0 (blue) and track 1 (orange), from the begining to the end with mean RMSEs of 0.18 and 0.17. Track 27 (green) is the black car that appeared in the middle of the video. There are only 3 confirmed tracks with mean RMSEs below 0.25. And all ghosts tracks were eliminated. My graph with only 3 confirmed tracks is very similar to the graph requested by the rubric. I'm so happy with my results now.
136 |
137 | In the video, the 3D boxes of cars sometimes disappear. But the 2D tracks are never lost. I modified the visualization code to exclude 3D boxes whose vertices are outside the field of view. The problem is Darknet does not provide 3D information when detecting 2D images, making the calculation of 3D boxes more difficult. In spite of lacking 3D information, my tracker managed to obtain almost perfect results, which is amazing! That's the magic of Kalman filters and sensor fusion.
138 |
139 | **RMSE through time (Step 4 with Darknet)**140 |
141 |
142 | ## Note for the reviewer:
143 |
144 | Notice that the precomputed detections of `fpn_resnet` given by Udacity have big gaps of false negatives in the lidar measurements. I did my best at tuning the parameters and algorithms of this project. However, the mean RMSE of Step 4 is slightly above of what the rubric asks. That's why I use the precomputed detections of `darknet` to fulfill the mean RMSE requested by the rubric. And now my results are almost perfect.
145 |
146 | Here is the github repository of my solution: https://github.com/jckuri/Sensor-Fusion-and-Object-Tracking-2
147 |
--------------------------------------------------------------------------------
/images/RMSE1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/RMSE1.png
--------------------------------------------------------------------------------
/images/RMSE2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/RMSE2.png
--------------------------------------------------------------------------------
/images/RMSE3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/RMSE3.png
--------------------------------------------------------------------------------
/images/RMSE4-Darknet.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/RMSE4-Darknet.png
--------------------------------------------------------------------------------
/images/RMSE4-GTL.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/RMSE4-GTL.png
--------------------------------------------------------------------------------
/images/RMSE4.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/RMSE4.png
--------------------------------------------------------------------------------
/images/step1-resnet.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/step1-resnet.png
--------------------------------------------------------------------------------
/images/step2-resnet.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/step2-resnet.png
--------------------------------------------------------------------------------
/images/step4-darknet.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/step4-darknet.png
--------------------------------------------------------------------------------
/images/step4-ground-truth.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/step4-ground-truth.png
--------------------------------------------------------------------------------
/images/step4-resnet.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jckuri/Sensor-Fusion-and-Object-Tracking-2/ec4a17a1169078894ad26e0da9cf2ddbc4cab547/images/step4-resnet.png
--------------------------------------------------------------------------------
/student/association.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Data association class with single nearest neighbor association and gating based on Mahalanobis distance
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # imports
14 | import numpy as np
15 | from scipy.stats.distributions import chi2
16 |
17 | # add project directory to python path to enable relative imports
18 | import os
19 | import sys
20 | PACKAGE_PARENT = '..'
21 | SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
22 | sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
23 |
24 | import misc.params as params
25 |
26 | class Association:
27 | '''Data association class with single nearest neighbor association and gating based on Mahalanobis distance'''
28 | def __init__(self):
29 | self.association_matrix = np.matrix([])
30 | self.unassigned_tracks = []
31 | self.unassigned_meas = []
32 |
33 | def associate(self, track_list, meas_list, KF, cnt_frame):
34 |
35 | ############
36 | # TODO Step 3: association:
37 | # - replace association_matrix with the actual association matrix based on Mahalanobis distance (see below) for all tracks and all measurements
38 | # - update list of unassigned measurements and unassigned tracks
39 | ############
40 |
41 | self.association_matrix = np.matrix([])
42 | self.unassigned_tracks = []
43 | self.unassigned_meas = []
44 |
45 | N = len(track_list) # N tracks
46 | M = len(meas_list) # M measurements
47 |
48 | if N > 0: self.unassigned_tracks = list(range(N))
49 | if M > 0: self.unassigned_meas = list(range(M))
50 | if N > 0 and M > 0:
51 | self.association_matrix = np.inf * np.ones((N, M))
52 | for i in range(N):
53 | track = track_list[i]
54 | for j in range(M):
55 | meas = meas_list[j]
56 | dist = self.MHD(track, meas, KF)
57 | if self.gating(dist, sensor = meas.sensor):
58 | #if True: ############# HERE!!!!!!!
59 | self.association_matrix[i, j] = dist
60 | #if i in self.unassigned_tracks: self.unassigned_tracks.remove(i)
61 | #if j in self.unassigned_meas: self.unassigned_meas.remove(j)
62 |
63 | return
64 |
65 | # the following only works for at most one track and one measurement
66 | self.association_matrix = np.matrix([]) # reset matrix
67 | self.unassigned_tracks = [] # reset lists
68 | self.unassigned_meas = []
69 |
70 | if len(meas_list) > 0:
71 | self.unassigned_meas = [0]
72 | if len(track_list) > 0:
73 | self.unassigned_tracks = [0]
74 | if len(meas_list) > 0 and len(track_list) > 0:
75 | self.association_matrix = np.matrix([[0]])
76 |
77 | ############
78 | # END student code
79 | ############
80 |
81 | def get_closest_track_and_meas(self):
82 | ############
83 | # TODO Step 3: find closest track and measurement:
84 | # - find minimum entry in association matrix
85 | # - delete row and column
86 | # - remove corresponding track and measurement from unassigned_tracks and unassigned_meas
87 | # - return this track and measurement
88 | ############
89 |
90 | # find closest track and measurement for next update
91 | A = self.association_matrix
92 | if np.min(A) == np.inf:
93 | return np.nan, np.nan
94 |
95 | # get indices of minimum entry
96 | ij_min = np.unravel_index(np.argmin(A, axis=None), A.shape)
97 | ind_track = ij_min[0]
98 | ind_meas = ij_min[1]
99 |
100 | # delete row and column for next update
101 | A = np.delete(A, ind_track, 0)
102 | A = np.delete(A, ind_meas, 1)
103 | self.association_matrix = A
104 |
105 | # update this track with this measurement
106 | update_track = self.unassigned_tracks[ind_track]
107 | update_meas = self.unassigned_meas[ind_meas]
108 |
109 | # remove this track and measurement from list
110 | self.unassigned_tracks.remove(update_track)
111 | self.unassigned_meas.remove(update_meas)
112 |
113 | return update_track, update_meas
114 |
115 | # the following only works for at most one track and one measurement
116 | update_track = 0
117 | update_meas = 0
118 |
119 | # remove from list
120 | self.unassigned_tracks.remove(update_track)
121 | self.unassigned_meas.remove(update_meas)
122 | self.association_matrix = np.matrix([])
123 |
124 | ############
125 | # END student code
126 | ############
127 | return update_track, update_meas
128 |
129 | def gating(self, MHD, sensor):
130 | ############
131 | # TODO Step 3: return True if measurement lies inside gate, otherwise False
132 | ############
133 |
134 | p = 0.9995 #0.999 #params.gating_threshold #0.999 #0.95
135 | limit = chi2.ppf(p, df = 2)
136 | return MHD < limit
137 |
138 | ############
139 | # END student code
140 | ############
141 |
142 | def MHD(self, track, meas, KF):
143 | ############
144 | # TODO Step 3: calculate and return Mahalanobis distance
145 | ############
146 |
147 | x = track.x
148 | H = meas.sensor.get_H(x)
149 | #gamma = meas.z - H * x
150 | gamma = meas.z - meas.sensor.get_hx(x)
151 | S = H * track.P * H.transpose() + meas.R
152 | return gamma.transpose() * np.linalg.inv(S) * gamma
153 |
154 | ############
155 | # END student code
156 | ############
157 |
158 | def associate_and_update(self, manager, meas_list, KF, cnt_frame):
159 | # associate measurements and tracks
160 | self.associate(manager.track_list, meas_list, KF, cnt_frame)
161 |
162 | #print(f'BEFORE: self.association_matrix.shape={self.association_matrix.shape}')
163 |
164 | # update associated tracks with measurements
165 | while self.association_matrix.shape[0]>0 and self.association_matrix.shape[1]>0:
166 |
167 | # search for next association between a track and a measurement
168 | ind_track, ind_meas = self.get_closest_track_and_meas()
169 | if np.isnan(ind_track):
170 | print('---no more associations---')
171 | break
172 | track = manager.track_list[ind_track]
173 |
174 | # check visibility, only update tracks in fov
175 | if not meas_list[0].sensor.in_fov(track.x):
176 | continue
177 |
178 | # Kalman update
179 | print('update track', track.id, 'with', meas_list[ind_meas].sensor.name, 'measurement', ind_meas)
180 | KF.update(track, meas_list[ind_meas])
181 |
182 | # update score and track state
183 | manager.handle_updated_track(track, cnt_frame)
184 |
185 | # save updated track
186 | manager.track_list[ind_track] = track
187 |
188 | # run track management
189 | manager.manage_tracks(self.unassigned_tracks, self.unassigned_meas, meas_list, cnt_frame)
190 |
191 | for track in manager.track_list:
192 | print('track', track.id, 'score =', track.score)
193 |
194 | #print(f'AFTER: self.association_matrix.shape={self.association_matrix.shape}')
195 |
--------------------------------------------------------------------------------
/student/evaluation.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Evaluate and plot results
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # imports
14 | import numpy as np
15 | import matplotlib
16 | matplotlib.use('wxagg') # change backend so that figure maximizing works on Mac as well
17 | import matplotlib.pyplot as plt
18 | import matplotlib.patches as patches
19 | from matplotlib.path import Path
20 | from matplotlib import colors
21 | from matplotlib.transforms import Affine2D
22 | import matplotlib.ticker as ticker
23 | import os
24 | import cv2
25 |
26 | # add project directory to python path to enable relative imports
27 | import os
28 | import sys
29 | PACKAGE_PARENT = '..'
30 | SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
31 | sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
32 |
33 | from tools.waymo_reader.simple_waymo_open_dataset_reader import label_pb2
34 |
35 | def plot_tracks(fig, ax, ax2, track_list, meas_list_cam, meas_list, lidar_labels, lidar_labels_valid,
36 | image, camera, configs_det, state=None):
37 |
38 | # plot image
39 | ax.cla()
40 | ax2.cla()
41 | ax2.imshow(image)
42 |
43 | # plot tracks, measurements and ground truth in birds-eye view
44 | for track in track_list:
45 | if state == None or track.state == state: # plot e.g. only confirmed tracks
46 |
47 | # choose color according to track state
48 | if track.state == 'confirmed':
49 | col = 'green'
50 | elif track.state == 'tentative':
51 | col = 'orange'
52 | else:
53 | col = 'red'
54 |
55 | # get current state variables
56 | w = track.width
57 | h = track.height
58 | l = track.length
59 | x = track.x[0]
60 | y = track.x[1]
61 | z = track.x[2]
62 | yaw = track.yaw
63 |
64 | # plot boxes in top view
65 | point_of_rotation = np.array([w/2, l/2])
66 | rec = plt.Rectangle(-point_of_rotation, width=w, height=l,
67 | color=col, alpha=0.2,
68 | transform=Affine2D().rotate_around(*(0,0), -yaw)+Affine2D().translate(-y,x)+ax.transData)
69 | ax.add_patch(rec)
70 |
71 | # write track id for debugging
72 | ax.text(float(-track.x[1]), float(track.x[0]+1), str(track.id))
73 |
74 | if track.state =='initialized':
75 | ax.scatter(float(-track.x[1]), float(track.x[0]), color=col, s=80, marker='x', label='initialized track')
76 | elif track.state =='tentative':
77 | ax.scatter(float(-track.x[1]), float(track.x[0]), color=col, s=80, marker='x', label='tentative track')
78 | elif track.state =='confirmed':
79 | ax.scatter(float(-track.x[1]), float(track.x[0]), color=col, s=80, marker='x', label='confirmed track')
80 |
81 | # project tracks in image
82 | # transform from vehicle to camera coordinates
83 | pos_veh = np.ones((4, 1)) # homogeneous coordinates
84 | pos_veh[0:3] = track.x[0:3]
85 | pos_sens = camera.veh_to_sens*pos_veh # transform from vehicle to sensor coordinates
86 | x = pos_sens[0]
87 | y = pos_sens[1]
88 | z = pos_sens[2]
89 |
90 | # compute rotation around z axis
91 | R = np.matrix([[np.cos(yaw), np.sin(yaw), 0],
92 | [-np.sin(yaw), np.cos(yaw), 0],
93 | [0, 0, 1]])
94 |
95 | # bounding box corners
96 | x_corners = [-l/2, l/2, l/2, l/2, l/2, -l/2, -l/2, -l/2]
97 | y_corners = [-w/2, -w/2, -w/2, w/2, w/2, w/2, w/2, -w/2]
98 | z_corners = [-h/2, -h/2, h/2, h/2, -h/2, -h/2, h/2, h/2]
99 |
100 | # bounding box
101 | corners_3D = np.array([x_corners, y_corners, z_corners])
102 |
103 | # rotate
104 | corners_3D = R*corners_3D
105 |
106 | # translate
107 | corners_3D += np.array([x, y, z]).reshape((3, 1))
108 | # print ( 'corners_3d', corners_3D)
109 |
110 | # remove bounding boxes that include negative x, projection makes no sense
111 | if np.any(corners_3D[0,:] <= 0):
112 | continue
113 |
114 | # project to image
115 | in_fov = True
116 | corners_2D = np.zeros((2,8))
117 | for k in range(8):
118 | if not camera.in_fov(corners_3D[:, k]): in_fov = False
119 | corners_2D[0,k] = camera.c_i - camera.f_i * corners_3D[1,k] / corners_3D[0,k]
120 | corners_2D[1,k] = camera.c_j - camera.f_j * corners_3D[2,k] / corners_3D[0,k]
121 | # print ( 'corners_2d', corners_2D)
122 |
123 | # edges of bounding box in vertex index from above, e.g. index 0 stands for [-l/2, -w/2, -h/2]
124 | draw_line_indices = [0, 1, 2, 3, 4, 5, 6, 7, 0, 5, 4, 1, 2, 7, 6, 3]
125 |
126 | paths_2D = np.transpose(corners_2D[:, draw_line_indices])
127 | # print ( 'paths_2D', paths_2D)
128 |
129 | codes = [Path.LINETO]*paths_2D.shape[0]
130 | codes[0] = Path.MOVETO
131 | path = Path(paths_2D, codes)
132 |
133 | # plot bounding box in image
134 | p = patches.PathPatch(
135 | path, fill=False, color=col, linewidth=3)
136 | if in_fov: ax2.add_patch(p)
137 |
138 | # plot labels
139 | for label, valid in zip(lidar_labels, lidar_labels_valid):
140 | if valid:
141 | ax.scatter(-1*label.box.center_y, label.box.center_x, color='gray', s=80, marker='+', label='ground truth')
142 | # plot measurements
143 | for meas in meas_list:
144 | ax.scatter(-1*meas.z[1], meas.z[0], color='blue', marker='.', label='measurement')
145 |
146 | # PLOT MEAS LIST CAM
147 | for meas in meas_list_cam:
148 | x = meas.z[0]
149 | y = meas.z[1]
150 | ax2.scatter(x, y, color='yellow', marker='.', label='measurement')
151 |
152 | # maximize window
153 | mng = plt.get_current_fig_manager()
154 | #mng.resize(*mng.window.maxsize())
155 | mng.resize(1280, 720)
156 | #mng.window.showMaximized()
157 | #mng.frame.Maximize(True)
158 |
159 | # axis
160 | ax.set_xlabel('y [m]')
161 | ax.set_ylabel('x [m]')
162 | ax.set_aspect('equal')
163 | ax.set_ylim(configs_det.lim_x[0], configs_det.lim_x[1]) # x forward, y left in vehicle coordinates
164 | ax.set_xlim(-configs_det.lim_y[1], -configs_det.lim_y[0])
165 | # correct x ticks (positive to the left)
166 | ticks_x = ticker.FuncFormatter(lambda x, pos: '{0:g}'.format(-x) if x!=0 else '{0:g}'.format(x))
167 | ax.xaxis.set_major_formatter(ticks_x)
168 |
169 | # remove repeated labels
170 | handles, labels = ax.get_legend_handles_labels()
171 | handle_list, label_list = [], []
172 | for handle, label in zip(handles, labels):
173 | if label not in label_list:
174 | handle_list.append(handle)
175 | label_list.append(label)
176 | ax.legend(handle_list, label_list, loc='center left', shadow=True, fontsize='x-large', bbox_to_anchor=(0.8, 0.5))
177 |
178 | plt.pause(0.01)
179 |
180 | return fig, ax, ax2
181 |
182 |
183 | def plot_rmse(manager, all_labels):
184 | fig, ax = plt.subplots()
185 | plot_empty = True
186 |
187 | # loop over all tracks
188 | for track_id in range(manager.last_id+1):
189 | rmse_sum = 0
190 | cnt = 0
191 | rmse = []
192 | time = []
193 |
194 | # loop over timesteps
195 | for i, result_dict in enumerate(manager.result_list):
196 | label_list = all_labels[i]
197 | if track_id not in result_dict:
198 | continue
199 | track = result_dict[track_id]
200 | if track.state != 'confirmed':
201 | continue
202 |
203 | # find closest label and calculate error at this timestamp
204 | min_error = np.inf
205 | for label, valid in zip(label_list[0], label_list[1]):
206 | error = 0
207 | if valid:
208 | error += (label.box.center_x - float(track.x[0]))**2
209 | error += (label.box.center_y - float(track.x[1]))**2
210 | error += (label.box.center_z - float(track.x[2]))**2
211 | if error < min_error:
212 | min_error = error
213 | if min_error < np.inf:
214 | error = np.sqrt(min_error)
215 | time.append(track.t)
216 | rmse.append(error)
217 | rmse_sum += error
218 | cnt += 1
219 |
220 | # calc overall RMSE
221 | if cnt != 0:
222 | plot_empty = False
223 | rmse_sum /= cnt
224 | # plot RMSE
225 | ax.plot(time, rmse, marker='x', label='RMSE track ' + str(track_id) + '\n(mean: '
226 | + '{:.2f}'.format(rmse_sum) + ')')
227 |
228 | # maximize window
229 | mng = plt.get_current_fig_manager()
230 | #mng.window.showMaximized()
231 | mng.resize(1280, 720)
232 | #mng.frame.Maximize(True)
233 | ax.set_ylim(0,1)
234 | if plot_empty:
235 | print('No confirmed tracks found to plot RMSE!')
236 | else:
237 | plt.legend(loc='center left', shadow=True, fontsize='x-large', bbox_to_anchor=(0.9, 0.5))
238 | plt.xlabel('time [s]')
239 | plt.ylabel('RMSE [m]')
240 | plt.show()
241 |
242 |
243 | def make_movie(path):
244 | # read track plots
245 | images = sorted([img for img in os.listdir(path) if img.endswith(".png")])
246 | frame = cv2.imread(os.path.join(path, images[0]))
247 | height, width, layers = frame.shape
248 |
249 | # save with 10fps to result dir
250 | video = cv2.VideoWriter(os.path.join(path, 'my_tracking_results.avi'), 0, 10, (width,height))
251 |
252 | for image in images:
253 | fname = os.path.join(path, image)
254 | video.write(cv2.imread(fname))
255 | os.remove(fname) # clean up
256 |
257 | cv2.destroyAllWindows()
258 | video.release()
259 |
--------------------------------------------------------------------------------
/student/filter.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Kalman filter class
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # imports
14 | import numpy as np
15 |
16 | # add project directory to python path to enable relative imports
17 | import os
18 | import sys
19 | PACKAGE_PARENT = '..'
20 | SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
21 | sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
22 | import misc.params as params
23 |
24 | def kalman_matrix(q1, q2, q3, q4):
25 | return np.matrix([[q1, 0, 0, q2, 0, 0 ],
26 | [0, q1, 0, 0, q2, 0 ],
27 | [0, 0, q1, 0, 0, q2],
28 | [q3, 0, 0, q4, 0, 0 ],
29 | [0, q3, 0, 0, q4, 0 ],
30 | [0, 0, q3, 0, 0, q4]])
31 |
32 |
33 | import random
34 | np.random.seed(seed = 1234)
35 | random.seed(1234)
36 |
37 | class Filter:
38 | '''Kalman filter class'''
39 | def __init__(self):
40 |
41 | self.dim_state = params.dim_state # process model dimension
42 | self.dt = params.dt # time increment
43 | self.q = params.q # process noise variable for Kalman filter Q
44 |
45 | def F(self):
46 | ############
47 | # TODO Step 1: implement and return system matrix F
48 | ############
49 | dt = self.dt
50 | #return kalman_matrix(1, dt, 0, 1) # really good
51 | #return kalman_matrix(1, 0.1, 0, 1) # REALLY GOOD
52 | return kalman_matrix(1, 0.2, 0, 1) # GREAT
53 | #return kalman_matrix(1, 0.18, 0, 1)
54 | #return kalman_matrix(1, 0.15, 0, 1)
55 | #return kalman_matrix(1, 0, 0, 1)
56 | #return kalman_matrix(1, 0.1, 0, 1)
57 |
58 | return np.matrix([[1, 0, 0, dt, 0, 0 ],
59 | [0, 1, 0, 0, dt, 0 ],
60 | [0, 0, 1, 0, 0, dt],
61 | [0, 0, 0, 1, 0, 0 ],
62 | [0, 0, 0, 0, 1, 0 ],
63 | [0, 0, 0, 0, 0, 1 ]])
64 |
65 | ############
66 | # END student code
67 | ############
68 |
69 | def Q(self):
70 | ############
71 | # TODO Step 1: implement and return process noise covariance Q
72 | ############
73 |
74 | #return kalman_matrix(1, 1, 1, 1) # really good
75 | #return kalman_matrix(0.5, 0.5, 0.5, 0.5)
76 | #return kalman_matrix(0.1, 0.1, 0.1, 0.1)
77 | #return kalman_matrix(0.01, 0.01, 0.01, 0.01) # REALLY GOOD
78 | #return kalman_matrix(0.01, 0.001, 0.001, 0.01) # MUCH BETTER
79 | return kalman_matrix(0.01, 0, 0, 0.01) # ALMOST EXCELLENT
80 | #return kalman_matrix(0.01, 0.001, 0.001, 0.01)
81 | #return kalman_matrix(0.005, 0.001, 0.001, 0.005)
82 | #return kalman_matrix(0.02, 0.02, 0.02, 0.02)
83 | #return kalman_matrix(0.005, 0.005, 0.005, 0.005)
84 | #return kalman_matrix(3, 1, 1, 3)
85 |
86 | q = self.q
87 | dt = self.dt
88 | q1 = ((dt**3)/3) * q
89 | q2 = ((dt**2)/2) * q
90 | q3 = dt * q
91 | return np.matrix([[q1, 0, 0, q2, 0, 0 ],
92 | [0, q1, 0, 0, q2, 0 ],
93 | [0, 0, q1, 0, 0, q2],
94 | [q2, 0, 0, q3, 0, 0 ],
95 | [0, q2, 0, 0, q3, 0 ],
96 | [0, 0, q2, 0, 0, q3]])
97 |
98 | ############
99 | # END student code
100 | ############
101 |
102 | def predict(self, track):
103 | ############
104 | # TODO Step 1: predict state x and estimation error covariance P to next timestep, save x and P in track
105 | ############
106 |
107 | F = self.F()
108 | x = F * track.x # state prediction
109 | P = F * track.P * F.transpose() + self.Q() # covariance prediction
110 | track.set_x(x)
111 | track.set_P(P)
112 |
113 | ############
114 | # END student code
115 | ############
116 |
117 | def update(self, track, meas):
118 | ############
119 | # TODO Step 1: update state x and covariance P with associated measurement, save x and P in track
120 | ############
121 |
122 | #print(f'Update track {track.id}: sensor.name={meas.sensor.name}, x.shape={track.x.shape}, x={track.x.transpose()}')
123 | x = track.x
124 | P = track.P
125 | H = meas.sensor.get_H(x) # measurement matrix
126 | gamma = self.gamma(track, meas)
127 | S = self.S(track, meas, H) # covariance of residual
128 | K = P * H.transpose() * np.linalg.inv(S) # Kalman gain
129 | x = x + K * gamma # state update
130 | I = np.identity(self.dim_state)
131 | P = (I - K * H) * P # covariance update
132 | track.set_x(x)
133 | track.set_P(P)
134 |
135 | ############
136 | # END student code
137 | ############
138 | track.update_attributes(meas)
139 |
140 | def gamma(self, track, meas):
141 | ############
142 | # TODO Step 1: calculate and return residual gamma
143 | ############
144 |
145 | #x = track.x
146 | #H = meas.sensor.get_H(x) # measurement matrix
147 | #return meas.z - H * x # residual
148 | #print(f'GAMMA: meas.sensor.name={meas.sensor.name}, meas.z={meas.z.transpose()}')
149 | return meas.z - meas.sensor.get_hx(track.x)
150 |
151 | ############
152 | # END student code
153 | ############
154 |
155 | def S(self, track, meas, H):
156 | ############
157 | # TODO Step 1: calculate and return covariance of residual S
158 | ############
159 |
160 | return H * track.P * H.transpose() + meas.R # covariance of residual
161 |
162 | ############
163 | # END student code
164 | ############
165 |
--------------------------------------------------------------------------------
/student/loop_over_dataset_f4.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Loop over all frames in a Waymo Open Dataset file,
6 | # detect and track objects and visualize results
7 | #
8 | # You should have received a copy of the Udacity license together with this program.
9 | #
10 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
11 | # ----------------------------------------------------------------------
12 | #
13 |
14 | ##################
15 | ## Imports
16 |
17 | ## general package imports
18 | import os
19 | import sys
20 | import numpy as np
21 | import math
22 | import cv2
23 | import matplotlib.pyplot as plt
24 | import copy
25 |
26 | ## Add current working directory to path
27 | sys.path.append(os.getcwd())
28 |
29 | ## Waymo open dataset reader
30 | from tools.waymo_reader.simple_waymo_open_dataset_reader import utils as waymo_utils
31 | from tools.waymo_reader.simple_waymo_open_dataset_reader import WaymoDataFileReader, dataset_pb2, label_pb2
32 |
33 | ## 3d object detection
34 | import student.objdet_pcl as pcl
35 | import student.objdet_detect as det
36 | import student.objdet_eval as eval
37 |
38 | import misc.objdet_tools as tools
39 | from misc.helpers import save_object_to_file, load_object_from_file, make_exec_list
40 |
41 | from student.filter import Filter
42 | from student.trackmanagement import Trackmanagement
43 | from student.association import Association
44 | from student.measurements import Sensor, Measurement
45 | from misc.evaluation import plot_tracks, plot_rmse, make_movie
46 | import misc.params as params
47 |
48 | ##################
49 | ## Set parameters and perform initializations
50 |
51 | ## Select Waymo Open Dataset file and frame numbers
52 | data_filename = 'training_segment-1005081002024129653_5313_150_5333_150_with_camera_labels.tfrecord' # Sequence 1
53 | # data_filename = 'training_segment-10072231702153043603_5725_000_5745_000_with_camera_labels.tfrecord' # Sequence 2
54 | # data_filename = 'training_segment-10963653239323173269_1924_000_1944_000_with_camera_labels.tfrecord' # Sequence 3
55 | show_only_frames = [0, 200] #[100, 130] #[0, 200] # show only frames in interval for debugging
56 |
57 | ## Prepare Waymo Open Dataset file for loading
58 | data_fullpath = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'dataset', data_filename) # adjustable path in case this script is called from another working directory
59 | results_fullpath = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'results')
60 | datafile = WaymoDataFileReader(data_fullpath)
61 | datafile_iter = iter(datafile) # initialize dataset iterator
62 |
63 | ## Initialize object detection
64 | configs_det = det.load_configs(model_name='darknet') # options are 'darknet', 'fpn_resnet'
65 | model_det = det.create_model(configs_det)
66 |
67 | configs_det.use_labels_as_objects = False # True = use groundtruth labels as objects, False = use model-based detection
68 | configs_det.lim_y = [-25, 25]
69 |
70 | ## Initialize tracking
71 | KF = Filter() # set up Kalman filter
72 | association = Association() # init data association
73 | manager = Trackmanagement() # init track manager
74 | lidar = None # init lidar sensor object
75 | camera = None # init camera sensor object
76 |
77 | ## Selective execution and visualization
78 | exec_detection = [] # options are 'bev_from_pcl', 'detect_objects', 'validate_object_labels', 'measure_detection_performance'; options not in the list will be loaded from file
79 | exec_tracking = ['perform_tracking'] # options are 'perform_tracking'
80 | exec_visualization = ['show_tracks', 'make_tracking_movie'] # options are 'show_range_image', 'show_bev', 'show_pcl', 'show_labels_in_image', 'show_objects_and_labels_in_bev', 'show_objects_in_bev_labels_in_camera', 'show_tracks', 'show_detection_performance', 'make_tracking_movie'
81 | exec_list = make_exec_list(exec_detection, exec_tracking, exec_visualization)
82 | vis_pause_time = 0 # set pause time between frames in ms (0 = stop between frames until key is pressed)
83 |
84 |
85 | ##################
86 | ## Perform detection & tracking over all selected frames
87 |
88 | cnt_frame = 0
89 | all_labels = []
90 | det_performance_all = []
91 | if 'show_tracks' in exec_list:
92 | fig, (ax2, ax) = plt.subplots(1,2) # init track plot
93 |
94 | while True:
95 | try:
96 | ## Get next frame from Waymo dataset
97 | frame = next(datafile_iter)
98 | if cnt_frame < show_only_frames[0]:
99 | cnt_frame = cnt_frame + 1
100 | continue
101 | elif cnt_frame > show_only_frames[1]:
102 | print('reached end of selected frames')
103 | break
104 |
105 | print('------------------------------')
106 | print('processing frame #' + str(cnt_frame))
107 |
108 | #################################
109 | ## Perform 3D object detection
110 |
111 | ## Extract calibration data and front camera image from frame
112 | lidar_name = dataset_pb2.LaserName.TOP
113 | camera_name = dataset_pb2.CameraName.FRONT
114 | lidar_calibration = waymo_utils.get(frame.context.laser_calibrations, lidar_name)
115 | camera_calibration = waymo_utils.get(frame.context.camera_calibrations, camera_name)
116 | if 'load_image' in exec_list:
117 | image = tools.extract_front_camera_image(frame)
118 |
119 | ## Compute lidar point-cloud from range image
120 | if 'pcl_from_rangeimage' in exec_list:
121 | print('computing point-cloud from lidar range image')
122 | lidar_pcl = tools.pcl_from_range_image(frame, lidar_name)
123 | else:
124 | print('loading lidar point-cloud from result file')
125 | lidar_pcl = load_object_from_file(results_fullpath, data_filename, 'lidar_pcl', cnt_frame)
126 |
127 | ## Compute lidar birds-eye view (bev)
128 | if 'bev_from_pcl' in exec_list:
129 | print('computing birds-eye view from lidar pointcloud')
130 | lidar_bev = pcl.bev_from_pcl(lidar_pcl, configs_det)
131 | else:
132 | print('loading birds-eve view from result file')
133 | lidar_bev = load_object_from_file(results_fullpath, data_filename, 'lidar_bev', cnt_frame)
134 |
135 | ## 3D object detection
136 | if (configs_det.use_labels_as_objects==True):
137 | print('using groundtruth labels as objects')
138 | detections = tools.convert_labels_into_objects(frame.laser_labels, configs_det)
139 | else:
140 | if 'detect_objects' in exec_list:
141 | print('detecting objects in lidar pointcloud')
142 | detections = det.detect_objects(lidar_bev, model_det, configs_det)
143 | else:
144 | print('loading detected objects from result file')
145 | detections = load_object_from_file(results_fullpath, data_filename, 'detections_' + configs_det.arch + '_' + str(configs_det.conf_thresh), cnt_frame)
146 |
147 | ## Validate object labels
148 | if 'validate_object_labels' in exec_list:
149 | print("validating object labels")
150 | valid_label_flags = tools.validate_object_labels(frame.laser_labels, lidar_pcl, configs_det, 0 if configs_det.use_labels_as_objects==True else 10)
151 | else:
152 | print('loading object labels and validation from result file')
153 | valid_label_flags = load_object_from_file(results_fullpath, data_filename, 'valid_labels', cnt_frame)
154 |
155 | ## Performance evaluation for object detection
156 | if 'measure_detection_performance' in exec_list:
157 | print('measuring detection performance')
158 | det_performance = eval.measure_detection_performance(detections, frame.laser_labels, valid_label_flags, configs_det.min_iou)
159 | else:
160 | print('loading detection performance measures from file')
161 | det_performance = load_object_from_file(results_fullpath, data_filename, 'det_performance_' + configs_det.arch + '_' + str(configs_det.conf_thresh), cnt_frame)
162 |
163 | det_performance_all.append(det_performance) # store all evaluation results in a list for performance assessment at the end
164 |
165 |
166 | ## Visualization for object detection
167 | if 'show_range_image' in exec_list:
168 | img_range = pcl.show_range_image(frame, lidar_name)
169 | img_range = img_range.astype(np.uint8)
170 | cv2.imshow('range_image', img_range)
171 | cv2.waitKey(vis_pause_time)
172 |
173 | if 'show_pcl' in exec_list:
174 | pcl.show_pcl(lidar_pcl)
175 |
176 | if 'show_bev' in exec_list:
177 | tools.show_bev(lidar_bev, configs_det)
178 | cv2.waitKey(vis_pause_time)
179 |
180 | if 'show_labels_in_image' in exec_list:
181 | img_labels = tools.project_labels_into_camera(camera_calibration, image, frame.laser_labels, valid_label_flags, 0.5)
182 | cv2.imshow('img_labels', img_labels)
183 | cv2.waitKey(vis_pause_time)
184 |
185 | if 'show_objects_and_labels_in_bev' in exec_list:
186 | tools.show_objects_labels_in_bev(detections, frame.laser_labels, lidar_bev, configs_det)
187 | cv2.waitKey(vis_pause_time)
188 |
189 | if 'show_objects_in_bev_labels_in_camera' in exec_list:
190 | tools.show_objects_in_bev_labels_in_camera(detections, lidar_bev, image, frame.laser_labels, valid_label_flags, camera_calibration, configs_det)
191 | cv2.waitKey(vis_pause_time)
192 |
193 |
194 | #################################
195 | ## Perform tracking
196 | if 'perform_tracking' in exec_list:
197 | # set up sensor objects
198 | if lidar is None:
199 | lidar = Sensor('lidar', lidar_calibration)
200 | if camera is None:
201 | camera = Sensor('camera', camera_calibration)
202 |
203 | # preprocess lidar detections
204 | meas_list_lidar = []
205 | for detection in detections:
206 | meas_list_lidar = lidar.generate_measurement(cnt_frame, detection[1:], meas_list_lidar)
207 |
208 | # preprocess camera detections
209 | meas_list_cam = []
210 | for label in frame.camera_labels[0].labels:
211 | if(label.type == label_pb2.Label.Type.TYPE_VEHICLE):
212 |
213 | box = label.box
214 | # use camera labels as measurements and add some random noise
215 | z = [box.center_x, box.center_y, box.width, box.length]
216 | z[0] = z[0] + np.random.normal(0, params.sigma_cam_i)
217 | z[1] = z[1] + np.random.normal(0, params.sigma_cam_j)
218 | meas_list_cam = camera.generate_measurement(cnt_frame, z, meas_list_cam)
219 |
220 | # Kalman prediction
221 | for track in manager.track_list:
222 | print('predict track', track.id)
223 | KF.predict(track)
224 | track.set_t((cnt_frame - 1)*0.1) # save next timestamp
225 |
226 | # associate all lidar measurements to all tracks
227 | association.associate_and_update(manager, meas_list_lidar, KF, cnt_frame)
228 |
229 | # associate all camera measurements to all tracks
230 | association.associate_and_update(manager, meas_list_cam, KF, cnt_frame)
231 |
232 | # save results for evaluation
233 | result_dict = {}
234 | for track in manager.track_list:
235 | result_dict[track.id] = track
236 | manager.result_list.append(copy.deepcopy(result_dict))
237 | label_list = [frame.laser_labels, valid_label_flags]
238 | all_labels.append(label_list)
239 |
240 | # visualization
241 | if 'show_tracks' in exec_list:
242 | #fig, ax, ax2 = plot_tracks(fig, ax, ax2, manager.track_list, meas_list_lidar, frame.laser_labels, valid_label_flags, image, camera, configs_det)
243 | fig, ax, ax2 = plot_tracks(fig, ax, ax2, manager.track_list, meas_list_cam, meas_list_lidar, frame.laser_labels, valid_label_flags, image, camera, configs_det)
244 | if 'make_tracking_movie' in exec_list:
245 | # save track plots to file
246 | fname = results_fullpath + '/tracking%03d.png' % cnt_frame
247 | print('Saving frame', fname)
248 | fig.savefig(fname)
249 |
250 | # increment frame counter
251 | cnt_frame = cnt_frame + 1
252 |
253 | except StopIteration:
254 | # if StopIteration is raised, break from loop
255 | print("StopIteration has been raised\n")
256 | break
257 |
258 |
259 | #################################
260 | ## Post-processing
261 |
262 | ## Evaluate object detection performance
263 | if 'show_detection_performance' in exec_list:
264 | eval.compute_performance_stats(det_performance_all, configs_det)
265 |
266 | ## Plot RMSE for all tracks
267 | if 'show_tracks' in exec_list:
268 | plot_rmse(manager, all_labels)
269 |
270 | ## Make movie from tracking results
271 | if 'make_tracking_movie' in exec_list:
272 | make_movie(results_fullpath)
273 |
--------------------------------------------------------------------------------
/student/measurements.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Classes for sensor and measurement
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # imports
14 | import numpy as np
15 |
16 | # add project directory to python path to enable relative imports
17 | import os
18 | import sys
19 | PACKAGE_PARENT = '..'
20 | SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
21 | sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
22 | import misc.params as params
23 |
24 | class Sensor:
25 | '''Sensor class including measurement matrix'''
26 | def __init__(self, name, calib):
27 | self.name = name
28 | if name == 'lidar':
29 | self.dim_meas = 3
30 | self.sens_to_veh = np.matrix(np.identity((4))) # transformation sensor to vehicle coordinates equals identity matrix because lidar detections are already in vehicle coordinates
31 | self.fov = [-np.pi/2, np.pi/2] # angle of field of view in radians
32 |
33 | elif name == 'camera':
34 | self.dim_meas = 2
35 | self.sens_to_veh = np.matrix(calib.extrinsic.transform).reshape(4,4) # transformation sensor to vehicle coordinates
36 | self.f_i = calib.intrinsic[0] # focal length i-coordinate
37 | self.f_j = calib.intrinsic[1] # focal length j-coordinate
38 | self.c_i = calib.intrinsic[2] # principal point i-coordinate
39 | self.c_j = calib.intrinsic[3] # principal point j-coordinate
40 | self.fov = [-0.35, 0.35] # angle of field of view in radians, inaccurate boundary region was removed
41 |
42 | self.veh_to_sens = np.linalg.inv(self.sens_to_veh) # transformation vehicle to sensor coordinates
43 |
44 | def in_fov(self, x):
45 | # check if an object x can be seen by this sensor
46 | ############
47 | # TODO Step 4: implement a function that returns True if x lies in the sensor's field of view,
48 | # otherwise False.
49 | ############
50 |
51 | #return True
52 | if self.name == 'lidar':
53 | if x[0] > 49: return False
54 | pos_veh = np.ones((4, 1)) # homogeneous coordinates
55 | pos_veh[0:3] = x[0:3]
56 | pos_sens = self.veh_to_sens*pos_veh # transform from vehicle to sensor coordinates
57 | alpha = np.arctan(pos_sens[1, 0]/pos_sens[0, 0]) # calc angle between object and x-axis
58 | # no normalization needed because returned alpha always lies between [-pi/2, pi/2]
59 | within_fov = alpha > self.fov[0] and alpha < self.fov[1]
60 | #print('alpha:', alpha, type(alpha), 'within_fov:', within_fov, type(within_fov))
61 | return within_fov
62 |
63 | ############
64 | # END student code
65 | ############
66 |
67 | def get_hx(self, x):
68 | # calculate nonlinear measurement expectation value h(x)
69 | if self.name == 'lidar':
70 | pos_veh = np.ones((4, 1)) # homogeneous coordinates
71 | pos_veh[0:3] = x[0:3]
72 | pos_sens = self.veh_to_sens*pos_veh # transform from vehicle to lidar coordinates
73 | return pos_sens[0:3]
74 | elif self.name == 'camera':
75 |
76 | ############
77 | # TODO Step 4: implement nonlinear camera measurement function h:
78 | # - transform position estimate from vehicle to camera coordinates
79 | # - project from camera to image coordinates
80 | # - make sure to not divide by zero, raise an error if needed
81 | # - return h(x)
82 | ############
83 |
84 | pos_veh = np.ones((4, 1)) # homogeneous coordinates
85 | pos_veh[0:3] = x[0:3]
86 | pos_sens = self.veh_to_sens * pos_veh # transform from vehicle to camera coordinates
87 | hx = np.zeros((2,1))
88 | if pos_sens[0] == 0:
89 | raise NameError('Jacobian not defined for x=0!')
90 | else:
91 | hx[0, 0] = self.c_i - self.f_i * pos_sens[1] / pos_sens[0] # project to image coordinates
92 | hx[1, 0] = self.c_j - self.f_j * pos_sens[2] / pos_sens[0]
93 | #print(f'CAMERA hx:\n{hx}')
94 | return hx
95 |
96 | ############
97 | # END student code
98 | ############
99 |
100 | def get_H(self, x):
101 | # calculate Jacobian H at current x from h(x)
102 | H = np.matrix(np.zeros((self.dim_meas, params.dim_state)))
103 | R = self.veh_to_sens[0:3, 0:3] # rotation
104 | T = self.veh_to_sens[0:3, 3] # translation
105 | if self.name == 'lidar':
106 | H[0:3, 0:3] = R
107 | elif self.name == 'camera':
108 | # check and print error message if dividing by zero
109 | if R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0] == 0:
110 | raise NameError('Jacobian not defined for this x!')
111 | else:
112 | H[0,0] = self.f_i * (-R[1,0] / (R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])
113 | + R[0,0] * (R[1,0]*x[0] + R[1,1]*x[1] + R[1,2]*x[2] + T[1]) \
114 | / ((R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])**2))
115 | H[1,0] = self.f_j * (-R[2,0] / (R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])
116 | + R[0,0] * (R[2,0]*x[0] + R[2,1]*x[1] + R[2,2]*x[2] + T[2]) \
117 | / ((R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])**2))
118 | H[0,1] = self.f_i * (-R[1,1] / (R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])
119 | + R[0,1] * (R[1,0]*x[0] + R[1,1]*x[1] + R[1,2]*x[2] + T[1]) \
120 | / ((R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])**2))
121 | H[1,1] = self.f_j * (-R[2,1] / (R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])
122 | + R[0,1] * (R[2,0]*x[0] + R[2,1]*x[1] + R[2,2]*x[2] + T[2]) \
123 | / ((R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])**2))
124 | H[0,2] = self.f_i * (-R[1,2] / (R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])
125 | + R[0,2] * (R[1,0]*x[0] + R[1,1]*x[1] + R[1,2]*x[2] + T[1]) \
126 | / ((R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])**2))
127 | H[1,2] = self.f_j * (-R[2,2] / (R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])
128 | + R[0,2] * (R[2,0]*x[0] + R[2,1]*x[1] + R[2,2]*x[2] + T[2]) \
129 | / ((R[0,0]*x[0] + R[0,1]*x[1] + R[0,2]*x[2] + T[0])**2))
130 | return H
131 |
132 | def generate_measurement(self, num_frame, z, meas_list):
133 | # generate new measurement from this sensor and add to measurement list
134 | ############
135 | # TODO Step 4: remove restriction to lidar in order to include camera as well
136 | ############
137 |
138 | #if self.name == 'lidar' or self.name == 'camera':
139 | if True:
140 | meas = Measurement(num_frame, z, self)
141 | meas_list.append(meas)
142 | #print(f'sensor.name: {self.name}, num_frame={num_frame}, z={z}')
143 | return meas_list
144 |
145 | ############
146 | # END student code
147 | ############
148 |
149 |
150 | ###################
151 |
152 | class Measurement:
153 | '''Measurement class including measurement values, covariance, timestamp, sensor'''
154 | def __init__(self, num_frame, z, sensor):
155 | #print(f'New measurement: num_frame={num_frame}, sensor.name={sensor.name}, z={z}')
156 | # create measurement object
157 | self.t = (num_frame - 1) * params.dt # time
158 | if sensor.name == 'lidar':
159 | sigma_lidar_x = params.sigma_lidar_x # load params
160 | sigma_lidar_y = params.sigma_lidar_y
161 | sigma_lidar_z = params.sigma_lidar_z
162 | self.z = np.zeros((sensor.dim_meas,1)) # measurement vector
163 | self.z[0] = z[0]
164 | self.z[1] = z[1]
165 | self.z[2] = z[2]
166 | self.sensor = sensor # sensor that generated this measurement
167 | self.R = np.matrix([[sigma_lidar_x**2, 0, 0 ], # measurement noise covariance matrix
168 | [0, sigma_lidar_y**2, 0 ],
169 | [0, 0, sigma_lidar_z**2]])
170 |
171 | self.width = z[4]
172 | self.length = z[5]
173 | self.height = z[3]
174 | self.yaw = z[6]
175 | elif sensor.name == 'camera':
176 |
177 | ############
178 | # TODO Step 4: initialize camera measurement including z, R, and sensor
179 | ############
180 |
181 | sigma_cam_i = params.sigma_cam_i
182 | sigma_cam_j = params.sigma_cam_j
183 | self.z = np.zeros((sensor.dim_meas, 1)) # measurement vector
184 | self.z[0] = z[0]
185 | self.z[1] = z[1]
186 | self.sensor = sensor # sensor that generated this measurement
187 | self.R = np.matrix([[sigma_cam_i**2, 0 ], # measurement noise covariance matrix
188 | [0, sigma_cam_j**2]])
189 | # From loop_over_dataset.py
190 | # z = [box.center_x, box.center_y, box.width, box.length]
191 | # meas_list_cam = camera.generate_measurement(cnt_frame, z, meas_list_cam)
192 | self.width = z[2]
193 | self.length = z[3]
194 | ############
195 | # END student code
196 | ############
197 |
--------------------------------------------------------------------------------
/student/objdet_detect.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Detect 3D objects in lidar point clouds using deep learning
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # general package imports
14 | import numpy as np
15 | import torch
16 | from easydict import EasyDict as edict
17 |
18 | # add project directory to python path to enable relative imports
19 | import os
20 | import sys
21 | PACKAGE_PARENT = '..'
22 | SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
23 | sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
24 |
25 | # model-related
26 | from tools.objdet_models.resnet.models import fpn_resnet
27 | from tools.objdet_models.resnet.utils.evaluation_utils import decode, post_processing
28 |
29 | from tools.objdet_models.darknet.models.darknet2pytorch import Darknet as darknet
30 | from tools.objdet_models.darknet.utils.evaluation_utils import post_processing_v2
31 |
32 |
33 | # load model-related parameters into an edict
34 | def load_configs_model(model_name='darknet', configs=None):
35 |
36 | # init config file, if none has been passed
37 | if configs==None:
38 | configs = edict()
39 |
40 | # get parent directory of this file to enable relative paths
41 | curr_path = os.path.dirname(os.path.realpath(__file__))
42 | parent_path = configs.model_path = os.path.abspath(os.path.join(curr_path, os.pardir))
43 |
44 | # set parameters according to model type
45 | if model_name == "darknet":
46 | configs.model_path = os.path.join(parent_path, 'tools', 'objdet_models', 'darknet')
47 | configs.pretrained_filename = os.path.join(configs.model_path, 'pretrained', 'complex_yolov4_mse_loss.pth')
48 | configs.arch = 'darknet'
49 | configs.batch_size = 4
50 | configs.cfgfile = os.path.join(configs.model_path, 'config', 'complex_yolov4.cfg')
51 | configs.conf_thresh = 0.5
52 | configs.distributed = False
53 | configs.img_size = 608
54 | configs.nms_thresh = 0.4
55 | configs.num_samples = None
56 | configs.num_workers = 4
57 | configs.pin_memory = True
58 | configs.use_giou_loss = False
59 |
60 | elif model_name == 'fpn_resnet':
61 | ####### ID_S3_EX1-3 START #######
62 | #######
63 | print("student task ID_S3_EX1-3")
64 |
65 | configs.model_path = os.path.join(parent_path, 'tools', 'objdet_models', 'resnet')
66 | configs.pretrained_filename = os.path.join(configs.model_path, 'pretrained', 'fpn_resnet_18_epoch_300.pth')
67 | configs.arch = 'fpn_resnet'
68 | configs.batch_size = 4
69 | #configs.cfgfile = os.path.join(configs.model_path, 'config', 'complex_yolov4.cfg')
70 | #configs.conf_thresh = 0.5
71 | configs.distributed = False
72 | configs.img_size = 608
73 | #configs.nms_thresh = 0.4
74 | configs.num_samples = None
75 | configs.num_workers = 1
76 | configs.pin_memory = True
77 | #configs.use_giou_loss = False
78 |
79 | #######
80 | ####### ID_S3_EX1-3 END #######
81 |
82 | else:
83 | raise ValueError("Error: Invalid model name")
84 | print('configs.pretrained_filename:', configs.pretrained_filename)
85 |
86 | # GPU vs. CPU
87 | configs.no_cuda = True # if true, cuda is not used
88 | configs.gpu_idx = 0 # GPU index to use.
89 | configs.device = torch.device('cpu' if configs.no_cuda else 'cuda:{}'.format(configs.gpu_idx))
90 |
91 | return configs
92 |
93 |
94 | # load all object-detection parameters into an edict
95 | def load_configs(model_name='fpn_resnet', configs=None):
96 |
97 | # init config file, if none has been passed
98 | if configs==None:
99 | configs = edict()
100 |
101 | # birds-eye view (bev) parameters
102 | configs.lim_x = [0, 50] # detection range in m
103 | configs.lim_y = [-25, 25]
104 | configs.lim_z = [-1, 3]
105 | configs.lim_r = [0, 1.0] # reflected lidar intensity
106 | configs.bev_width = 608 # pixel resolution of bev image
107 | configs.bev_height = 608
108 |
109 | # add model-dependent parameters
110 | configs = load_configs_model(model_name, configs)
111 |
112 | # visualization parameters
113 | configs.output_width = 608 # width of result image (height may vary)
114 | configs.obj_colors = [[0, 255, 255], [0, 0, 255], [255, 0, 0]] # 'Pedestrian': 0, 'Car': 1, 'Cyclist': 2
115 |
116 | return configs
117 |
118 |
119 | def setup_configs_for_resnet(configs):
120 | configs.pin_memory = True
121 | configs.distributed = False # For testing on 1 GPU only
122 |
123 | configs.input_size = (608, 608)
124 | configs.hm_size = (152, 152)
125 | configs.down_ratio = 4
126 | configs.max_objects = 50
127 |
128 | configs.imagenet_pretrained = False
129 | configs.head_conv = 64
130 | configs.num_classes = 3
131 | configs.num_center_offset = 2
132 | configs.num_z = 1
133 | configs.num_dim = 3
134 | configs.num_direction = 2 # sin, cos
135 |
136 | configs.heads = {
137 | 'hm_cen': configs.num_classes,
138 | 'cen_offset': configs.num_center_offset,
139 | 'direction': configs.num_direction,
140 | 'z_coor': configs.num_z,
141 | 'dim': configs.num_dim
142 | }
143 | configs.num_input_features = 4
144 |
145 | configs.conf_thresh = 0.5
146 |
147 | def create_resnet_model(configs):
148 | """Create model based on architecture name"""
149 | try:
150 | arch_parts = configs.arch.split('_')
151 | #num_layers = int(arch_parts[-1])
152 | num_layers = 18 #18
153 | except:
154 | raise ValueError
155 | if 'fpn_resnet' in configs.arch:
156 | print('using ResNet architecture with feature pyramid')
157 | model = fpn_resnet.get_pose_net(num_layers=num_layers, heads=configs.heads, head_conv=configs.head_conv,
158 | imagenet_pretrained=configs.imagenet_pretrained)
159 | elif 'resnet' in configs.arch:
160 | print('using ResNet architecture')
161 | model = resnet.get_pose_net(num_layers=num_layers, heads=configs.heads, head_conv=configs.head_conv,
162 | imagenet_pretrained=configs.imagenet_pretrained)
163 | else:
164 | assert False, 'Undefined model backbone'
165 |
166 | return model
167 |
168 |
169 | # create model according to selected model type
170 | def create_model(configs):
171 |
172 | # check for availability of model file
173 | assert os.path.isfile(configs.pretrained_filename), "No file at {}".format(configs.pretrained_filename)
174 |
175 | # create model depending on architecture name
176 | if (configs.arch == 'darknet') and (configs.cfgfile is not None):
177 | print('using darknet')
178 | model = darknet(cfgfile=configs.cfgfile, use_giou_loss=configs.use_giou_loss)
179 |
180 | elif 'fpn_resnet' in configs.arch:
181 | print('using ResNet architecture with feature pyramid')
182 |
183 | ####### ID_S3_EX1-4 START #######
184 | #######
185 | print("student task ID_S3_EX1-4")
186 | setup_configs_for_resnet(configs)
187 | model = create_resnet_model(configs)
188 | #######
189 | ####### ID_S3_EX1-4 END #######
190 |
191 | else:
192 | assert False, 'Undefined model backbone'
193 |
194 | # load model weights
195 | model.load_state_dict(torch.load(configs.pretrained_filename, map_location='cpu'))
196 | print('Loaded weights from {}\n'.format(configs.pretrained_filename))
197 |
198 | # set model to evaluation state
199 | configs.device = torch.device('cpu' if configs.no_cuda else 'cuda:{}'.format(configs.gpu_idx))
200 | model = model.to(device=configs.device) # load model to either cpu or gpu
201 | model.eval()
202 |
203 | return model
204 |
205 |
206 | # detect trained objects in birds-eye view
207 | def detect_objects(input_bev_maps, model, configs):
208 |
209 | # deactivate autograd engine during test to reduce memory usage and speed up computations
210 | with torch.no_grad():
211 |
212 | # perform inference
213 | outputs = model(input_bev_maps)
214 |
215 | # decode model output into target object format
216 | if 'darknet' in configs.arch:
217 |
218 | # perform post-processing
219 | output_post = post_processing_v2(outputs, conf_thresh=configs.conf_thresh, nms_thresh=configs.nms_thresh)
220 | detections = []
221 | for sample_i in range(len(output_post)):
222 | if output_post[sample_i] is None:
223 | continue
224 | detection = output_post[sample_i]
225 | for obj in detection:
226 | x, y, w, l, im, re, _, _, _ = obj
227 | yaw = np.arctan2(im, re)
228 | detections.append([1, x, y, 0.0, 1.50, w, l, yaw])
229 |
230 | elif 'fpn_resnet' in configs.arch:
231 | # decode output and perform post-processing
232 |
233 | ####### ID_S3_EX1-5 START #######
234 | #######
235 | print("student task ID_S3_EX1-5")
236 |
237 | #######
238 | ####### ID_S3_EX1-5 END #######
239 |
240 |
241 |
242 | ####### ID_S3_EX2 START #######
243 | #######
244 | # Extract 3d bounding boxes from model response
245 | print("student task ID_S3_EX2")
246 | objects = []
247 |
248 | ## step 1 : check whether there are any detections
249 |
250 | ## step 2 : loop over all detections
251 |
252 | ## step 3 : perform the conversion using the limits for x, y and z set in the configs structure
253 |
254 | ## step 4 : append the current object to the 'objects' array
255 |
256 | #######
257 | ####### ID_S3_EX2 START #######
258 |
259 | return objects
260 |
261 |
--------------------------------------------------------------------------------
/student/params.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Parameter file for tracking
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # general parameters
14 | dim_state = 6 # process model dimension
15 |
16 | # Kalman filter parameters (Step 1)
17 | dt = 0.1 # time increment
18 | q = 3 #0.1 #3 # process noise variable for Kalman filter Q
19 |
20 | # track management parameters (Step 2)
21 | #confirmed_threshold = 0.8 # track score threshold to switch from 'tentative' to 'confirmed'
22 | #delete_threshold = 0.6 # track score threshold to delete confirmed tracks
23 | window = 10 # number of frames for track score calculation
24 | max_P = 3**2 # delete track if covariance of px or py bigger than this
25 | sigma_p44 = 50 # initial setting for estimation error covariance P entry for vx
26 | sigma_p55 = 50 # initial setting for estimation error covariance P entry for vy
27 | sigma_p66 = 5 # initial setting for estimation error covariance P entry for vz
28 | weight_dim = 0.1 # sliding average parameter for dimension estimation
29 |
30 | # association parameters (Step 3)
31 | gating_threshold = 0.995 # percentage of correct measurements that shall lie inside gate
32 |
33 | # measurement parameters (Step 4)
34 | sigma_lidar_x = 0.1 #0.1 # measurement noise standard deviation for lidar x position
35 | sigma_lidar_y = 0.1 #0.1 # measurement noise standard deviation for lidar y position
36 | sigma_lidar_z = 0.1 #0.1 # measurement noise standard deviation for lidar z position
37 | sigma_cam_i = 5 #5 # measurement noise standard deviation for image i coordinate
38 | sigma_cam_j = 5 #5 # measurement noise standard deviation for image j coordinate
39 |
--------------------------------------------------------------------------------
/student/trackmanagement.py:
--------------------------------------------------------------------------------
1 | # ---------------------------------------------------------------------
2 | # Project "Track 3D-Objects Over Time"
3 | # Copyright (C) 2020, Dr. Antje Muntzinger / Dr. Andreas Haja.
4 | #
5 | # Purpose of this file : Classes for track and track management
6 | #
7 | # You should have received a copy of the Udacity license together with this program.
8 | #
9 | # https://www.udacity.com/course/self-driving-car-engineer-nanodegree--nd013
10 | # ----------------------------------------------------------------------
11 | #
12 |
13 | # imports
14 | import numpy as np
15 | import collections
16 |
17 | # add project directory to python path to enable relative imports
18 | import os
19 | import sys
20 | PACKAGE_PARENT = '..'
21 | SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
22 | sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
23 | import misc.params as params
24 |
25 | class Track:
26 | '''Track class with state, covariance, id, score'''
27 | def __init__(self, meas, id, cnt_frame):
28 | print('creating track no.', id)
29 | M_rot = meas.sensor.sens_to_veh[0:3, 0:3] # rotation matrix from sensor to vehicle coordinates
30 |
31 | ############
32 | # TODO Step 2: initialization:
33 | # - replace fixed track initialization values by initialization of x and P based on
34 | # unassigned measurement transformed from sensor to vehicle coordinates
35 | # - initialize track state and track score with appropriate values
36 | ############
37 |
38 | self.x = np.matrix([[49.53980697],
39 | [ 3.41006279],
40 | [ 0.91790581],
41 | [ 0. ],
42 | [ 0. ],
43 | [ 0. ]])
44 | self.P = np.matrix([[9.0e-02, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00],
45 | [0.0e+00, 9.0e-02, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00],
46 | [0.0e+00, 0.0e+00, 6.4e-03, 0.0e+00, 0.0e+00, 0.0e+00],
47 | [0.0e+00, 0.0e+00, 0.0e+00, 2.5e+03, 0.0e+00, 0.0e+00],
48 | [0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 2.5e+03, 0.0e+00],
49 | [0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 2.5e+01]])
50 | self.state = 'confirmed'
51 | self.score = 0
52 |
53 | # PART 2
54 | z = np.ones((4, 1))
55 | z[:meas.sensor.dim_meas] = meas.z
56 | self.x = np.zeros((6, 1))
57 | self.x[:3] = (meas.sensor.sens_to_veh * z)[:3]
58 | self.P = np.matrix([[9.0e-02, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00],
59 | [0.0e+00, 9.0e-02, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00],
60 | [0.0e+00, 0.0e+00, 6.4e-03, 0.0e+00, 0.0e+00, 0.0e+00],
61 | [0.0e+00, 0.0e+00, 0.0e+00, params.sigma_p44, 0.0e+00, 0.0e+00],
62 | [0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, params.sigma_p55, 0.0e+00],
63 | [0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, params.sigma_p66]])
64 | self.state = 'initialized'
65 | self.score = 1. / params.window
66 | self.assignments = {cnt_frame: [1]} #[1] #[1, 0]
67 | #self.last_assignments = self.assignments
68 |
69 | ############
70 | # END student code
71 | ############
72 |
73 | # other track attributes
74 | self.id = id
75 | self.width = meas.width
76 | self.height = meas.height
77 | self.length = meas.length
78 | self.yaw = np.arccos(M_rot[0,0]*np.cos(meas.yaw) + M_rot[0,1]*np.sin(meas.yaw)) # transform rotation from sensor to vehicle coordinates
79 | self.t = meas.t
80 |
81 | def append_assignment_and_compute_score(self, assignment, cnt_frame):
82 | if cnt_frame in self.assignments:
83 | self.assignments[cnt_frame].append(assignment)
84 | else:
85 | self.assignments[cnt_frame] = [assignment]
86 | points = 0
87 | for frame in self.assignments.keys():
88 | if cnt_frame - frame < params.window:
89 | assignments = self.assignments[frame]
90 | points += any(assignments)
91 | points += len(assignments) > 1 and all(assignments)
92 | self.score = points / float(params.window)
93 | #print(f'score={self.score}')
94 |
95 | def set_x(self, x):
96 | self.x = x
97 |
98 | def set_P(self, P):
99 | self.P = P
100 |
101 | def set_t(self, t):
102 | self.t = t
103 |
104 | def update_attributes(self, meas):
105 | # use exponential sliding average to estimate dimensions and orientation
106 | if meas.sensor.name == 'lidar':
107 | c = params.weight_dim
108 | self.width = c*meas.width + (1 - c)*self.width
109 | self.length = c*meas.length + (1 - c)*self.length
110 | self.height = c*meas.height + (1 - c)*self.height
111 | M_rot = meas.sensor.sens_to_veh
112 | self.yaw = np.arccos(M_rot[0,0]*np.cos(meas.yaw) + M_rot[0,1]*np.sin(meas.yaw)) # transform rotation from sensor to vehicle coordinates
113 |
114 |
115 | ###################
116 |
117 | class Trackmanagement:
118 | '''Track manager with logic for initializing and deleting objects'''
119 | def __init__(self):
120 | self.N = 0 # current number of tracks
121 | self.track_list = []
122 | self.last_id = -1
123 | self.result_list = []
124 |
125 | def manage_tracks(self, unassigned_tracks, unassigned_meas, meas_list, cnt_frame):
126 | ############
127 | # TODO Step 2: implement track management:
128 | # - decrease the track score for unassigned tracks
129 | # - delete tracks if the score is too low or P is too big (check params.py for parameters that might be helpful, but
130 | # feel free to define your own parameters)
131 | ############
132 |
133 | #for meas in meas_list:
134 | # print(f'meas.sensor.name={meas.sensor.name}, meas.z={meas.z.transpose()}\n{meas.sensor.sens_to_veh}=sens_to_veh, meas.sensor.fov={meas.sensor.fov}')
135 |
136 | # decrease score for unassigned tracks
137 | for i in unassigned_tracks:
138 | track = self.track_list[i]
139 | # check visibility
140 | #if meas_list:
141 | # sensor = meas_list[0].sensor
142 | # if sensor.name == 'lidar' and not sensor.in_fov(track.x):
143 | # pass
144 | track.append_assignment_and_compute_score(0, cnt_frame)
145 |
146 | # delete old tracks
147 | to_be_deleted = []
148 | for i in range(len(self.track_list)):
149 | track = self.track_list[i]
150 | P = track.P
151 | if (track.state == 'initialized' and track.score < 0.3 and len(track.assignments) >= 3) or \
152 | (track.state == 'tentative' and track.score <= 0.6) or \
153 | (track.state == 'confirmed' and track.score <= 0.8) or \
154 | (P[0,0] > params.max_P or P[1,1] > params.max_P):
155 | to_be_deleted.append(track)
156 | for track in to_be_deleted:
157 | self.delete_track(track)
158 |
159 | ############
160 | # END student code
161 | ############
162 |
163 | # initialize new track with unassigned measurement
164 | for j in unassigned_meas:
165 | if meas_list[j].sensor.name == 'lidar': # only initialize with lidar measurements
166 | self.init_track(meas_list[j], cnt_frame)
167 |
168 | def addTrackToList(self, track):
169 | self.track_list.append(track)
170 | self.N += 1
171 | self.last_id = track.id
172 |
173 | def init_track(self, meas, cnt_frame):
174 | track = Track(meas, self.last_id + 1, cnt_frame)
175 | self.addTrackToList(track)
176 |
177 | def delete_track(self, track):
178 | print('deleting track no.', track.id)
179 | self.track_list.remove(track)
180 |
181 | def handle_updated_track(self, track, cnt_frame):
182 | ############
183 | # TODO Step 2: implement track management for updated tracks:
184 | # - increase track score
185 | # - set track state to 'tentative' or 'confirmed'
186 | ############
187 |
188 | track.append_assignment_and_compute_score(1, cnt_frame)
189 | if track.state == 'initialized' and track.score >= 0.8:
190 | track.state = 'tentative'
191 | if track.state == 'tentative' and track.score >= 1.5:
192 | track.state = 'confirmed'
193 |
194 | ############
195 | # END student code
196 | ############
197 |
--------------------------------------------------------------------------------