(step, depth, K, f, cx, cy, x_res, y_res)
| 74 | return K |
| 75 | |
| 76 | def generate_sampling_frustum(step, depth, K, f, cx, cy, x_res, y_res): |
| 77 | x_max = depth * (x_res - cx) / f |
| 78 | x_min = -depth * cx / f |
| 79 | y_max = depth * (y_res - cy) / f |
| 80 | y_min = -depth * cy / f |
| 81 | |
| 82 | zs = np.arange(0, depth, step) |
| 83 | xs = np.arange(x_min, x_max, step) |
| 84 | ys = np.arange(y_min, y_max, step) |
| 85 | |
| 86 | X0 = [] |
| 87 | |
| 88 | for z in zs: |
| 89 | for x in xs: |
| 90 | for y in ys: |
| 91 | P = np.array([x, y, z]) |
| 92 | p = np.dot(K, P) |
| 93 | if p[2] < 0.00001: |
| 94 | continue |
| 95 | p = p / p[2] |
| 96 | if is_inside_frustum(p, x_res, y_res): |
| 97 | X0.append(P) |
| 98 | X0 = np.array(X0) |
| 99 | return X0 |
| 100 | |
| 101 | def compute_frustums_overlap(pose0, pose1, sampling_frustum, K, x_res, y_res): |
| 102 | R0 = pose0[0:3, 0:3] |
no test coverage detected