EricPedley
commented
1 month ago Benchmarking results from the orin:
forge@uav-forge-orin:~/uavf_2024
> kernprof -lv tests/imaging/drone_tracker_tests.py
20it [01:16, 3.81s/it]
Wrote profile results to drone_tracker_tests.py.lprof
Timer unit: 1e-06 s
Total time: 65.4881 s
File: /home/forge/uavf_2024/uavf_2024/imaging/particle_filter.py
Function: compute_measurements at line 117
Line # Hits Time Per Hit % Time Line Contents
==============================================================
117 @profile
118 def compute_measurements(self, cam_pose: tuple[np.ndarray, Rotation], states: np.ndarray) -> BoundingBox:
119 '''
120 `states` is (n, 7)
121 returns ndarray of shape (n, 4) where the 4 elements are [x1,y1,x2,y2]
122 '''
123 23260 100939.8 4.3 0.2 cam = CameraModel(self.focal_len_pixels,
124 11630 27494.0 2.4 0.0 [self.resolution[0]/2, self.resolution[1]/2],
125 11630 228127.5 19.6 0.3 cam_pose[1].as_matrix(),
126 11630 50272.7 4.3 0.1 cam_pose[0].reshape(3,1))
127
128 11630 44174.0 3.8 0.1 n = states.shape[0]
129 11630 381818.1 32.8 0.6 positions = states[:, :3]
130 11630 317663.1 27.3 0.5 radii = states[:, -1]
131
132 11630 1312691.4 112.9 2.0 cam_position_tensor = Tensor(cam_pose[0]).to(self._device)
133 11630 594472.2 51.1 0.9 rays_to_center = positions - cam_position_tensor
134
135 11630 12479.0 1.1 0.0 n_samples = 25
136 11630 37973416.9 3265.1 58.0 orthogonal_rays_normalized = make_ortho_vectors(rays_to_center, n_samples)
137
138 11630 1852451.1 159.3 2.8 pts3 = positions[:, None, :] + orthogonal_rays_normalized * radii[:,None,None]
139
140 11630 296315.5 25.5 0.5 pts3_flat = pts3.reshape(-1, 3) # (n*n_samples, 3)
141
142 # project points into the camera
143 11630 16447452.0 1414.2 25.1 projected_points = cam.project(pts3_flat.T, self._device).T.reshape(n, n_samples, 2)
144 11630 1283011.3 110.3 2.0 x_mins = torch.min(projected_points[:,:,0], dim=1).values # (n,)
145 11630 1186818.7 102.0 1.8 y_mins = torch.min(projected_points[:,:,1], dim=1).values # (n,)
146 11630 1225458.6 105.4 1.9 x_maxs = torch.max(projected_points[:,:,0], dim=1).values # (n,)
147 11630 1167045.2 100.3 1.8 y_maxs = torch.max(projected_points[:,:,1], dim=1).values # (n,)
148
149 11630 985989.0 84.8 1.5 return torch.vstack([x_mins, y_mins, x_maxs, y_maxs]).T # (n, 4)
Total time: 14.5859 s
File: /home/forge/uavf_2024/uavf_2024/imaging/particle_filter.py
Function: update at line 151
Line # Hits Time Per Hit % Time Line Contents
==============================================================
151 @profile
152 def update(self, cam_pose: tuple[np.ndarray, Rotation], measurement:Measurement):
153 '''
154 measurements is a list of 2D integer bounding boxes in pixel coordinates (x1,y1,x2,y2)
155 '''
156
157 # add particles to `samples` that would line up with the measurements
158 # self.samples.extend(
159 # self.gen_samples_from_measurement(cam_pose, measurement.box, 10)
160 # )
161
162 19 14454575.8 760767.1 99.1 measurements = self.compute_measurements(cam_pose, self.samples)
163 19 87296.3 4594.5 0.6 likelihoods = self.compute_likelihoods(measurements, measurement.box.to_xyxy())
164
165 # resample the particles
166 19 44032.7 2317.5 0.3 self.resample(likelihoods)
Summary
Speeds up particle filter by around 4x. I think we could still try more performance optimizations but it'd be hard. The slowest thing rn is the measurement function, which has to find vectors on the edges of the bounding spheres and run camera projection on them. The camera projection is like ~20% of the runtime of that, and I think we could speed it up by using quaternions for the camera rotation instead of rotation matrices. Also, the code is somehow slower on the GPU than the CPU, despite the slowest parts being pytorch operations, which suggests to me something is wrong and we could do some debugging of how the data is being moved to GPU and operated on.