(pts3d, focal, msk, device, pp=None, niter_PnP=10)
| 347 | |
| 348 | |
| 349 | def fast_pnp(pts3d, focal, msk, device, pp=None, niter_PnP=10): |
| 350 | # extract camera poses and focals with RANSAC-PnP |
| 351 | if msk.sum() < 4: |
| 352 | return None # we need at least 4 points for PnP |
| 353 | pts3d, msk = map(to_numpy, (pts3d, msk)) |
| 354 | |
| 355 | H, W, THREE = pts3d.shape |
| 356 | assert THREE == 3 |
| 357 | pixels = pixel_grid(H, W) |
| 358 | |
| 359 | if focal is None: |
| 360 | S = max(W, H) |
| 361 | tentative_focals = np.geomspace(S / 2, S * 3, 21) |
| 362 | else: |
| 363 | tentative_focals = [focal] |
| 364 | |
| 365 | if pp is None: |
| 366 | pp = (W / 2, H / 2) |
| 367 | else: |
| 368 | pp = to_numpy(pp) |
| 369 | |
| 370 | best = (0,) |
| 371 | for focal in tentative_focals: |
| 372 | K = np.float32([(focal, 0, pp[0]), (0, focal, pp[1]), (0, 0, 1)]) |
| 373 | |
| 374 | success, R, T, inliers = cv2.solvePnPRansac( |
| 375 | pts3d[msk], |
| 376 | pixels[msk], |
| 377 | K, |
| 378 | None, |
| 379 | iterationsCount=niter_PnP, |
| 380 | reprojectionError=5, |
| 381 | flags=cv2.SOLVEPNP_SQPNP, |
| 382 | ) |
| 383 | if not success: |
| 384 | continue |
| 385 | |
| 386 | score = len(inliers) |
| 387 | if success and score > best[0]: |
| 388 | best = score, R, T, focal |
| 389 | |
| 390 | if not best[0]: |
| 391 | return None |
| 392 | |
| 393 | _, R, T, best_focal = best |
| 394 | R = cv2.Rodrigues(R)[0] # world to cam |
| 395 | R, T = map(torch.from_numpy, (R, T)) |
| 396 | return best_focal, inv(sRT_to_4x4(1, R, T, device)) # cam to world |
| 397 | |
| 398 | |
| 399 | def get_med_dist_between_poses(poses): |
no test coverage detected