def track_camera(corner_storage, intrinsic_mat, known_view1, known_view2): n = len(corner_storage) point_cloud_builder = PointCloudBuilder() view_mat = [eye3x4()] * n view_mat[known_view1[0]] = pose_to_view_mat3x4(known_view1[1]) view_mat[known_view2[0]] = pose_to_view_mat3x4(known_view2[1]) min_angle = 1.0 max_error = 1.0 new_points = [] ids = [] correspondences = build_correspondences(corner_storage[known_view1[0]], corner_storage[known_view2[0]]) while len(new_points) < 10: new_points, ids, _ = triangulate_correspondences( correspondences, pose_to_view_mat3x4(known_view1[1]), pose_to_view_mat3x4(known_view2[1]), intrinsic_mat, TriangulationParameters(max_error, min_angle, 0)) min_angle -= 0.2 max_error += 0.4 point_cloud_builder.add_points(ids, new_points) for i in range(known_view1[0] - 1, -1, -1): solve_frame(i, corner_storage, point_cloud_builder, view_mat, intrinsic_mat, 1) for i in range(known_view1[0] + 1, n): solve_frame(i, corner_storage, point_cloud_builder, view_mat, intrinsic_mat, -1) return view_mat, point_cloud_builder
def _test_frame_pair(intrinsic_mat: np.ndarray, corners_1: FrameCorners, corners_2: FrameCorners, param_koeff: float = 1) -> Tuple[int, Optional[Pose]]: correspondences = build_correspondences(corners_1, corners_2) if len(correspondences.ids) < 6: return 0, None points2d_1 = correspondences.points_1 points2d_2 = correspondences.points_2 essential, essential_inliers = cv2.findEssentialMat(points2d_1, points2d_2, intrinsic_mat, threshold=param_koeff) homography, homography_inliers = cv2.findHomography(points2d_1, points2d_2, method=cv2.RANSAC) if len(np.where(homography_inliers > 0)[0]) > len(np.where(essential_inliers > 0)[0]): return 0, None if essential.shape != (3, 3): return 0, None num_passed, rot, t, mask = cv2.recoverPose(essential, points2d_1, points2d_2, intrinsic_mat, mask=essential_inliers) outlier_ids = np.array( [pt_id for pt_id, mask_elem in zip(correspondences.ids, mask) if mask_elem[0] == 0], dtype=correspondences.ids.dtype) inlier_correspondences = _remove_correspondences_with_ids(correspondences, outlier_ids) if len(inlier_correspondences.ids) < 4: return 0, None view_matr_1 = eye3x4() view_matr_2 = np.hstack((rot, t)) triangulation_params = TriangulationParameters(param_koeff, 1.0 / param_koeff, 0.0) pts_3d, trianulated_ids, med_cos = triangulate_correspondences(inlier_correspondences, view_matr_1, view_matr_2, intrinsic_mat, triangulation_params) if len(pts_3d) < 4: return 0, None print(len(inlier_correspondences.ids), len(pts_3d)) return len(pts_3d), view_mat3x4_to_pose(view_matr_2)
def pose_by_frames(frame1: FrameCorners, frame2: FrameCorners, intrinsic_mat: np.ndarray) -> Tuple[Pose, int]: correspondences = build_correspondences(frame1, frame2) mat, mask = cv2.findEssentialMat(correspondences.points_1, correspondences.points_2, intrinsic_mat, cv2.RANSAC, 0.99, 1) if mat is None or mat.shape != (3, 3): return None, 0 if mask is not None: correspondences = _remove_correspondences_with_ids( correspondences, np.argwhere(mask.flatten() == 0)) R1, R2, t = cv2.decomposeEssentialMat(mat) max_pose = None max_npoints = 0 for mat in [R1.T, R2.T]: for vec in [t, -t]: pose = Pose(mat, mat @ vec) points, _, _ = triangulate_correspondences( correspondences, eye3x4(), pose_to_view_mat3x4(pose), intrinsic_mat, INITIAL_TRIANGULATION_PARAMETERS) if len(points) > max_npoints: max_pose = pose max_npoints = len(points) return max_pose, max_npoints
def retriangulate_point(self, point_id): coords = [] frames = [] v_mats = [] for i, frame in enumerate(self.corner_storage): if point_id in frame.ids and i in self.view_mats.keys(): idx = np.where(frame.ids == point_id)[0][0] coords += [frame.points[idx]] frames += [i] v_mats += [self.view_mats[i]] if len(coords) < 3: return None, None if len(coords) > self.retriangulate_frames: idxs = np.random.choice(len(coords), size=self.retriangulate_frames, replace=False) coords = np.array(coords)[idxs] frames = np.array(frames)[idxs] v_mats = np.array(v_mats)[idxs] best_coords = None best_cnt = 0 for _ in range(4): i, j = np.random.choice(len(coords), 2, replace=True) corrs = Correspondences(np.array([point_id]), np.array([coords[i]]), np.array([coords[j]])) point3d, _, _ = triangulate_correspondences(corrs, v_mats[i], v_mats[j], self.intrinsic_mat, self.triangulation_parameters) if len(point3d) == 0: continue errors = np.array([compute_reprojection_errors(point3d, np.array([p]), self.intrinsic_mat @ m) for m, p in zip(v_mats, coords)]) cnt = np.sum(errors < self.max_repr_error) if best_coords is None or best_cnt < cnt: best_cnt = cnt best_coords = point3d if best_cnt == 0: return None, None return best_coords, best_cnt
def triangulate(frame_0, frame_1, params=TriangulationParameters(2, 1e-3, 1e-4), ids_to_remove=None) \ -> Tuple[np.ndarray, np.ndarray, float]: corrs = build_correspondences(corner_storage[frame_0], corner_storage[frame_1]) return triangulate_correspondences(corrs, view_mats[frame_0], view_mats[frame_1], intrinsic_mat, params)
def calc_known_views(corner_storage, intrinsic_mat, indent=5): num_frames = len(corner_storage) known_view_1 = (None, None) known_view_2 = (None, None) num_points = -1 for frame_1 in range(num_frames): for frame_2 in range(frame_1 + indent, num_frames): corrs = build_correspondences(corner_storage[frame_1], corner_storage[frame_2]) if len(corrs.ids) < 6: continue points_1 = corrs.points_1 points_2 = corrs.points_2 H, mask_h = cv2.findHomography(points_1, points_2, method=cv2.RANSAC) if mask_h is None: continue mask_h = mask_h.reshape(-1) E, mask_e = cv2.findEssentialMat(points_1, points_2, method=cv2.RANSAC, cameraMatrix=intrinsic_mat) if mask_e is None: continue mask_e = mask_e.reshape(-1) if mask_h.sum() / mask_e.sum() > 0.5: continue corrs = Correspondences(corrs.ids[(mask_e == 1)], points_1[(mask_e == 1)], points_2[(mask_e == 1)]) R1, R2, t = cv2.decomposeEssentialMat(E) for poss_pose in [ Pose(R1.T, R1.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ t), Pose(R2.T, R2.T @ (-t)) ]: points3d, _, _ = triangulate_correspondences( corrs, eye3x4(), pose_to_view_mat3x4(poss_pose), intrinsic_mat, TriangulationParameters(1, 2, .1)) if len(points3d) > num_points: num_points = len(points3d) known_view_1 = (frame_1, view_mat3x4_to_pose(eye3x4())) known_view_2 = (frame_2, poss_pose) return known_view_1, known_view_2
def add_points_to_cloud(point_cloud_builder, corners1, corners2, view1, view2, intrinsic_mat, triang_params): correspondences = build_correspondences(corners1, corners2, ids_to_remove=point_cloud_builder.ids) points, ids, median_cos = triangulate_correspondences(correspondences, view1, view2, intrinsic_mat, triang_params) point_cloud_builder.add_points(ids, points) return len(points)
def track(iters, trackers, cloud, camera): start_ids, start_points = cloud.ids, cloud.points for iter in range(iters): for frame_num, t1 in enumerate(trackers): print("\rIteration {}/{}, triangulate for frame {}/{}".format( iter + 1, iters, t1.id, len(trackers)), end=' ' * 20) if t1.mtx is None: continue for j in range(frame_num - FRAMES_MAX_WINDOW, frame_num + FRAMES_MAX_WINDOW + 1, FRAMES_STEP): if j < 0 or j >= len(trackers): continue t2 = trackers[j] if abs(t1.id - t2.id) <= FRAMES_MIN_WINDOW or t2.mtx is None: continue corrs = build_correspondences(t1.corners, t2.corners) if not len(corrs.ids): continue points, ids, _ = triangulate_correspondences( corrs, t1.mtx, t2.mtx, camera, triang_params) if len(points): cloud.add_points(ids, points) fc = FrequencyCounter() for t in trackers: inliers = t.pnp(cloud, camera) inliers = np.array(inliers).flatten().astype(int) print("\rIteration {}/{}, PnP for frame {}/{}, {} inliners".format( iter + 1, iters, t.id, len(trackers), len(inliers)), end=' ' * 20) if len(inliers): ids1, ids2 = build_index_intersection(cloud.ids, inliers) fc.add(inliers[ids2], cloud.points[ids1], t.corners.ids) good_points_ids, good_points = fc.get_freqs_above( INLINER_FREQUENCY_TRASHHOLD, MIN_INLINER_FRAMES) cloud = PointCloudBuilder(good_points_ids, good_points) for t in trackers: t.pnp(cloud, camera) print("\rIteration {}/{}, {} points in the cloud".format( iter + 1, iters, len(cloud.ids)), end=' ' * 20 + "\n") fc = FrequencyCounter() for t in trackers: inliers = t.pnp(cloud, camera) inliers = np.array(inliers).flatten().astype(int) if len(inliers): ids1, ids2 = build_index_intersection(cloud.ids, inliers) fc.add(inliers[ids2], cloud.points[ids1], t.corners.ids) good_points_ids, good_points = fc.get_freqs_above( INLINER_FREQUENCY_TRASHHOLD, MIN_INLINER_FRAMES) cloud = PointCloudBuilder(good_points_ids, good_points) return cloud
def get_matrix_poses(corner_storage, intrisinc_mat): pairs = get_best_intersected(corner_storage) best_pair = -1, -1 best_pair_result = -1 for i, j, _ in pairs[:100]: ids1, ids2 = build_index_intersection(corner_storage[i].ids, corner_storage[j].ids) points1 = corner_storage[i].points[ids1] points2 = corner_storage[j].points[ids2] E, mask = cv2.findEssentialMat(points1, points2, focal=intrisinc_mat[0][0]) if mask.sum() < 10: continue F, mask = cv2.findFundamentalMat(points1, points2) if mask.sum() < 10: continue _, R, t, mask = cv2.recoverPose(E, points1, points1, focal=intrisinc_mat[0][0]) if mask.sum() < 10: continue corrs = build_correspondences(corner_storage[i], corner_storage[j]) points, ids, _ = triangulate_correspondences( corrs, eye3x4(), np.hstack((R, t)), intrisinc_mat, TriangulationParameters(max_reprojection_error=5, min_triangulation_angle_deg=5., min_depth=0.001)) current_result = len(ids) // 20 if current_result > best_pair_result: best_pair = i, j best_pair_result = current_result i, j = best_pair ids1, ids2 = build_index_intersection(corner_storage[i].ids, corner_storage[j].ids) points1 = corner_storage[i].points[ids1] points2 = corner_storage[j].points[ids2] E, mask = cv2.findEssentialMat(points1, points2, focal=intrisinc_mat[0][0]) F, mask = cv2.findFundamentalMat(points1, points2) _, R, t, mask = cv2.recoverPose(E, points1, points1, focal=intrisinc_mat[0][0]) print(f"Chosen frames {i} and {j}") return (i, view_mat3x4_to_pose(eye3x4())),\ (j, view_mat3x4_to_pose(np.hstack((R, t))))
def _update_cloud(self, ind1, ind2): frame1, frame2 = self._corner_storage[ind1], self._corner_storage[ind2] mat1, mat2 = self._track[ind1], self._track[ind2] correspondences = build_correspondences(frame1, frame2) if len(correspondences.ids) == 0: return 0 points, ids, _ = triangulate_correspondences( correspondences, mat1, mat2, self._intrinsic_mat, self._triangulation_parameters) self._builder.add_points(ids, points) return len(ids)
def enrich_point_cloud(self, view_1: np.ndarray, view_2: np.ndarray, corners_1: FrameCorners, corners_2: FrameCorners, triangulation_parameters: TriangulationParameters) -> None: correspondences = build_correspondences(corners_1, corners_2) points, ids, median_cos = triangulate_correspondences( correspondences, view_1, view_2, self.intrinsic_mat, triangulation_parameters) self.point_cloud_builder.add_points(ids, points)
def init_first_camera_positions(intrinsic_mat, corner_storage): frame_count = len(corner_storage) best_pair = (-1, -1) zero_view_mat = eye3x4() best_view_mat = None best_triangulated_points = -1 confidence = 0.9 params = TriangulationParameters(max_reprojection_error=2, min_triangulation_angle_deg=1, min_depth=0.5) for i in range(0, frame_count, 10): print("Init first camera. Frame %d/%d" % (i + 1, frame_count)) for j in range(i + 3, min(i + 30, frame_count), 3): correspondences = build_correspondences(corner_storage[i], corner_storage[j]) if len(correspondences.ids) < 5: continue points_1, points_2 = correspondences.points_1, correspondences.points_2 e_matrix, e_mask = findEssentialMat(points_1, points_2, intrinsic_mat, method=RANSAC, threshold=2, prob=confidence) h_matrix, h_mask = findHomography(points_1, points_2, method=RANSAC, ransacReprojThreshold=2, confidence=confidence) e_inliers, h_inliers = sum(e_mask.reshape(-1)), sum( h_mask.reshape(-1)) if e_inliers / h_inliers < 0.1: continue outliers = np.delete(correspondences.ids, correspondences.ids[e_mask]) correspondences = build_correspondences(corner_storage[i], corner_storage[j], outliers) R1, R2, t = decomposeEssentialMat(e_matrix) for rv in [R1, R2]: for tv in [-t, t]: candidate_veiw_mat = np.hstack((rv, tv)) points, ids, _ = triangulate_correspondences( correspondences, zero_view_mat, candidate_veiw_mat, intrinsic_mat, params) if len(points) > best_triangulated_points: best_triangulated_points = len(points) best_pair = (i, j) best_view_mat = candidate_veiw_mat return (best_pair[0], zero_view_mat), (best_pair[1], best_view_mat)
def update_cloud(j): if i != j and j not in err_indexes: correspondences_i = build_correspondences(corner_storage[i], corner_storage[j]) points3d_j, corr_ids_j, median_cos = triangulate_correspondences( correspondences_i, view_mats[i], view_mats[j], intrinsic_mat, TRIANG_PARAMS) if len(points3d_j) > MIN_SIZE: point_cloud_builder.add_points(corr_ids_j.astype(np.int64), points3d_j) tqdm_iter.set_description( template.format(len(inliers), len(points3d_j), len(point_cloud_builder._points)))
def solve_frame(frame_num, corner_storage, point_cloud_builder, view_mat, intrinsic_mat, direction): print("Frame ", frame_num) corners = corner_storage[frame_num] _, corner_indexes, points_indexes = np.intersect1d(corners.ids, point_cloud_builder.ids, assume_unique=True, return_indices=True) corners = corners.points[corner_indexes] points = point_cloud_builder.points[points_indexes] x, r, t, inliers = cv2.solvePnPRansac(points, corners, intrinsic_mat, None, reprojectionError=1.0, flags=cv2.SOLVEPNP_EPNP) good_corners = corners[inliers] good_points = points[inliers] x, r, t = cv2.solvePnP(good_points, good_corners, intrinsic_mat, None, r, t, useExtrinsicGuess=True, flags=cv2.SOLVEPNP_ITERATIVE) print("Points in cloud ", len(point_cloud_builder.ids)) new_views = rodrigues_and_translation_to_view_mat3x4(r, t) if new_views is None: view_mat[frame_num] = view_mat[frame_num + direction] else: view_mat[frame_num] = new_views for i in range(40): other_frame_num = frame_num + i * direction if check_baseline(view_mat[other_frame_num], view_mat[frame_num], 0.1): correspondences = build_correspondences( corner_storage[other_frame_num], corner_storage[frame_num], ids_to_remove=point_cloud_builder.ids) if len(correspondences) != 0: new_points, ids, _ = triangulate_correspondences( correspondences, view_mat[other_frame_num], view_mat[frame_num], intrinsic_mat, TriangulationParameters(1.0, 1.0, 0.1)) point_cloud_builder.add_points(ids, new_points)
def get_pose_with_score(frame_1, frame_2, corner_storage, intrinsic_mat): corners1, corners2 = corner_storage[frame_1], corner_storage[frame_2] correspondences = build_correspondences(corners1, corners2) essential_mat, mask_essential = cv2.findEssentialMat(correspondences[1], correspondences[2], intrinsic_mat, method=cv2.RANSAC, threshold=1.0) if essential_mat is None or mask_essential is None: return None, 0 _, mask_homography = cv2.findHomography(correspondences[1], correspondences[2], method=cv2.RANSAC) essential_inliers, homography_inliers = mask_essential.flatten().sum( ), mask_homography.flatten().sum() if homography_inliers > essential_inliers * 0.5: return None, 0 correspondences = _remove_correspondences_with_ids( correspondences, np.argwhere(mask_essential == 0)) R1, R2, t = cv2.decomposeEssentialMat(essential_mat) candidates = [ Pose(R1.T, R1.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ t), Pose(R2.T, R2.T @ (-t)) ] best_pose_score, best_pose = 0, None triangulation_parameters = TriangulationParameters(1, 2, .1) for pose in candidates: points, _, _ = triangulate_correspondences(correspondences, eye3x4(), pose_to_view_mat3x4(pose), intrinsic_mat, triangulation_parameters) if len(points) > best_pose_score: best_pose_score = len(points) best_pose = pose return best_pose, best_pose_score
def triangulate(ind_1, ind_2, pose_1, pose_2, corner_storage, intrinsic_mat, ids_to_remove, parameters=TriangulationParameters(8.0, 0, 2)): frame_corners_1, frame_corners_2 = corner_storage[ind_1], corner_storage[ ind_2] correspondences = build_correspondences(frame_corners_1, frame_corners_2, ids_to_remove) view_1, view_2 = pose_to_view_mat3x4(pose_1), pose_to_view_mat3x4(pose_2) return triangulate_correspondences(correspondences, view_1, view_2, intrinsic_mat, parameters)
def _add_points_from_frame(self, frame_num_1: int, frame_num_2: int, initial_triangulation: bool = False) -> int: corners_1 = self.corner_storage[frame_num_1] corners_2 = self.corner_storage[frame_num_2] correspondences = build_correspondences(corners_1, corners_2, ids_to_remove=self.pc_builder.ids) if len(correspondences.ids) > 0: max_reproj_error = 1.0 min_angle = 1.0 view_1 = self.tracked_views[frame_num_1] view_2 = self.tracked_views[frame_num_2] triangulation_params = TriangulationParameters(max_reproj_error, min_angle, 0) pts_3d, triangulated_ids, med_cos = triangulate_correspondences(correspondences, view_1, view_2, self.intrinsic_mat, triangulation_params) if initial_triangulation: num_iter = 0 while len(pts_3d) < 8 and len(correspondences.ids) > 7 and num_iter < 100: max_reproj_error *= 1.2 min_angle *= 0.8 triangulation_params = TriangulationParameters(max_reproj_error, min_angle, 0) pts_3d, triangulated_ids, med_cos = triangulate_correspondences(correspondences, view_1, view_2, self.intrinsic_mat, triangulation_params) num_iter += 1 if num_iter >= 100 and len(pts_3d) < 4: raise TrackingError('Failed to triangulate enough points') self.pc_builder.add_points(triangulated_ids, pts_3d) return len(pts_3d) elif initial_triangulation: raise TrackingError('Not found correspondences on image pair') else: return 0
def try_add_new_point_to_cloud(corner_id): # все фреймы на которых есть данный уголок, и у которых уже найдена view_mat # а можно для уголка хранить не все фреймы где он есть, а только те, где он инлаер! frames = [] for frame in frames_of_corner[corner_id]: if view_mats[frame[0]] is not None: frames.append(frame) if len(frames) < 2: return max_dist = 0 best_frames = [None, None] best_ids = [None, None] # можно ускорить хотя бы вдвое for frame_1 in frames: for frame_2 in frames: if frame_1 == frame_2: continue # эм, по идее это не возможно, когда есть view_mat есть и t_vec if t_vecs[frame_1[0]] is None or t_vecs[frame_2[0]] is None: continue t_vec_1 = t_vecs[frame_1[0]] t_vec_2 = t_vecs[frame_2[0]] dist = np.linalg.norm(t_vec_1 - t_vec_2) if max_dist < dist: max_dist = dist best_frames = [frame_1[0], frame_2[0]] best_ids = [frame_1[1], frame_2[1]] if max_dist > MIN_DIST_TRIANGULATE: ids = np.array([corner_id]) points1 = corner_storage[best_frames[0]].points[np.array( [best_ids[0]])] points2 = corner_storage[best_frames[1]].points[np.array( [best_ids[1]])] # что за ужасный синтаксис?????? corrs = corrs = Correspondences(ids, points1, points2) points3d, ids, med_cos = triangulate_correspondences( corrs, view_mats[best_frames[0]], view_mats[best_frames[1]], intrinsic_mat, parameters=TriangulationParameters(2, 1e-3, 1e-4)) # зачем проверка??? зачем делаем поп????? if len(points3d) > 0: points_cloud[ids[0]] = points3d[0] cur_corners_occurencies.pop(ids[0], None)
def __init__(self, corner_storage, intrinsic_mat, view_1, view_2): self.corner_storage = corner_storage self.frame_count = len(corner_storage) self.intrinsic_mat = intrinsic_mat self.triangulation_parameters = TriangulationParameters(1, 3, .1) self.view_mats = {} if view_1 is None or view_2 is None: print('Finding initial poses') pose1_idx, pose1, pose2_idx, pose2 = self.find_best_start_poses() print('Initial poses found in frames {0} and {1}'.format(pose1_idx, pose2_idx)) else: pose1 = pose_to_view_mat3x4(view_1[1]) pose2 = pose_to_view_mat3x4(view_2[1]) pose1_idx = view_1[0] pose2_idx = view_2[0] corners_1 = self.corner_storage[pose1_idx] corners_2 = self.corner_storage[pose2_idx] corrs = build_correspondences(corners_1, corners_2) p, ids, med = triangulate_correspondences(corrs, pose1, pose2, self.intrinsic_mat, self.triangulation_parameters) self.ids = ids self.point_cloud_builder = PointCloudBuilder(ids, p) self.point_frames = {} self.last_retriangulated = {} for i in ids: self.point_frames[i] = [pose1_idx, pose2_idx] self.last_retriangulated[i] = 2 self.used_inliers = {} self.view_nones = {i for i in range(self.frame_count)} self.view_mats[pose1_idx] = pose1 self.used_inliers[pose1_idx] = 0 self.view_nones.remove(pose1_idx) self.view_mats[pose2_idx] = pose2 self.used_inliers[pose2_idx] = 0 self.view_nones.remove(pose2_idx) self.last_added_idx = pose2_idx self.last_inliers = [] self.retriangulate_frames = 3 self.max_repr_error = 1.5 self.used_inliers_point = {} self.step = 0
def calculate_known_views( intrinsic_mat, corner_storage: CornerStorage, min_correspondencies_count=100, max_homography=0.7, ) -> Tuple[Tuple[int, Pose], Tuple[int, Pose]]: best_points_num, best_known_views = -1, ((None, None), (None, None)) for i in range(len(corner_storage)): for j in range(i + 1, min(i + 40, len(corner_storage))): corresp = build_correspondences(corner_storage[i], corner_storage[j]) if len(corresp[0]) < min_correspondencies_count: break E, mask = cv2.findEssentialMat(corresp.points_1, corresp.points_2, cameraMatrix=intrinsic_mat, method=cv2.RANSAC) mask = (mask.squeeze() == 1) corresp = Correspondences(corresp.ids[mask], corresp.points_1[mask], corresp.points_2[mask]) if E is None: continue # Validate E using homography H, mask = cv2.findHomography(corresp.points_1, corresp.points_2, method=cv2.RANSAC) if np.count_nonzero(mask) / len(corresp.ids) > max_homography: continue R1, R2, T = cv2.decomposeEssentialMat(E) for view_mat2 in [ np.hstack((R, t)) for R in [R1, R2] for t in [T, -T] ]: view_mat1 = np.eye(3, 4) points, _, _ = triangulate_correspondences( corresp, view_mat1, view_mat2, intrinsic_mat, triang_params) print('Try frames {}, {}: {} correspondent points'.format( i, j, len(points))) if len(points) > best_points_num: best_known_views = ((i, view_mat3x4_to_pose(view_mat1)), (j, view_mat3x4_to_pose(view_mat2))) best_points_num = len(points) if best_points_num > 1500: return best_known_views return best_known_views
def find_init_points(frame_num_1: int, frame_num_2: int, view_1: np.ndarray, view_2: np.ndarray, point_storage: PointStorage, intrinsic_mat: np.ndarray) -> None: ids_1, corners_1 = point_storage.frame_points(frame_num_1, detected=False) ids_2, corners_2 = point_storage.frame_points(frame_num_2, detected=False) correspondences = build_correspondences(ids_1, ids_2, corners_1, corners_2) points3d, point_ids, _ = triangulate_correspondences( correspondences, view_1, view_2, intrinsic_mat, TRIANGULATION_PARAMETERS ) point_storage.set_3d(point_ids, points3d)
def try_restore_relative_pose(self, correspondences: Correspondences, distance_inlier_threshold: np.float64, # 1.0 reprojection_threshold: np.float64, # 3.0 homography_inlier_ration_threshold: np.float64) \ -> Optional[Tuple[np.ndarray, int, np.float]]: if len(correspondences.ids) < 6: return None essential_mat, essential_inliers_mask = cv2.findEssentialMat( correspondences.points_1, correspondences.points_2, self.intrinsic_mat, method=cv2.RANSAC, prob=INIT_RANSAC_CONFIDENCE, threshold=distance_inlier_threshold ) _, homography_inliers_mask = cv2.findHomography( correspondences.points_1, correspondences.points_2, method=cv2.RANSAC, ransacReprojThreshold=reprojection_threshold ) homography_inlier_ratio = homography_inliers_mask.sum(axis=None) / essential_inliers_mask.sum(axis=None) if homography_inlier_ratio > homography_inlier_ration_threshold: return None recover_inliers_count, r_mat, t_vec, recover_inliers_mask = cv2.recoverPose( essential_mat, correspondences.points_1, correspondences.points_2, self.intrinsic_mat, mask=essential_inliers_mask ) recover_inliers_mask = recover_inliers_mask.flatten() assert (recover_inliers_mask.sum() == recover_inliers_count) view = np.hstack([r_mat, t_vec]) correspondences = Correspondences( correspondences.ids[recover_inliers_mask], correspondences.points_1[recover_inliers_mask], correspondences.points_2[recover_inliers_mask] ) _, ids, median_cos = triangulate_correspondences(correspondences, eye3x4(), view, self.intrinsic_mat, TRIANGULATION_PARAMS) return view, len(ids), median_cos
def _retriangulate(self, point_id, max_pairs=5): frames, corners, poses = [], [], [] for frame, index_on_frame in self.corner_pos_in_frames[point_id]: if self.tracked_poses[frame] is not None: frames.append(frame) corners.append( self.corner_storage[frame].points[index_on_frame]) poses.append(self.tracked_poses[frame].pos) if len(frames) < 2: return None if len(frames) == 2: cloud_pts, _ = self._triangulate(frames[0], frames[1]) if len(cloud_pts) == 0: return None return cloud_pts[0], 2 frames, corners, poses = np.array(frames), np.array(corners), np.array( poses) best_pos, best_inliers = None, None for _ in range(max_pairs): frame_1, frame_2 = np.random.choice(len(frames), 2, replace=False) corner_pos_1, corner_pos_2 = corners[frame_1], corners[frame_2] cloud_pts, _, _ = triangulate_correspondences( Correspondences(np.zeros(1), np.array([corner_pos_1]), np.array([corner_pos_2])), poses[frame_1], poses[frame_2], self.intrinsic_mat, TriangulationParameters(self.MAX_REPROJ_ERR, 2.5, 0.0)) if len(cloud_pts) == 0: continue inliers = 0 for frame, corner in zip(frames, corners): repr_errors = compute_reprojection_errors( cloud_pts, np.array([corner]), self.intrinsic_mat @ self.tracked_poses[frame].pos) inliers += np.sum(repr_errors <= self.MAX_REPROJ_ERR) if best_pos is None or best_inliers < inliers: best_pos = cloud_pts[0] best_inliers = inliers if best_pos is None: return None return best_pos, best_inliers
def _triangulate(self, frame_num_1, frame_num_2): corners_1 = self.corner_storage[frame_num_1] corners_2 = self.corner_storage[frame_num_2] corresps = build_correspondences(corners_1, corners_2, ids_to_remove=np.array(list( map(int, self.point_cloud.keys())), dtype=int)) view_1 = self.tracked_poses[frame_num_1].pos view_2 = self.tracked_poses[frame_num_2].pos triangulation_params = TriangulationParameters(1, 1, 0) pts_3d, triangulated_ids, med_cos = triangulate_correspondences( corresps, view_1, view_2, self.intrinsic_mat, triangulation_params) return pts_3d, triangulated_ids
def select_frames(frames, corner_storage, camera_matrix): frame1 = (0, view_mat3x4_to_pose(eye3x4())) frame2 = (-1, view_mat3x4_to_pose(eye3x4())) mx = 0 for i in range(1, len(frames)): correspondences = build_correspondences(corner_storage[frame1[0]], corner_storage[i]) if len(correspondences.ids) < 8: continue E, mask = cv2.findEssentialMat(correspondences.points_1, correspondences.points_2, camera_matrix, method=cv2.RANSAC) if mask is None: continue correspondences = _remove_correspondences_with_ids( correspondences, np.argwhere(mask.flatten() == 0)) if len(correspondences.ids) < 8 or not validate(correspondences, mask): continue R1, R2, t = cv2.decomposeEssentialMat(E) ps = [ Pose(R1.T, R1.T @ t), Pose(R2.T, R2.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ (-t)) ] for pose in ps: points, _, _ = triangulate_correspondences( correspondences, pose_to_view_mat3x4(frame1[1]), pose_to_view_mat3x4(pose), camera_matrix, TRIANG_PARAMS) if len(points) > mx: frame2 = (i, pose) mx = len(points) print(frame1[0], frame2[0]) return frame1, frame2
def find_view_by_two_frames(ids_1: np.ndarray, ids_2: np.ndarray, corners_1: np.ndarray, corners_2: np.ndarray, intrinsic_mat: np.ndarray) \ -> Tuple[Optional[np.ndarray], Optional[np.ndarray], int]: correspondences = build_correspondences(ids_1, ids_2, corners_1, corners_2) if len(correspondences.ids) < 7: return None, None, 0 mat, mat_mask = cv2.findEssentialMat( correspondences.points_1, correspondences.points_2, intrinsic_mat, method=cv2.RANSAC, prob=RANSAC_P, threshold=MAX_EPIPOL_LINE_DIST ) mat_mask = mat_mask.flatten() correspondences = remove_correspondences_with_ids( correspondences, np.argwhere(mat_mask == 0).astype(np.int32)) view_1 = eye3x4() best_view = None best_count = -1 rotation_1, rotation_2, translation = cv2.decomposeEssentialMat(mat) rotation_1, rotation_2 = rotation_1.T, rotation_2.T for r in (rotation_1, rotation_2): for t in (translation, -translation): view_2 = pose_to_view_mat3x4(Pose(r, r @ t)) _, point_ids, _ = triangulate_correspondences( correspondences, view_1, view_2, intrinsic_mat, TRIANGULATION_PARAMETERS ) if best_count < len(point_ids): best_view = view_2 best_count = len(point_ids) return view_1, best_view, best_count
def calc_starting_points( intrinsic_mat: np.ndarray, corner_storage: CornerStorage, known_view_1: Tuple[int, Pose], known_view_2: Tuple[int, Pose]) -> Tuple[np.ndarray, np.ndarray]: id1, pose1 = known_view_1 id2, pose2 = known_view_2 corners1 = corner_storage[id1] corners2 = corner_storage[id2] mat1 = pose_to_view_mat3x4(pose1) mat2 = pose_to_view_mat3x4(pose2) correspondences = build_correspondences(corners1, corners2) points_3d, points_ids, _ = triangulate_correspondences( correspondences, mat1, mat2, intrinsic_mat, INITIAL_TRIANGULATION_PARAMETERS) return points_3d, points_ids
def extract_points(corners1, corners2, view_mat1, view_mat2, intrinsic_mat, ids_to_remove=None): corr = build_correspondences(corners1, corners2, ids_to_remove) if not len(corr.ids): return None points3d, corr_ids, median_cos = triangulate_correspondences( corr, view_mat1, view_mat2, intrinsic_mat, TriangulationParameters(max_reprojection_error=1, min_triangulation_angle_deg=1, min_depth=0.1)) if not len(corr_ids): return None return corr_ids, points3d
def add_new_points(frames, view_mats, frame_ind_1, frame_ind_2, all_points, intrinsic_mat): correspondences = build_correspondences(frames[frame_ind_1], frames[frame_ind_2]) points, ids, _ = triangulate_correspondences(correspondences, view_mats[frame_ind_1], view_mats[frame_ind_2], intrinsic_mat, TRIANGULATION_PARAMS) new_points_number = 0 for point, point_id in zip(points, ids): if point_id not in all_points or all_points[point_id] is None: all_points[point_id] = point new_points_number += 1 print( f'Frames: {frame_ind_1} and {frame_ind_2}, found {new_points_number} new points' )
def add_new_point(corner, corner_to_frames, view_mats, tvecs, corner_storage, cloud, cur_corners_occurencies, intrinsic_mat): frames = [] for frame in corner_to_frames[corner]: if view_mats[frame[0]] is not None: frames.append(frame) if len(frames) < 2: return max_dist = 0 best_frames = [None, None] best_ids = [None, None] for frame_1 in frames: for frame_2 in frames: if frame_1 == frame_2: continue tvec_1 = tvecs[frame_1[0]] tvec_2 = tvecs[frame_2[0]] d = np.linalg.norm(tvec_1 - tvec_2) if max_dist < d: max_dist = d best_frames = [frame_1[0], frame_2[0]] best_ids = [frame_1[1], frame_2[1]] if max_dist > 0.01: ids = np.array([corner]) points1 = corner_storage[best_frames[0]].points[np.array([best_ids[0] ])] points2 = corner_storage[best_frames[1]].points[np.array([best_ids[1] ])] correspondences = Correspondences(ids, points1, points2) points3d, ids, _ = triangulate_correspondences( correspondences, view_mats[best_frames[0]], view_mats[best_frames[1]], intrinsic_mat, parameters=TriangulationParameters(2, 1e-3, 1e-4)) if len(points3d) > 0: cloud[ids[0]] = points3d[0] cur_corners_occurencies.pop(ids[0], None)