def back_project(camera, shot, pixel, depth): K = multiview.K_from_camera(camera) R = cv2.Rodrigues(np.array(shot['rotation'], dtype=float))[0] t = shot['translation'] A = K.dot(R) b = depth * np.array([pixel[0], pixel[1], 1]) - K.dot(t) return np.linalg.solve(A, b)
def triangulate_track(track, graph, reconstruction, P_by_id, KR1_by_id, Kinv_by_id, reproj_threshold, min_ray_angle=2.0): ''' Triangulate a track ''' Ps, Ps_initial, KR1_initial, Kinv_initial = [], [], [], [] xs, xs_initial = [], [] for shot in graph[track]: if shot in reconstruction['shots']: if shot not in P_by_id: s = reconstruction['shots'][shot] c = reconstruction['cameras'][s['camera']] P = projection_matrix(c, s) P_by_id[shot] = P KR1_by_id[shot] = np.linalg.inv(P[:, :3]) Kinv_by_id[shot] = np.linalg.inv(multiview.K_from_camera(c)) Ps_initial.append(P_by_id[shot]) xs_initial.append(graph[track][shot]['feature']) KR1_initial.append(KR1_by_id[shot]) Kinv_initial.append(Kinv_by_id[shot]) valid_set = [] if len(Ps_initial) >= 2: max_angle = 0 for i, j in combinations(range(len(Ps_initial)), 2): angle = angle_between_rays(KR1_initial[i], xs_initial[i], KR1_initial[j], xs_initial[j]) if 1: if i not in valid_set: valid_set.append(i) if j not in valid_set: valid_set.append(j) max_angle = max(angle, max_angle) if max_angle > np.radians(min_ray_angle): for k in valid_set: Ps.append(np.dot(Kinv_initial[k], Ps_initial[k])) xx = np.dot(Kinv_initial[k][:2, :], multiview.homogeneous(np.array(xs_initial[k]))) xs.append(xx[0:2]) X = multiview.triangulate(Ps, xs) error = 0 Xh = multiview.homogeneous(X) for P, x in zip(Ps, xs): xx, yy, zz = P.dot(Xh) if zz <= 0: error = 999999999.0 reprojected_x = np.array([xx / zz, yy / zz]) error = max(error, (reprojected_x - x).max()) if error < reproj_threshold: reconstruction['points'][track] = { "coordinates": list(X), }
def resect(data, graph, reconstruction, shot_id): '''Add a shot to the reconstruction. ''' xs = [] Xs = [] for track in graph[shot_id]: if track in reconstruction['points']: xs.append(graph[shot_id][track]['feature']) Xs.append(reconstruction['points'][track]['coordinates']) x = np.array(xs) X = np.array(Xs) if len(x) < 5: return False exif = data.load_exif(shot_id) camera_model = exif['camera'] K = multiview.K_from_camera(reconstruction['cameras'][camera_model]) dist = np.array([0, 0, 0, 0.]) # Prior on focal length R, t, inliers = cv2.solvePnPRansac(X.astype(np.float32), x.astype(np.float32), K, dist, reprojectionError=data.config.get( 'resection_threshold', 0.004)) if inliers is None: print 'Resection', shot_id, 'no inliers' return False print 'Resection', shot_id, 'inliers:', len(inliers), '/', len(x) if len(inliers) >= data.config.get('resection_min_inliers', 15): reconstruction['shots'][shot_id] = { "camera": camera_model, "rotation": list(R.flat), "translation": list(t.flat), } add_gps_position(data, reconstruction, shot_id) return True else: return False
def projection_matrix(camera, shot): K = multiview.K_from_camera(camera) Rt = Rt_from_shot(shot) return np.dot(K, Rt)