def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3( camera_parameters, rgb_sequence[0].shape[0] ) # TODO: implement frame_count = len(corner_storage) view_mats = [pose_to_view_mat3x4(known_view_1[1])] * frame_count corners_0 = corner_storage[0] point_cloud_builder = PointCloudBuilder(corners_0.ids[:1], np.zeros((1, 3))) calc_point_cloud_colors( point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0 ) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3( camera_parameters, rgb_sequence[0].shape[0] ) view_mats, point_ids, points3d, corner_storage = track(intrinsic_mat, corner_storage, known_view_1, known_view_2) point_cloud_builder = PointCloudBuilder(point_ids, points3d) calc_point_cloud_colors( point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0 ) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
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 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 __init__(self, corner_storage: CornerStorage, intrinsic_mat: np.ndarray, parameters: TriangulationParameters, known_view_1: Tuple[int, Pose], known_view_2: Tuple[int, Pose]): self._corner_storage = corner_storage self._intrinsic_mat = intrinsic_mat self._triangulation_parameters = parameters self._length = len(corner_storage) self._builder = PointCloudBuilder() self._track = [None] * self._length self._track[known_view_1[0]] = pose_to_view_mat3x4(known_view_1[1]) self._track[known_view_2[0]] = pose_to_view_mat3x4(known_view_2[1]) self._update_cloud(known_view_1[0], known_view_2[0])
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 track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3( camera_parameters, rgb_sequence[0].shape[0] ) view_mats, point_cloud_builder = [eye3x4()] * len(corner_storage), PointCloudBuilder() try: known_view_1, known_view_2 = _find_pos_pair(intrinsic_mat, corner_storage) tracker = CameraTracker(intrinsic_mat, corner_storage, known_view_1, known_view_2) view_mats, point_cloud_builder = tracker.track() except TrackingError as error: print(error) print('Poses and point cloud are not calculated') calc_point_cloud_colors( point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0 ) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def __init__(self, intrinsic_mat, corner_storage, known_view_1, known_view_2): self.intrinsic_mat = intrinsic_mat self.corner_storage = corner_storage frame_num_1 = known_view_1[0] frame_num_2 = known_view_2[0] self.pc_builder = PointCloudBuilder() view_matr_1 = pose_to_view_mat3x4(known_view_1[1]) view_matr_2 = pose_to_view_mat3x4(known_view_2[1]) self.tracked_views = [eye3x4()] * len(self.corner_storage) self.tracked_views[frame_num_1] = view_matr_1 self.tracked_views[frame_num_2] = view_matr_2 self.initial_baseline = get_baseline(view_matr_1, view_matr_2) print("initial baseline =", self.initial_baseline) self._add_points_from_frame(frame_num_1, frame_num_2, initial_triangulation=True) self.min_init_frame = min(frame_num_1, frame_num_2) self.max_init_frame = max(frame_num_1, frame_num_2) self.outliers = set()
def __init__(self, camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]], known_view_2: Optional[Tuple[int, Pose]] ): self.rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) self.frame_count = len(self.rgb_sequence) self.camera_parameters = camera_parameters self.corner_storage = without_short_tracks(corner_storage, CORNER_MIN_TRACK_LENGTH) self.intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, self.rgb_sequence[0].shape[0]) self.point_cloud_builder = PointCloudBuilder() self.known_view_mats = {} self.unknown_view_ids = set(range(self.frame_count)) if known_view_1 is None or known_view_2 is None: self.initialize_known_views() else: for (frame_idx, pose) in (known_view_1, known_view_2): self.update_view(frame_idx, pose_to_view_mat3x4(pose))
def track(self): step_num = 1 num_of_defined_poses = np.sum([ track_pos_info is not None for track_pos_info in self.tracked_poses ]) while num_of_defined_poses != self.num_of_frames: unsolved_frames = [ i for i in range(self.num_of_frames) if self.tracked_poses[i] is None ] new_cams_info = [] for frame, cam_info in zip(unsolved_frames, map(self._get_pos, unsolved_frames)): if cam_info is not None: new_cams_info.append((frame, cam_info)) if len(new_cams_info) == 0: print(f'Can not get more camera positions, ' f'{self.num_of_frames - num_of_defined_poses}' f' frames left without defined camera position') self.tracked_poses = [ CameraInfo(view_mat3x4_to_pose(eye3x4()), 0) if pos is None else pos for pos in self.tracked_poses ] break best_frame = None best_new_cam_info = None for frame, cam_info in new_cams_info: if best_new_cam_info is None or best_new_cam_info.inliers < cam_info.inliers: best_new_cam_info = cam_info best_frame = frame print('Added camera position for frame ', best_frame) print('Number of inliers: ', best_new_cam_info.inliers) self.tracked_poses[best_frame] = best_new_cam_info self._retriangulate_3d_points(best_frame) if step_num % 4 == 0: self._update_camera_poses() step_num += 1 num_of_defined_poses = np.sum([ tracked_pos_info is not None for tracked_pos_info in self.tracked_poses ]) print( f'{num_of_defined_poses}/{self.num_of_frames} camera positions found, {len(self.point_cloud)} points in cloud' ) self._update_camera_poses() ids, cloud_points = [], [] for pt_id, clout_pt_info in self.point_cloud.items(): ids.append(pt_id) cloud_points.append(clout_pt_info.pos) return list(map(lambda tracked_pos_info: tracked_pos_info.pos, self.tracked_poses)), \ PointCloudBuilder(np.array(ids), np.array(cloud_points))
def track_and_find_point_cloud(intrinsic_mat, corner_storage, known_view_1, known_view_2): num_frames = len(corner_storage) view_mats = [None] * num_frames point_cloud_builder = PointCloudBuilder() print(f'Frames with known views: {known_view_1[0]} and {known_view_2[0]}') view_mats[known_view_1[0]] = pose_to_view_mat3x4(known_view_1[1]) view_mats[known_view_2[0]] = pose_to_view_mat3x4(known_view_2[1]) triang_params = TriangulationParameters(max_reprojection_error=MAX_REPR_ERR, min_triangulation_angle_deg=MIN_TRIANG_ANGLE_DEG, min_depth=MIN_DEPTH) add_points_to_cloud(point_cloud_builder, corner_storage[known_view_1[0]], corner_storage[known_view_2[0]], view_mats[known_view_1[0]], view_mats[known_view_2[0]], intrinsic_mat, triang_params) while True: was_updated = False for i in range(num_frames): if view_mats[i] is not None: continue print(f"\nCurrent frame: {i}") # find intersection of current point cloud and current frame corners = corner_storage[i] corner_ids = [] points_3d = [] points_2d = [] for id, corner in zip(corners.ids, corners.points): if id not in point_cloud_builder.ids: continue ind_in_builder, _ = np.nonzero(point_cloud_builder.ids == id) corner_ids.append(id) points_3d.append(point_cloud_builder.points[ind_in_builder[0]]) points_2d.append(corner) print(f"Size of intersection of current point cloud and current frame: {len(corner_ids)}") if len(corner_ids) < 5: continue points_3d = np.array(points_3d) points_2d = np.array(points_2d) retval, rvec, tvec, inliers = cv2.solvePnPRansac(points_3d.reshape((-1, 1, 3)), points_2d.reshape((-1, 1, 2)), cameraMatrix=intrinsic_mat, distCoeffs=None, reprojectionError=PNP_REPROJ_ERROR) if not retval: print("Unsuccessful solution of PnP") continue print(f"Position found by {len(inliers)} inliers") view_mat = rodrigues_and_translation_to_view_mat3x4(rvec, tvec) view_mats[i] = view_mat for j in range(num_frames): if i != j and view_mats[j] is not None: points_added = add_points_to_cloud(point_cloud_builder, corner_storage[i], corner_storage[j], view_mats[i], view_mats[j], intrinsic_mat, triang_params) if points_added > 0: was_updated = True print(f"Current size of point cloud: {point_cloud_builder.points.size}") if was_updated is False: break for i in range(num_frames): if view_mats[i] is None: view_mats[i] = view_mats[i - 1] return view_mats, point_cloud_builder
class CameraTracker: def __init__(self, camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]], known_view_2: Optional[Tuple[int, Pose]] ): self.rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) self.frame_count = len(self.rgb_sequence) self.camera_parameters = camera_parameters self.corner_storage = without_short_tracks(corner_storage, CORNER_MIN_TRACK_LENGTH) self.intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, self.rgb_sequence[0].shape[0]) self.point_cloud_builder = PointCloudBuilder() self.known_view_mats = {} self.unknown_view_ids = set(range(self.frame_count)) if known_view_1 is None or known_view_2 is None: self.initialize_known_views() else: for (frame_idx, pose) in (known_view_1, known_view_2): self.update_view(frame_idx, pose_to_view_mat3x4(pose)) def update_view(self, frame_idx: int, view_mat: np.ndarray): self.known_view_mats[frame_idx] = view_mat self.unknown_view_ids.remove(frame_idx) def track_camera(self): last_idx, last_view = next(iter(self.known_view_mats.items())) while self.unknown_view_ids: for (idx, view) in self.known_view_mats.items(): if idx != last_idx: self.enrich_point_cloud(last_view, view, self.corner_storage[last_idx], self.corner_storage[idx], TRIANGULATION_PARAMS) # TODO: multi-threading here restored_views = map(lambda i: (i, self.try_restore_view(i)), self.unknown_view_ids) restored_views = list(filter(lambda val: val[1][0] is not None, restored_views)) if len(restored_views) == 0: raise RuntimeError('No more views can be restored!') new_view_tuple = min(restored_views, key=lambda val: val[1][2]) (new_idx, (new_view, inliers_count, _)) = new_view_tuple self.update_view(new_idx, new_view) print('Restored position at frame {} with {} inliers'.format(new_idx, inliers_count)) 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 try_restore_view(self, frame_idx: int) -> Tuple[Optional[np.ndarray], int, np.float64]: corners = self.corner_storage[frame_idx] _, (corner_ids, cloud_ids) = snp.intersect(corners.ids.flatten(), np.sort(self.point_cloud_builder.ids.flatten()), indices=True) points_3d = self.point_cloud_builder.points[cloud_ids] points_2d = corners.points[corner_ids] result = self.try_solve_pnp(points_3d, points_2d) if result is None: return None, 0, np.inf view, inlier_ids = result inlier_ids = inlier_ids.flatten() reprojection_errors = compute_reprojection_errors(points_3d[inlier_ids], points_2d[inlier_ids], self.intrinsic_mat @ view) return view, len(inlier_ids), reprojection_errors.mean() def try_solve_pnp(self, points_3d: np.ndarray, points_2d: np.ndarray) -> Optional[Tuple[np.ndarray, np.ndarray]]: if len(points_3d) < 6: return None ok, r_vec, t_vec, inliers = cv2.solvePnPRansac( objectPoints=points_3d, imagePoints=points_2d, cameraMatrix=self.intrinsic_mat, distCoeffs=None, confidence=PNP_RANSAC_CONFIDENCE, reprojectionError=PNP_RANSAC_INLIERS_MAX_REPROJECTION_ERROR, flags=cv2.SOLVEPNP_EPNP ) if not ok: return None inliers = inliers.flatten() if len(inliers) < 6: return None _, r_vec, t_vec = cv2.solvePnP( points_3d, points_2d, self.intrinsic_mat, distCoeffs=None, rvec=r_vec, tvec=t_vec, useExtrinsicGuess=True, flags=cv2.SOLVEPNP_ITERATIVE ) return rodrigues_and_translation_to_view_mat3x4(r_vec, t_vec), inliers def initialize_known_views(self): median_corners = np.median(np.array([len(self.corner_storage[i].ids) for i in range(self.frame_count)])) def score(result): _, _, _, inliers_count, median_cos = result return (inliers_count / median_corners) ** 2 + (1 - median_cos ** 2) leniency = 1.0 while True: print('Trying to initialize views...') best_view_result = max(self.initial_views_generator(leniency), key=score, default=None) if best_view_result is None: print('Initialization failed, relaxing constraints...') leniency *= 1.05 continue frame_1, frame_2, view, _, _ = best_view_result print('Found initial poses for frames {} and {}'.format(frame_1, frame_2)) self.update_view(frame_1, eye3x4()) self.update_view(frame_2, view) return def initial_views_generator(self, leniency: np.float64): for frame_1 in range(self.frame_count): print('Trying to match frame {} with a subsequent frame...'.format(frame_1)) for frame_2 in range(frame_1 + INIT_MIN_FRAME_DISTANCE, min(frame_1 + 1 + INIT_MAX_FRAME_DISTANCE, self.frame_count)): correspondences = build_correspondences(self.corner_storage[frame_1], self.corner_storage[frame_2]) result = self.try_restore_relative_pose( correspondences, INIT_MAX_LINE_DISTANCE * leniency, INIT_HOMOGRAPHY_MAX_REPROJECTION_ERROR / leniency, INIT_MAX_HOMOGRAPHY_INLIER_RATIO ) if result is not None: yield frame_1, frame_2, *result 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
class CameraTracker: def __init__(self, corner_storage: CornerStorage, intrinsic_mat: np.ndarray, parameters: TriangulationParameters, known_view_1: Tuple[int, Pose], known_view_2: Tuple[int, Pose]): self._corner_storage = corner_storage self._intrinsic_mat = intrinsic_mat self._triangulation_parameters = parameters self._length = len(corner_storage) self._builder = PointCloudBuilder() self._track = [None] * self._length self._track[known_view_1[0]] = pose_to_view_mat3x4(known_view_1[1]) self._track[known_view_2[0]] = pose_to_view_mat3x4(known_view_2[1]) self._update_cloud(known_view_1[0], known_view_2[0]) 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 track(self): for cur_index in range(self._length): print(f'Processing frame {cur_index}/{self._length}') corners = self._corner_storage[cur_index] ids = [] object_points = [] image_points = [] for id, point in zip(corners.ids, corners.points): indices_x, _ = np.where(self._builder.ids == id) if len(indices_x) == 0: continue ids.append(id) object_points.append(self._builder.points[indices_x[0]]) image_points.append(point) if len(object_points) < 5: continue retval, rvec, tvec, inliers = cv2.solvePnPRansac( np.array(object_points), np.array(image_points), self._intrinsic_mat, None, flags=cv2.SOLVEPNP_EPNP) if not retval: continue view_mat = rodrigues_and_translation_to_view_mat3x4(rvec, tvec) print(f'Position based on {len(inliers)} inliers') self._track[cur_index] = view_mat updatesCount = sum([self._update_cloud(cur_index, next_index) for next_index in range(cur_index) \ if self._track[next_index] is not None]) print( f'Points updated:{updatesCount} Current cloud size:{len(self._builder.ids)}' ) for i in range(len(self._track)): self._track[i] = self._track[i] if self._track[ i] is not None else self._track[i - 1] return np.array(self._track), self._builder
def track_and_calc_colors( camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None ) -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) if known_view_1 is None or known_view_2 is None: known_view_1, known_view_2 = calculate_known_views( intrinsic_mat, corner_storage) print('Calculated known views: {} and {}'.format( known_view_1[0], known_view_2[0])) random.seed(239) view_mats = np.full((len(rgb_sequence), ), None) view_mats[known_view_1[0]] = pose_to_view_mat3x4(known_view_1[1]) view_mats[known_view_2[0]] = pose_to_view_mat3x4(known_view_2[1]) point_cloud_builder = PointCloudBuilder() def triangulate_and_add_points(corners1, corners2, view_mat1, view_mat2): points3d, ids, median_cos = triangulate_correspondences( build_correspondences(corners1, corners2), view_mat1, view_mat2, intrinsic_mat, triang_params) point_cloud_builder.add_points(ids, points3d) triangulate_and_add_points(corner_storage[known_view_1[0]], corner_storage[known_view_2[0]], view_mats[known_view_1[0]], view_mats[known_view_2[0]]) inliers_points3d, inliers_points2d = None, None def find_errs(rtvecs): rvecs, tvecs = rtvecs[:3], rtvecs[3:] rmat, _ = cv2.Rodrigues(np.expand_dims(rvecs, axis=1)) rmat = np.concatenate((rmat, np.expand_dims(tvecs, axis=1)), axis=1) return (project_points(inliers_points3d, intrinsic_mat @ rmat) - inliers_points2d).flatten() while True: random_ids = list(range(len(view_mats))) random.shuffle(random_ids) found_new_view_mat = False for i in random_ids: if view_mats[i] is not None: continue common, (ids1, ids2) = snp.intersect(point_cloud_builder.ids.flatten(), corner_storage[i].ids.flatten(), indices=True) if len(common) <= 10: continue points3d = point_cloud_builder.points[ids1] points2d = corner_storage[i].points[ids2] retval, rvecs, tvecs, inliers = cv2.solvePnPRansac( points3d, points2d, intrinsic_mat, iterationsCount=108, reprojectionError=triang_params.max_reprojection_error, distCoeffs=None, confidence=0.999, flags=cv2.SOLVEPNP_EPNP) if retval: # M-оценки inliers_points3d = points3d[inliers.flatten()] inliers_points2d = points2d[inliers.flatten()] rtvecs = least_squares(find_errs, x0=np.concatenate( (rvecs, tvecs)).flatten(), loss='huber', method='trf').x rvecs = rtvecs[:3].reshape((-1, 1)) tvecs = rtvecs[3:].reshape((-1, 1)) if not retval: continue print( 'Iteration {}/{}, processing {}th frame: {} inliers, {} points in point cloud' .format( len([v for v in view_mats if v is not None]) - 1, len(rgb_sequence) - 2, i, len(inliers), len(point_cloud_builder.points))) view_mats[i] = rodrigues_and_translation_to_view_mat3x4( rvecs, tvecs) found_new_view_mat = True # Add new points inliers = np.array(inliers).astype(int).flatten() inlier_corners = FrameCorners( *[c[inliers] for c in corner_storage[i]]) for j in range(len(view_mats)): if view_mats[j] is not None: triangulate_and_add_points(corner_storage[j], inlier_corners, view_mats[j], view_mats[i]) sys.stdout.flush() if not found_new_view_mat: break for i in range(0, len(view_mats)): if view_mats[i] is None: print('Я сдох: не все кадры обработаны :(') exit(1) if len(view_mats) < 100: # Иначе долго работает view_mats = bundle_adjustment(view_mats, point_cloud_builder, intrinsic_mat, corner_storage) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) i_1 = known_view_1[0] i_2 = known_view_2[0] print('Known frames are', i_1, 'and', i_2) global INLIERS_MIN_SIZE, DELTA, MIN_SIZE, TRIANG_PARAMS view_mat3x4_1 = pose_to_view_mat3x4(known_view_1[1]) view_mat3x4_2 = pose_to_view_mat3x4(known_view_2[1]) correspondences = build_correspondences(corner_storage[i_1], corner_storage[i_2]) print(len(correspondences.ids)) points3d, corr_ids, median_cos = triangulate_correspondences( correspondences, view_mat3x4_1, view_mat3x4_2, intrinsic_mat, TRIANG_PARAMS) view_mats, point_cloud_builder = [view_mat3x4_1 for _ in corner_storage ], PointCloudBuilder( corr_ids.astype(np.int64), points3d) err_indexes = set(range(len(corner_storage))) err_indexes.remove(i_1) err_indexes.remove(i_2) res_code, rodrig, inliers_1, cloud_points = get_ransac( point_cloud_builder, corner_storage[i_1], intrinsic_mat) view_mats[i_1] = rodrig res_code, rodrig, inliers_2, cloud_points = get_ransac( point_cloud_builder, corner_storage[i_2], intrinsic_mat) view_mats[i_2] = rodrig INLIERS_MIN_SIZE = min(len(inliers_1), len(inliers_2)) - 20 * max( len(corner_storage) // 100, 1) descr_template = 'Point cloud calc - {} inliers, {} points found, cloud size is {}' tqdm_iterator = tqdm(range(len(corner_storage) - 2), total=len(corner_storage), desc=descr_template.format('?', '?', '?')) params = [ corner_storage, view_mats, point_cloud_builder, intrinsic_mat, err_indexes, descr_template, tqdm_iterator ] DELTA = 5 * max(len(corner_storage) // 100, 1) MIN_SIZE = 20 * max(len(corner_storage) // 100, 1) TRIANG_PARAMS = TriangulationParameters(2, 1e-2, 1e-2) for i in range(i_1 + 1, i_2): update(i, *params) print('Points between handled') DELTA = 20 * max(len(corner_storage) // 100, 1) MIN_SIZE = 20 * max(len(corner_storage) // 100, 1) TRIANG_PARAMS = TriangulationParameters(4, 1e-2, 1e-2) for i in range(i_1, -1, -1): if i in err_indexes: update(i, *params) for i in range(i_2, len(corner_storage)): if i in err_indexes: update(i, *params) for i in range(i_1, -1, -1): if i in err_indexes: view_mats[i] = view_mats[i + 1] for i in range(i_2, len(corner_storage)): if i in err_indexes: view_mats[i] = view_mats[i - 1] calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def run_bundle_adjustment(intrinsic_mat: np.ndarray, list_of_corners: List[FrameCorners], max_inlier_reprojection_error: float, view_mats: List[np.ndarray], pc_builder: PointCloudBuilder) -> List[np.ndarray]: n_frames = len(view_mats) inliers = get_inliers(intrinsic_mat, list_of_corners, max_inlier_reprojection_error, view_mats, pc_builder) rs = [cv2.Rodrigues(i[:, :3])[0].flatten() for i in view_mats] ts = [i[:, 3] for i in view_mats] point_id_to_cloud = -np.ones(pc_builder.ids.max() + 1, dtype=np.int32) point_id_to_cloud[pc_builder.ids.flatten()] = np.arange(len( pc_builder.ids)) re = calc_reprojection_error(inliers, pc_builder, point_id_to_cloud, list_of_corners, intrinsic_mat, view_mats) print('Reprojection error before bundle adjustment:', re) for start in range(0, n_frames, FRAMES_STEP): end = min(start + FRAMES_STEP, n_frames) cameras_params_dim = (end - start) * 6 matches = get_matches(inliers[start:end], len(list_of_corners), start) relevant_point_ids = np.array( list( sorted( {point_id for point_id, frame_id, point_index in matches}))) point_id_to_cur = {p: i for i, p in enumerate(relevant_point_ids)} _, (_, cloud_indices) = snp.intersect(relevant_point_ids, pc_builder.ids.flatten(), indices=True) relevant_points = pc_builder.points[cloud_indices] alpha = ALPHA_INITIAL us_mean_prev = -float('inf') for i in range(ITERATIONS): us = get_residuals(pc_builder, point_id_to_cloud, list_of_corners, intrinsic_mat, matches, view_mats) jacobian = calc_jacobian(matches, cameras_params_dim, rs, ts, pc_builder, list_of_corners, point_id_to_cur, point_id_to_cloud, intrinsic_mat, start) jtj = jacobian.T @ jacobian us_mean = us.mean() alpha = alpha / ALPHA_DEC_STEP if us_mean < us_mean_prev else alpha * ALPHA_INC_STEP us_mean_prev = us_mean alpha_multiplier = 1 + alpha jtj[np.arange(jtj.shape[0]), np.arange(jtj.shape[0])] *= alpha_multiplier u = jtj[:cameras_params_dim, :cameras_params_dim] v = jtj[cameras_params_dim:, cameras_params_dim:] w = jtj[:cameras_params_dim, cameras_params_dim:] wt = jtj[cameras_params_dim:, :cameras_params_dim] v_inv = block_inv(v) g = jacobian.T @ us b = w @ v_inv @ g[cameras_params_dim:] - g[:cameras_params_dim] a = (u - w @ v_inv @ wt).toarray() delta_c = np.linalg.solve(a, b) delta_x = v_inv @ (-g[cameras_params_dim:] - wt @ delta_c) for k, j in enumerate(range(start, end)): rs[j] += delta_c[k * 6:k * 6 + 3] ts[j] += delta_c[k * 6 + 3:k * 6 + 6] view_mats[j] = rodrigues_and_translation_to_view_mat3x4( rs[j], ts[j].reshape(3, 1)) relevant_points += delta_x.reshape((-1, 3)) pc_builder.update_points(relevant_point_ids, relevant_points) re = calc_reprojection_error(inliers, pc_builder, point_id_to_cloud, list_of_corners, intrinsic_mat, view_mats) print('Reprojection error after bundle adjustment:', re) return view_mats
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) view_mats = np.zeros((len(corner_storage), 3, 4), dtype=np.float64) processed_frames = np.zeros(len(corner_storage), dtype=np.bool) points3d = np.zeros((corner_storage.max_corner_id() + 1, 3), dtype=np.float64) added_points = np.zeros(corner_storage.max_corner_id() + 1, dtype=np.bool) print('Trying to extract 3d points from known frames...') view_mats[known_view_1[0]] = pose_to_view_mat3x4(known_view_1[1]) view_mats[known_view_2[0]] = pose_to_view_mat3x4(known_view_2[1]) processed_frames[known_view_1[0]] = processed_frames[ known_view_2[0]] = True extracted_points = extract_points(corner_storage[known_view_1[0]], corner_storage[known_view_2[0]], view_mats[known_view_1[0]], view_mats[known_view_2[0]], intrinsic_mat) if not extracted_points: print( 'Extracting 3d points from known frames failed: ' 'either there are no common points, or triangulation angle between frames is too small.\n' 'Try to choose another initial frames.', file=sys.stderr) exit(0) print('Succeeded! Trying to build point cloud...') added_points[extracted_points[0]] = True points3d[extracted_points[0]] = extracted_points[1] was_updated = True while was_updated: was_updated = False unprocessed_frames = np.arange(len(corner_storage), dtype=np.int32)[~processed_frames] for frame in unprocessed_frames: points3d_ids = np.arange(corner_storage.max_corner_id() + 1, dtype=np.int32)[added_points] common, (indices1, indices2) = snp.intersect( points3d_ids, corner_storage[frame].ids.flatten(), indices=True) if len(common) <= 5: continue try: print(f'Processing frame {frame}: ', end='') retval, rvec, tvec, inliers = cv2.solvePnPRansac( objectPoints=points3d[common], imagePoints=corner_storage[frame].points[indices2], cameraMatrix=intrinsic_mat, distCoeffs=None) except: retval = False if not retval: print(f'failed to solve PnP RANSAC, trying another frame') continue print(f'succeeded, found {len(inliers)} inliers') was_updated = True view_mats[frame] = rodrigues_and_translation_to_view_mat3x4( rvec, tvec) for processed_frame in np.arange(len(corner_storage), dtype=np.int32)[processed_frames]: extracted_points = extract_points( corner_storage[frame], corner_storage[processed_frame], view_mats[frame], view_mats[processed_frame], intrinsic_mat, points3d_ids) if extracted_points: added_points[extracted_points[0]] = True points3d[extracted_points[0]] = extracted_points[1] processed_frames[frame] = True print(f'Current point cloud contains {sum(added_points)} points') for _ in range(2): for i in range(1, len(corner_storage)): if not processed_frames[i]: processed_frames[i] = True view_mats[i] = view_mats[i - 1] processed_frames = processed_frames[::-1] view_mats = view_mats[::-1] print( f'Finished building point cloud, now it contains {sum(added_points)} points' ) point_cloud_builder = PointCloudBuilder(ids=np.arange( corner_storage.max_corner_id() + 1, dtype=np.int32)[added_points], points=points3d[added_points]) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) triang_params = TriangulationParameters( max_reprojection_error=MAX_PROJ_ERROR, min_triangulation_angle_deg=MIN_TRIANG_ANG, min_depth=MIN_DEPTH) correspondences = build_correspondences(corner_storage[known_view_1[0]], corner_storage[known_view_2[0]]) view_mat_1 = pose_to_view_mat3x4(known_view_1[1]) view_mat_2 = pose_to_view_mat3x4(known_view_2[1]) points, ids, median_cos = triangulate_correspondences( correspondences, view_mat_1, view_mat_2, intrinsic_mat, triang_params) if len(points) < 10: print("\rPlease, try other frames") exit(0) view_mats = [None] * len(rgb_sequence) view_mats[known_view_1[0]] = view_mat_1 view_mats[known_view_2[0]] = view_mat_2 point_cloud_builder = PointCloudBuilder(ids, points) was_update = True while was_update: was_update = False for i, (frame, corners) in enumerate(zip(rgb_sequence, corner_storage)): if view_mats[i] is not None: continue _, (idx_1, idx_2) = snp.intersect(point_cloud_builder.ids.flatten(), corners.ids.flatten(), indices=True) try: retval, rvec, tvec, inliers = cv2.solvePnPRansac( point_cloud_builder.points[idx_1], corners.points[idx_2], intrinsic_mat, distCoeffs=None) inliers = np.array(inliers, dtype=int) if len(inliers) > 0: view_mats[i] = rodrigues_and_translation_to_view_mat3x4( rvec, tvec) was_update = True print( f"\rFrame {i} out of {len(rgb_sequence)}, inliners: {len(inliers)}", end="") except Exception: print(f"\rFrame {i} out of {len(rgb_sequence)}, inliners: {0}", end="") if view_mats[i] is None: continue cur_corner = filter_frame_corners(corner_storage[i], inliers.flatten()) for j in range(len(rgb_sequence)): if view_mats[j] is None: continue correspondences = build_correspondences( corner_storage[j], cur_corner) if len(correspondences.ids) == 0: continue points, ids, median_cos = triangulate_correspondences( correspondences, view_mats[j], view_mats[i], intrinsic_mat, triang_params) point_cloud_builder.add_points(ids, points) print() first_mat = next((mat for mat in view_mats if mat is not None), None) if first_mat is None: print("\rFail") exit(0) view_mats[0] = first_mat for i in range(1, len(view_mats)): if view_mats[i] is None: view_mats[i] = view_mats[i - 1] view_mats = np.array(view_mats) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
class CameraTracker: 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 track(self): while len(self.view_mats) < self.frame_count: print('Processing {0}/{1} frame'.format(len(self.view_mats) + 1, self.frame_count)) for i, v_mat in self.view_mats.items(): corners_1 = self.corner_storage[i] corners_2 = self.corner_storage[self.last_added_idx] corrs = build_correspondences(corners_1, corners_2) try: p, ids, _ = triangulate_correspondences(corrs, v_mat, self.view_mats[self.last_added_idx], self.intrinsic_mat, self.triangulation_parameters) new_corners = [] new_ids = [] for j, pt in zip(ids, p): if j in self.last_inliers or j not in self.point_frames: new_ids += [j] new_corners += [pt] for i_ in ids: if i_ not in self.point_frames: self.point_frames[i_] = [i] if self.last_added_idx not in self.point_frames[i_]: self.point_frames[i_] += [self.last_added_idx] if len(new_ids) > 0: self.point_cloud_builder.add_points(np.array(new_ids), np.array(new_corners)) except: continue print('Points cloud size: {0}'.format(len(self.point_cloud_builder.points))) view_mat, best_idx, inliers = self.solve_pnp_ransac() self.view_mats[best_idx] = view_mat[:3] self.used_inliers[best_idx] = inliers self.view_nones.remove(best_idx) self.last_added_idx = best_idx if self.step % (self.frame_count // 20) == 0 and self.step > 0: self.retriangulate_points() if self.step % (self.frame_count // 10) == 0 and self.step > 0: self.update_tracks() # k = list(self.view_mats.keys()) # k = k[self.step - 10:self.step] # self.bundle_adjustment(k) self.step += 1 def update_tracks(self): for i in self.view_mats.keys(): v_mat, inliers = self.solve_pnp_ransac_one(i) if self.used_inliers[i] <= len(inliers): self.view_mats[i] = v_mat[:3] self.used_inliers[i] = len(inliers) def solve_pnp_ransac(self): best = None best_ans = [] nones = list(self.view_nones) for i in nones: try: v_mat, inliers = self.solve_pnp_ransac_one(i) except: continue if len(inliers) > len(best_ans): best = v_mat, i best_ans = inliers print('Used {} inliers'.format(len(best_ans))) self.last_inliers = best_ans return best[0], best[1], len(best_ans) def solve_pnp_ransac_one(self, frame_id): ids_3d = self.point_cloud_builder.ids ids_2d = self.corner_storage[frame_id].ids ids_intersection, _ = snp.intersect(ids_3d.flatten(), ids_2d.flatten(), indices=True) idx3d = [i for i, j in enumerate(ids_3d) if j in ids_intersection] idx2d = [i for i, j in enumerate(ids_2d) if j in ids_intersection] points3d = self.point_cloud_builder.points[idx3d] points2d = self.corner_storage[frame_id].points[idx2d] if len(points3d) < 6: return None, [] _, _, _, inliers = cv2.solvePnPRansac( objectPoints=points3d, imagePoints=points2d, cameraMatrix=self.intrinsic_mat, distCoeffs=np.array([]), iterationsCount=250, flags=cv2.SOLVEPNP_EPNP ) if len(points3d[inliers]) < 6: return None, [] _, r_vec, t_vec = cv2.solvePnP( objectPoints=points3d[inliers], imagePoints=points2d[inliers], cameraMatrix=self.intrinsic_mat, distCoeffs=np.array([]), flags=cv2.SOLVEPNP_ITERATIVE ) view_mat = rodrigues_and_translation_to_view_mat3x4(r_vec, t_vec) return view_mat, inliers def retriangulate_points(self): to_retriangulate = [i for i in self.point_cloud_builder.ids if ((i[0] not in self.last_retriangulated) or (len(self.point_frames[i[0]]) - self.last_retriangulated[i[0]]) > 5) and i[0] in self.point_frames] np.random.shuffle(to_retriangulate) to_retriangulate = to_retriangulate[:300] self.retriangulate_points_arr(to_retriangulate) def retriangulate_points_arr(self, to_retriangulate): retriangulated_coords = [] retriangulated_ids = [] for idx in to_retriangulate: new_coords, cnt = self.retriangulate_point(idx) if new_coords is None: continue if idx[0] not in self.used_inliers_point or self.used_inliers_point[idx[0]] < cnt: self.used_inliers_point[idx[0]] = cnt retriangulated_ids += [idx[0]] retriangulated_coords += [new_coords[0]] self.last_retriangulated[idx[0]] = len(self.point_frames[idx[0]]) retriangulated_ids = np.array(retriangulated_ids) retriangulated_coords = np.array(retriangulated_coords) if len(retriangulated_coords) == 0: return print('Retriangulated {} points'.format(len(retriangulated_ids))) self.point_cloud_builder.update_points(retriangulated_ids, retriangulated_coords) 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 find_best_start_poses(self): best_i = 0 best_j = 0 best_j_pose = None best_pose_points = 0 step = self.frame_count // 20 for i in range(self.frame_count): for j in range(i + 5, self.frame_count, step): pose, pose_points = self.get_pose(i, j) if pose_points > best_pose_points: best_i = i best_j = j best_j_pose = pose best_pose_points = pose_points return best_i, eye3x4(), best_j, pose_to_view_mat3x4(best_j_pose) def get_pose(self, frame_1, frame_2): p1 = self.corner_storage[frame_1] p2 = self.corner_storage[frame_2] corresp = build_correspondences(p1, p2) if len(corresp.ids) < 6: return None, 0 E, mask_essential = cv2.findEssentialMat(corresp[1], corresp[2], self.intrinsic_mat, method=cv2.RANSAC, threshold=1.0) _, mask_homography = cv2.findHomography(corresp[1], corresp[2], method=cv2.RANSAC) if mask_essential is None or mask_homography is None: return None, 0 essential_inliers = mask_essential.flatten().sum() homography_inliers = mask_homography.flatten().sum() if homography_inliers / essential_inliers > 0.5: return None, 0 corresp = _remove_correspondences_with_ids(corresp, np.argwhere(mask_essential == 0)) R1, R2, t = cv2.decomposeEssentialMat(E) possible_poses = [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_points = 0 best_pose = None for pose in possible_poses: p, ids, med = triangulate_correspondences(corresp, eye3x4(), pose_to_view_mat3x4(pose), self.intrinsic_mat, self.triangulation_parameters) if len(p) > best_pose_points: best_pose_points = len(p) best_pose = pose return best_pose, best_pose_points def bundle_adjustment(self, frames): def view_mat3x4_to_rodrigues_and_translation(vmat): r_mat = vmat[:, :3] _t_vec = vmat[:, 3:] _r_vec, _ = cv2.Rodrigues(r_mat) return _r_vec, _t_vec def bundle_adjustment_sparsity(n_cameras, n_points): m = camera_indices.size * 2 n = n_cameras * 6 + n_points * 3 A_ = lil_matrix((m, n), dtype=int) i = np.arange(len(camera_indices)) for s in range(6): A_[2 * i, camera_indices * 6 + s] = 1 A_[2 * i + 1, camera_indices * 6 + s] = 1 for s in range(3): A_[2 * i, n_cameras * 6 + points_indices * 3 + s] = 1 A_[2 * i + 1, n_cameras * 6 + points_indices * 3 + s] = 1 return A_ def rotate(points_, rot_vecs): theta = np.linalg.norm(rot_vecs, axis=1)[:, np.newaxis] with np.errstate(invalid='ignore'): v = rot_vecs / theta v = np.nan_to_num(v) dot = np.sum(points_ * v, axis=1)[:, np.newaxis] cos_theta = np.cos(theta) sin_theta = np.sin(theta) return cos_theta * points_ + sin_theta * np.cross(v, points_) + dot * (1 - cos_theta) * v def project(points_, camera_params): points_proj = rotate(points_, camera_params[:, :3]) points_proj += camera_params[:, 3:6] points_proj = np.dot(self.intrinsic_mat, points_proj.T) points_proj /= points_proj[[2]] return points_proj[:2].T def fun(params_, n_cameras, n_points): camera_params = params_[:n_cameras * 6].reshape((n_cameras, 6)) points_3d = params_[n_cameras * 6:].reshape((n_points, 3)) points_proj = project(points_3d[points_indices], camera_params[camera_indices]) return (points_proj - corners_points).ravel() def residuals(params_): return fun(params_, len(frames), len(intersected)) def apply_res(res_): for i in range(len(intersected)): p_ = res_[6 * len(frames) + i * 3:6 * len(frames) + i * 3 + 3] _idx = np.argwhere(self.point_cloud_builder.ids == intersected[i])[0][0] _p = self.point_cloud_builder.points[_idx] check_point(_p, p_, intersected[i]) for i in range(len(frames)): r_vec_ = np.array([res_[i * 6], res_[i * 6 + 1], res_[i * 6 + 2]]) t_vec_ = np.array([[res_[i * 6 + 3]], [res_[i * 6 + 4]], [res_[i * 6 + 5]]]) v_mat = rodrigues_and_translation_to_view_mat3x4(r_vec_, t_vec_) self.view_mats[frames[i]] = v_mat def check_point(p_before, p_after, idx): projected_before = [] projected_after = [] point2d = [] for frame in frames: projected_before += [project_points(np.array([p_before]), self.intrinsic_mat @ self.view_mats[frame])] projected_after += [project_points(np.array([p_after]), self.intrinsic_mat @ self.view_mats[frame])] idx_ = np.argwhere(self.corner_storage[frame].ids == idx)[0][0] p = self.corner_storage[frame].points[idx_] point2d += [p] projected_before = np.array(projected_before) projected_after = np.array(projected_after) point2d = np.array(point2d) err_before = np.linalg.norm(point2d - projected_before) err_before_amount = np.sum(err_before < 1.0) err_after = np.linalg.norm(point2d - projected_after) err_after_amount = np.sum(err_after < 1.0) if err_before_amount < err_after_amount: self.point_cloud_builder.points[idx] = p_after intersected = self.point_cloud_builder.ids.flatten() corners_idxs = {} for frame in frames: crnrs = self.corner_storage[frame].ids crnrs = crnrs.flatten() intersected, _ = snp.intersect( intersected, crnrs, indices=True) intersected = np.array(list(sorted(np.random.choice(intersected, min(500, len(intersected)), replace=False)))) for frame in frames: corner_idx, _ = snp.intersect( intersected, self.corner_storage[frame].ids.flatten(), indices=True) corners_idxs[frame] = corner_idx points = [] for idx in intersected: idx_ = np.argwhere(self.point_cloud_builder.ids == idx)[0][0] p = self.point_cloud_builder.points[idx_] points += [p[0], p[1], p[2]] corners_points = [] for frame in frames: for idx in corners_idxs[frame]: idx_ = np.argwhere(self.corner_storage[frame].ids == idx)[0][0] p = self.corner_storage[frame].points[idx_] corners_points += [[p[0], p[1]]] camera_parameters = [] for frame in frames: r_vec, t_vec = view_mat3x4_to_rodrigues_and_translation(self.view_mats[frame]) camera_parameters += [r_vec[0], r_vec[1], r_vec[2], t_vec[0], t_vec[1], t_vec[2]] params = camera_parameters + points camera_indices = [] for i in range(len(frames)): camera_indices += [i] * len(intersected) camera_indices = np.array(camera_indices) points_indices = np.tile(np.arange(len(intersected)), len(frames)) self.retriangulate_points_arr([np.array([item]) for item in intersected]) A = bundle_adjustment_sparsity(len(frames), len(intersected)) res = least_squares(residuals, params, jac_sparsity=A, verbose=2, x_scale='jac', ftol=1e-4, xtol=1e-4, method='trf') apply_res(res.x)
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) frames_num = len(rgb_sequence) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) if known_view_1 is None or known_view_2 is None: known_view_1, known_view_2 = select_frames(rgb_sequence, corner_storage, intrinsic_mat) if known_view_2[0] == -1: print("Failed to find good starting frames") exit(0) correspondences = build_correspondences(corner_storage[known_view_1[0]], corner_storage[known_view_2[0]]) view_mat1 = pose_to_view_mat3x4(known_view_1[1]) view_mat2 = pose_to_view_mat3x4(known_view_2[1]) points, ids, _ = triangulate_correspondences(correspondences, view_mat1, view_mat2, intrinsic_mat, TRIANG_PARAMS) if len(ids) < 8: print("Bad frames: too few correspondences!") exit(0) view_mats, point_cloud_builder = [None] * frames_num, PointCloudBuilder( ids, points) view_mats[known_view_1[0]] = view_mat1 view_mats[known_view_2[0]] = view_mat2 updated = True pass_num = 0 while updated: updated = False pass_num += 1 for i in range(frames_num): if view_mats[i] is not None: continue corners = corner_storage[i] _, ind1, ind2 = np.intersect1d(point_cloud_builder.ids, corners.ids.flatten(), return_indices=True) try: _, rvec, tvec, inliers = cv2.solvePnPRansac( point_cloud_builder.points[ind1], corners.points[ind2], intrinsic_mat, distCoeffs=None) if inliers is None: print(f"Pass {pass_num}, frame {i}. No inliers!") continue print( f"Pass {pass_num}, frame {i}. Number of inliers == {len(inliers)}" ) view_mats[i] = rodrigues_and_translation_to_view_mat3x4( rvec, tvec) except Exception: continue updated = True cloud_changed = add_points(point_cloud_builder, corner_storage, i, view_mats, intrinsic_mat) if cloud_changed: print(f"Size of point cloud == {len(point_cloud_builder.ids)}") first_not_none = next(item for item in view_mats if item is not None) if view_mats[0] is None: view_mats[0] = first_not_none for i in range(frames_num): if view_mats[i] is None: view_mats[i] = view_mats[i - 1] calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3( camera_parameters, rgb_sequence[0].shape[0] ) # TODO: implement frame_count = len(corner_storage) view_mats = [None] * frame_count view_1_id, view_1 = known_view_1[0], pose_to_view_mat3x4(known_view_1[1]) view_2_id, view_2 = known_view_2[0], pose_to_view_mat3x4(known_view_2[1]) view_mats[view_1_id] = view_1 view_mats[view_2_id] = view_2 correspondences = build_correspondences(corner_storage[view_1_id], corner_storage[view_2_id], None) triangulation_parameters = TriangulationParameters(1, 5, 0) points, ids, _ = triangulate_correspondences(correspondences, view_1, view_2, intrinsic_mat, TriangulationParameters(0.1, 0.1, 0)) #corners_0 = corner_storage[0] point_cloud_builder = PointCloudBuilder(ids, points) unknown_frames = frame_count - 2 known_frames = {view_1_id, view_2_id} while unknown_frames > 0: for i in range(frame_count): if view_mats[i] is None: corners = corner_storage[i] interesting_ids, in_corners, in_cloud = np.intersect1d(corners.ids.flatten(), point_cloud_builder.ids.flatten(), return_indices=True) points_2d = corners.points[in_corners] points_3d = point_cloud_builder.points[in_cloud] if len(ids) < 3: continue method = cv2.SOLVEPNP_EPNP if len(ids) == 3: method = cv2.SOLVEPNP_P3P retval, rvec, tvec, inliers = cv2.solvePnPRansac(points_3d, points_2d, intrinsic_mat, None, flags=method) if not retval: continue retval, rvec, tvec = cv2.solvePnP(points_3d[inliers], points_2d[inliers], intrinsic_mat, None, rvec=rvec, tvec=tvec, useExtrinsicGuess=True) if not retval: continue view_mats[i] = rodrigues_and_translation_to_view_mat3x4(rvec, tvec) unknown_frames -= 1 known_frames.add(i) outliers = np.delete(interesting_ids, inliers) for frame in known_frames: correspondences = build_correspondences(corners, corner_storage[frame], outliers) points_3d, corner_ids, _ = triangulate_correspondences( correspondences, view_mats[i], view_mats[frame], intrinsic_mat, triangulation_parameters) point_cloud_builder.add_points(corner_ids, points_3d) calc_point_cloud_colors( point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0 ) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
class CameraTracker: def __init__(self, intrinsic_mat, corner_storage, known_view_1, known_view_2): self.intrinsic_mat = intrinsic_mat self.corner_storage = corner_storage frame_num_1 = known_view_1[0] frame_num_2 = known_view_2[0] self.pc_builder = PointCloudBuilder() view_matr_1 = pose_to_view_mat3x4(known_view_1[1]) view_matr_2 = pose_to_view_mat3x4(known_view_2[1]) self.tracked_views = [eye3x4()] * len(self.corner_storage) self.tracked_views[frame_num_1] = view_matr_1 self.tracked_views[frame_num_2] = view_matr_2 self.initial_baseline = get_baseline(view_matr_1, view_matr_2) print("initial baseline =", self.initial_baseline) self._add_points_from_frame(frame_num_1, frame_num_2, initial_triangulation=True) self.min_init_frame = min(frame_num_1, frame_num_2) self.max_init_frame = max(frame_num_1, frame_num_2) self.outliers = set() def _get_pos(self, frame_number) -> np.ndarray: corners = self.corner_storage[frame_number] _, pts_ids, corners_ids = np.intersect1d(self.pc_builder.ids, corners.ids, assume_unique=True, return_indices=True) common_pts_3d = self.pc_builder.points[pts_ids] common_ids = self.pc_builder.ids[pts_ids] common_corners = corners.points[corners_ids] inlier_mask = np.zeros_like(common_pts_3d, dtype=np.bool) inlier_counter = 0 for common_id, mask_elem in zip(common_ids[:, 0], inlier_mask): if common_id not in self.outliers: mask_elem[:] = True inlier_counter += 1 if inlier_counter > 7: common_pts_3d = common_pts_3d[inlier_mask].reshape(-1, 3) common_ids = common_ids[inlier_mask[:, :1]].reshape(-1, 1) common_corners = common_corners[inlier_mask[:, :2]].reshape(-1, 2) max_reproj_error = 3.0 if len(common_pts_3d) < 4: raise TrackingError('Not enough points to solve RANSAC on frame ' + str(frame_number)) _, rot_vec, tr_vec, inliers = cv2.solvePnPRansac(common_pts_3d, common_corners, self.intrinsic_mat, None, reprojectionError=max_reproj_error, flags=cv2.SOLVEPNP_EPNP) extrinsic_mat = rodrigues_and_translation_to_view_mat3x4(rot_vec, tr_vec) proj_matr = self.intrinsic_mat @ extrinsic_mat if inliers is None: raise TrackingError('Failed to solve PnP on frame' + str(frame_number)) while len(inliers) < 8 and max_reproj_error < 50: max_reproj_error *= 1.2 inliers = calc_inlier_indices(common_pts_3d, common_corners, proj_matr, max_reproj_error) inlier_pts = common_pts_3d[inliers] inlier_corners = common_corners[inliers] outlier_ids = np.setdiff1d(common_ids, common_ids[inliers], assume_unique=True) self.outliers.update(outlier_ids) if len(inliers) < 4: inlier_pts = common_pts_3d inlier_corners = common_corners print('Found position on', len(inlier_corners), 'inliers') _, rot_vec, tr_vec, inliers = cv2.solvePnPRansac(inlier_pts, inlier_corners, self.intrinsic_mat, None, rot_vec, tr_vec, useExtrinsicGuess=True) return rodrigues_and_translation_to_view_mat3x4(rot_vec, tr_vec) 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 track(self): curr_frame = self.min_init_frame + 1 seq_size = len(self.corner_storage) for _ in range(2, seq_size): if curr_frame == self.max_init_frame: curr_frame += 1 if curr_frame >= seq_size: curr_frame = self.min_init_frame - 1 self.outliers = set() print('Frame', curr_frame) try: self.tracked_views[curr_frame] = self._get_pos(curr_frame) except TrackingError as error: print(error) print('Stopping tracking') break prev_curr_diff = 5 num_pairs = 0 num_added = 0 while num_pairs < 5: prev_frame = \ curr_frame - prev_curr_diff if curr_frame > self.min_init_frame else curr_frame + prev_curr_diff prev_curr_diff += 1 if prev_frame < seq_size and (self.min_init_frame <= prev_frame or curr_frame < self.min_init_frame): if check_baseline(self.tracked_views[prev_frame], self.tracked_views[curr_frame], self.initial_baseline * 0.15): num_added += self._add_points_from_frame(prev_frame, curr_frame) num_pairs += 1 else: break print('Added', num_added, 'points') print('Point cloud size = ', len(self.pc_builder.ids)) if curr_frame > self.min_init_frame: curr_frame += 1 else: curr_frame -= 1 return self.tracked_views, self.pc_builder
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) if known_view_1 is None or known_view_2 is None: known_view_1, known_view_2 = get_matrix_poses(corner_storage, intrinsic_mat) video_size = len(rgb_sequence) frame_trackers = [ FrameTrack(i, corner_storage[i]) for i in range(video_size) ] known_tracker_creator = lambda x: FrameTrack(x[0], corner_storage[x[0]], pose_to_view_mat3x4(x[1])) frame_trackers[known_view_1[0]] = known_tracker_creator(known_view_1) frame_trackers[known_view_2[0]] = known_tracker_creator(known_view_2) init_params = triang_params for angle in range(90, 0, -2): params = TriangulationParameters( max_reprojection_error=MAX_REPROJECTION_ERROR, min_triangulation_angle_deg=angle, min_depth=0.001) _, points = triangulate_trackers(frame_trackers[known_view_1[0]], frame_trackers[known_view_2[0]], intrinsic_mat, params) if len(points) > 100: print(f"Chosen init angle: {angle}") init_params = params break ids, points = triangulate_trackers(frame_trackers[known_view_1[0]], frame_trackers[known_view_2[0]], intrinsic_mat, init_params) point_cloud_builder = PointCloudBuilder(ids, points) if len(points) < MIN_STARTING_POINTS: print( f"Not enough starting points ({len(points)}), please choose another initial frames pair" f"\n0, 20 is a good pair for short fox, ") return [], PointCloudBuilder() frame_trackers[known_view_1[0]].update_reproj_error( point_cloud_builder, intrinsic_mat) frame_trackers[known_view_2[0]].update_reproj_error( point_cloud_builder, intrinsic_mat) point_cloud_builder = track(ITERATIONS, frame_trackers, point_cloud_builder, intrinsic_mat) view_mats = [x.mtx for x in frame_trackers] for i in range(1, len(view_mats)): if view_mats[i] is None: view_mats[i] = view_mats[i - 1] for i in range(len(view_mats) - 2, -1, -1): if view_mats[i] is None: view_mats[i] = view_mats[i + 1] calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) view_mats_tmp = [None for _ in range(len(corner_storage))] all_points = {} not_none_mats = [known_view_1[0], known_view_2[0]] none_mats = set( [ind for ind in range(len(view_mats_tmp)) if ind not in not_none_mats]) view_mats_tmp[known_view_1[0]] = pose_to_view_mat3x4(known_view_1[1]) view_mats_tmp[known_view_2[0]] = pose_to_view_mat3x4(known_view_2[1]) add_new_points(corner_storage, view_mats_tmp, known_view_1[0], known_view_2[0], all_points, intrinsic_mat) old_len = 2 for _ in range(len(view_mats_tmp)): for ind in none_mats: corner_ids = list(corner_storage[ind].ids.reshape(-1)) intersection_inds = [ point_ind for point_ind in corner_ids if point_ind in all_points and all_points[point_ind] is not None ] # There is an assert, so points number has to be at least 5 if len(intersection_inds) < 5: continue # print(ind, len(corner_storage), len(corner_storage[ind].points), np.max(intersection_inds)) corner_points = { i: p for i, p in zip(corner_ids, corner_storage[ind].points) } intersection_points_c = np.array( [corner_points[i] for i in intersection_inds]) intersection_points_f = np.array( [all_points[i] for i in intersection_inds]) # print(intersection_points_f) # print(intersection_points_f.shape, intersection_points_c.shape, intrinsic_mat) retval, rvec, tvec, inliers = cv2.solvePnPRansac( intersection_points_f, intersection_points_c, intrinsic_mat, None) if not retval: continue print(f'Processing frame: {ind}') newly_none_number = 0 for i in intersection_inds: if i not in inliers: newly_none_number += 1 all_points[i] = None print( f'{newly_none_number} points filled as None, len of inliers: {inliers.shape[0]}' ) print( f'Number of not points: {len([p for p in all_points.keys() if all_points[p] is not None])}' ) view_mats_tmp[ind] = rodrigues_and_translation_to_view_mat3x4( rvec, tvec) for not_none_ind in not_none_mats: add_new_points(corner_storage, view_mats_tmp, ind, not_none_ind, all_points, intrinsic_mat) not_none_mats.append(ind) none_mats.remove(ind) break if len(not_none_mats) == old_len: break old_len = len(not_none_mats) view_mats = [None for _ in range(len(corner_storage))] for view_mat_ind in not_none_mats: view_mats[view_mat_ind] = view_mats_tmp[view_mat_ind] last_ind = 0 for i in range(len(view_mats)): if view_mats[i] is None: view_mats[i] = view_mats[last_ind] else: last_ind = i all_points = {k: v for k, v in all_points.items() if v is not None} point_cloud_builder = PointCloudBuilder( np.array(list(all_points.keys()), dtype=np.int), np.array(list(all_points.values()))) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors( camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None ) -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) if known_view_1 is None or known_view_2 is None: known_view_1, known_view_2 = initial_camera_views( corner_storage, intrinsic_mat) print(known_view_1[1]) print(known_view_2[1]) print(known_view_1[0], known_view_2[0]) print(intrinsic_mat) points_3d, points_ids = calc_starting_points(intrinsic_mat, corner_storage, known_view_1, known_view_2) #initial_points_3d, initial_points_ids = points_3d.copy(), points_ids.copy() n_points = corner_storage.max_corner_id() + 1 res_points_3d = np.full((n_points, 3), None) #res_points_3d[points_ids] = points_3d view_mats = [np.full((3, 4), None) for _ in range(len(corner_storage))] view_mats[known_view_1[0]], view_mats[ known_view_2[0]] = pose_to_view_mat3x4( known_view_1[1]), pose_to_view_mat3x4(known_view_2[1]) print(f'len(corner_storage):{len(corner_storage)}') id1 = known_view_1[0] for n_frame, corners in enumerate(corner_storage[id1 + 1:], id1 + 1): common_obj, common_img, common_ids = get_common_points( corners, points_ids, points_3d) mat, points_3d, points_ids, reprojection_error = build_view_mat( common_obj, common_img, common_ids, intrinsic_mat) points_ids = np.array(points_ids) view_mats[n_frame] = mat is_null = np.array([(x[0] is None) for x in res_points_3d[points_ids]]) #res_points_3d[points_ids[is_null]] = points_3d[is_null] n_inliers = len(points_3d) points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat, corner_storage, view_mats, points_3d, points_ids) print( f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. " f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}" ) for n_frame, corners in list(enumerate(corner_storage))[::-1]: common_obj, common_img, common_ids = get_common_points( corners, points_ids, points_3d) mat, points_3d, points_ids, reprojection_error = build_view_mat( common_obj, common_img, common_ids, intrinsic_mat) view_mats[n_frame] = mat is_null = np.array([(x[0] is None) for x in res_points_3d[points_ids]]) #res_points_3d[points_ids[is_null]] = points_3d[is_null] n_inliers = len(points_3d) points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat, corner_storage, view_mats, points_3d, points_ids) print( f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. " f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}" ) # Approximating n_iter = 1 for iter in range(n_iter): for n_frame, corners in enumerate(corner_storage): common_obj, common_img, common_ids = get_common_points( corners, points_ids, points_3d) mat, points_3d, points_ids, reprojection_error = build_view_mat( common_obj, common_img, common_ids, intrinsic_mat) view_mats[n_frame] = mat if iter == n_iter - 1: res_points_3d[points_ids] = points_3d n_inliers = len(points_3d) points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat, corner_storage, view_mats, points_3d, points_ids) print( f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. " f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}" ) for n_frame, corners in list(enumerate(corner_storage))[::-1]: common_obj, common_img, common_ids = get_common_points( corners, points_ids, points_3d) mat, points_3d, points_ids, reprojection_error = build_view_mat( common_obj, common_img, common_ids, intrinsic_mat) view_mats[n_frame] = mat if iter == n_iter - 1: res_points_3d[points_ids] = points_3d n_inliers = len(points_3d) points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat, corner_storage, view_mats, points_3d, points_ids) print( f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. " f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}" ) res_points_ids = np.array( [i for i, x in enumerate(res_points_3d) if x[0] is not None]) res_points_3d = np.array(res_points_3d[res_points_ids], dtype=float) point_cloud_builder = PointCloudBuilder(ids=res_points_ids, points=res_points_3d) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) if known_view_1 is None or known_view_2 is None: known_view_1, known_view_2 = calc_known_views(corner_storage, intrinsic_mat) # TODO: implement num_frames_retr = 10 triang_params = TriangulationParameters(8.0, 1.0, .1) num_frames = len(corner_storage) frame_1 = known_view_1[0] frame_2 = known_view_2[0] print(f'{frame_1}, {frame_2}') tvecs = [None] * num_frames tvecs[frame_1] = known_view_1[1].t_vec tvecs[frame_2] = known_view_2[1].t_vec view_mat_1 = pose_to_view_mat3x4(known_view_1[1]) view_mat_2 = pose_to_view_mat3x4(known_view_2[1]) view_mats = [None] * num_frames view_mats[frame_1] = view_mat_1 view_mats[frame_2] = view_mat_2 cloud = {} corrs = build_correspondences(corner_storage[frame_1], corner_storage[frame_2]) points3d, ids, _ = triangulate_correspondences(corrs, view_mat_1, view_mat_2, intrinsic_mat, triang_params) for point3d, id in zip(points3d, ids): cloud[id] = point3d current_corners_occurences = {} corner_to_frames = {} for i, corners in enumerate(corner_storage): for j, id in enumerate(corners.ids.flatten()): current_corners_occurences[id] = 0 if id not in corner_to_frames.keys(): corner_to_frames[id] = [[i, j]] else: corner_to_frames[id].append([i, j]) while len(untracked_frames(view_mats)) > 0: untr_frames = untracked_frames(view_mats) max_num_inl = -1 best_frame = -1 best_rvec = None best_tvec = None for frame in untr_frames: rvec, tvec, num_inl = calc_camera_pose(frame, corner_storage, cloud, intrinsic_mat) if rvec is not None: if num_inl > max_num_inl: best_frame = frame max_num_inl = num_inl best_rvec = rvec best_tvec = tvec if max_num_inl == -1: break corners_to_add = [] for id in corner_storage[best_frame].ids.flatten(): if id in current_corners_occurences.keys(): current_corners_occurences[id] += 1 if current_corners_occurences[id] >= 2: corners_to_add.append(id) for corner in corners_to_add: add_new_point(corner, corner_to_frames, view_mats, tvecs, corner_storage, cloud, current_corners_occurences, intrinsic_mat) print(f"Frame: {best_frame}, Inliers: {max_num_inl}") print(f"Cloud size: {len(cloud)}") view_mats[best_frame] = rodrigues_and_translation_to_view_mat3x4( best_rvec, best_tvec) tvecs[best_frame] = best_tvec last_mat = None for frame in range(num_frames): if view_mats[frame] is None: view_mats[frame] = last_mat else: last_mat = view_mats[frame] ids = [] points = [] for id in cloud.keys(): ids.append(id) points.append(cloud[id]) point_cloud_builder = PointCloudBuilder(np.array(ids, dtype=np.int), np.array(points)) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: # функция для проверки что матрица поворота дейтсвительно матрица поворота def check_if_mat_is_rot_mat(mat): mat = mat[:, :3] det = np.linalg.det(mat) # определитель должен быть = 1 diff1 = abs(1 - det) x = mat @ mat.T # обратная матрица должна получаться транспонированием diff2 = abs(np.sum(np.eye(3) - x)) eps = 0.001 return diff1 < eps and diff2 < eps MAX_REPROJECTION_ERROR = 1.6 MIN_DIST_TRIANGULATE = 0.01 rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) frame_count = len(corner_storage) t_vecs = [None] * frame_count view_mats = [None] * frame_count # позиции камеры на всех кадрах # для каждого уголка находим все кадры на котором он есть и его порядковый номер в каждом из них cur_corners_occurencies = {} frames_of_corner = {} for i, corners in enumerate(corner_storage): for id_in_list, j in enumerate(corners.ids.flatten()): cur_corners_occurencies[j] = 0 if j not in frames_of_corner.keys(): frames_of_corner[j] = [[i, id_in_list]] else: frames_of_corner[j].append([i, id_in_list]) #по двум кадрам находим общие точки и восстанавливаем их позиции в 3D 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) #константы для инициализации BASELINE_THRESHOLD = 0.9 REPROJECTION_ERROR_THRESHOLD = 1.4 MIN_3D_POINTS = 10 frame_steps = [9, 30, 40] #нахождение относительного движения между двумя данными кадрами def get_first_cam_pose(frame_0, frame_1): corrs = build_correspondences(corner_storage[frame_0], corner_storage[frame_1]) e_mat, _ = cv2.findEssentialMat(corrs.points_1, corrs.points_2, intrinsic_mat, cv2.RANSAC, 0.999, 2.0) def essential_mat_to_camera_view_matrix(e_mat): U, S, VH = np.linalg.svd(e_mat, full_matrices=True) W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]], dtype=np.float) u3 = U[:, 2] possible_R = [U @ W @ VH, U @ W.T @ VH] possible_t = [u3, -u3] best_R = None best_t = None max_positive_z = 0 for R in possible_R: for t in possible_t: view_mat0 = np.hstack((np.eye(3), np.zeros((3, 1)))) view_mat1 = np.hstack((R, t.reshape((3, 1)))) view_mats[frame_0] = view_mat0 view_mats[frame_1] = view_mat1 points3d, ids, median_cos = triangulate(frame_0, frame_1) cur_positive_z = 0 for pt in points3d: pt = np.append(pt, np.zeros((1))).reshape((4, 1)) pt_transformed0 = (view_mat0 @ pt).flatten() z0 = pt_transformed0[2] pt_transformed1 = (view_mat1 @ pt).flatten() z1 = pt_transformed1[2] cur_positive_z += (z0 > 0.1) + (z1 > 0.1) if cur_positive_z > max_positive_z: max_positive_z = cur_positive_z best_R = R best_t = t return best_R, best_t R, t = essential_mat_to_camera_view_matrix(e_mat) baseline = np.linalg.norm(t) view_mat0 = np.hstack((np.eye(3), np.zeros((3, 1)))) view_mat1 = np.hstack((R, t.reshape((3, 1)))) view_mats[frame_0] = view_mat0 view_mats[frame_1] = view_mat1 points3d, ids, median_cos = triangulate(frame_0, frame_1) points0 = [] points1 = [] for i in ids: for j in frames_of_corner[i]: if j[0] == frame_0: points0.append(j[1]) if j[0] == frame_1: points1.append(j[1]) points0 = np.array(points0) points1 = np.array(points1) sum_error = compute_reprojection_errors( points3d, corner_storage[frame_0].points[points0], intrinsic_mat @ view_mat0) + compute_reprojection_errors( points3d, corner_storage[frame_1].points[points1], intrinsic_mat @ view_mat1) reprojection_error = np.mean(sum_error) points3d_count = len(points3d) view_mats[frame_0] = None view_mats[frame_1] = None #проверяем насколько хороша данная пара кадров, и заодно проверяем что матрица R - реально поворот if baseline < BASELINE_THRESHOLD or reprojection_error > REPROJECTION_ERROR_THRESHOLD or points3d_count < MIN_3D_POINTS or not check_if_mat_is_rot_mat( np.hstack((R, t.reshape((3, 1))))): return False, baseline, reprojection_error, points3d_count, None, None else: return True, baseline, reprojection_error, points3d_count, R, t #делаем перебор пар кадров для инициализации INF = 1e9 best_frame_step = 5 best = [-INF, INF, -INF] ind = None for frame_step in frame_steps: for i in range(frame_count // 2): if i + frame_step < frame_count * 0.85: ok, baseline, reproection_error, points3d_count, Rx, tx = get_first_cam_pose( i, i + frame_step) if ok: best = [baseline, reproection_error, points3d_count] ind = i best_frame_step = frame_step if frame_count > 100: break # крашаемся, если не удалось инициализироваться ни по какой паре кадров if ind is None: raise NotImplementedError() #инициализируемся по лучшей найденной паре frame_0 = ind frame_1 = ind + best_frame_step ok, baseline, reproection_error, points3d_count, R, t = get_first_cam_pose( frame_0, frame_1) print('INITIALIZATION RESULTS:') print('FRAMES: frame_0={}, frame_1={}, total_frames={}'.format( frame_0, frame_1, frame_count)) print(baseline, reproection_error, points3d_count, R, t) view_mat0 = np.hstack((np.eye(3), np.zeros((3, 1)))) view_mat1 = np.hstack((R, t.reshape((3, 1)))) view_mats[frame_0] = view_mat0 view_mats[frame_1] = view_mat1 t_vecs[frame_0] = np.zeros((3), dtype=np.float64) t_vecs[frame_1] = t # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! # зачем хранить инфу о пендинг точках вперемешку с остальными точками облака??? # 'id': coords [., ., .] 3d coordinates, rays [(frame, coords)] - for new points addition points_cloud = {} # инициализируем облако точек по 2 положениям points3d, ids, median_cos = triangulate(frame_0, frame_1) for id, point3d in zip(ids, points3d): points_cloud[id] = point3d def find_camera_pose(frame_id): # сначала находим инлаеры ранзаком # потом на инлаерах делаем пнп с итеративной мин кв corners = corner_storage[frame_id] points3d = [] points2d = [] for id, point2d in zip(corners.ids.flatten(), corners.points): if id in points_cloud.keys(): points3d.append(points_cloud[id]) points2d.append(point2d) # чтобы епнп работал points3d = np.array(points3d, dtype=np.float64) points2d = np.array(points2d, dtype=np.float64) if len(points3d) < 4: return None # выбираем эталонную 4 точек, строим по ней R и t # затем смотрим, на какой выборке эталонных точек больше всего других точек # спроецируются корректно success, R, t, inliers = cv2.solvePnPRansac( objectPoints=points3d, imagePoints=points2d, cameraMatrix=intrinsic_mat, distCoeffs=None, flags=cv2.SOLVEPNP_EPNP, confidence=0.9995, reprojectionError=MAX_REPROJECTION_ERROR) if not success: return None inliers = np.asarray(inliers, dtype=np.int32).flatten() points3d = np.array(points3d) points2d = np.array(points2d) points3d = points3d[inliers] points2d = points2d[inliers] retval, R, t = cv2.solvePnP(objectPoints=points3d, imagePoints=points2d, cameraMatrix=intrinsic_mat, distCoeffs=None, flags=cv2.SOLVEPNP_ITERATIVE, useExtrinsicGuess=True, rvec=R, tvec=t) return R, t, len(inliers) # можно ускорить def frames_without_pose(): frames = [] for ind, mat in enumerate(view_mats): if mat is None: frames.append(ind) return frames 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) # в этой функции мы еще бонусом возвращаем те точки, которые встречаются хотя бы 2 раза # а также мы добавляем оккуранси тем точкам которые уже в облаке)))))))))))))))))))))))))))) def add_corners_occurencies(frame_id): two_and_greater = [] for id in corner_storage[frame_id].ids.flatten(): if id in cur_corners_occurencies.keys(): cur_corners_occurencies[id] += 1 if cur_corners_occurencies[id] >= 2: two_and_greater.append(id) return two_and_greater # очень эффективно)))))))))) while len(frames_without_pose()) > 0: # пока есть не найденные положения камеры будем искать лучшее для восстановления # т.е. такое, где больше всего инлаеров после применения пнп ранзака wanted_frames = frames_without_pose() # inliers_amount = [] не понял зачем этот лист max_col = -1 best_frame_id = -1 best_R = None best_t = None for frame_id in wanted_frames: result = find_camera_pose(frame_id) if result is not None: R, t, col = result if col > max_col: max_col = col best_frame_id = frame_id best_R = R best_t = t if max_col == -1: # больше позиции камер не находятся break view_mats[best_frame_id] = rodrigues_and_translation_to_view_mat3x4( best_R, best_t) t_vecs[best_frame_id] = best_t try_add = add_corners_occurencies(best_frame_id) for corner in try_add: try_add_new_point_to_cloud(corner) print( 'Now we add camera pose on {}-th frame, camera pose was calculated by {} inliers' .format(best_frame_id, max_col)) ttl_poses_calculated = np.sum([mat is not None for mat in view_mats]) percent = "{:.0f}".format(ttl_poses_calculated / frame_count * 100.0) print('{}% poses calculated'.format(percent)) print('{} points in 3D points cloud'.format(len(points_cloud))) last_view_mat = None for i in range(frame_count): if view_mats[i] is None: view_mats[i] = last_view_mat else: last_view_mat = view_mats[i] #check that all matrices are rotation matrices for ind, i in enumerate(view_mats): if not check_if_mat_is_rot_mat(i): print( 'In frame {}/{} we have matrix wich is not rotation matrix:('. format(ind + 1, frame_count)) raise NotImplementedError() ids = [] points = [] for id in points_cloud.keys(): ids.append(id) points.append(points_cloud[id]) point_cloud_builder = PointCloudBuilder(np.array(ids, dtype=np.int32), np.array(points)) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) frame_count = len(corner_storage) view_mats = [None] * frame_count if known_view_1 is None or known_view_2 is None: known_view_1, known_view_2 = init_first_camera_positions( intrinsic_mat, corner_storage) id_1, view_mat_1 = known_view_1 id_2, view_mat_2 = known_view_2 view_mats[id_1], view_mats[id_2] = view_mat_1, view_mat_2 else: id_1, pose_1 = known_view_1 id_2, pose_2 = known_view_2 view_mats[id_1] = pose_to_view_mat3x4(pose_1) view_mats[id_2] = pose_to_view_mat3x4(pose_2) params = TriangulationParameters(max_reprojection_error=2, min_triangulation_angle_deg=1, min_depth=0.5) correspondences = build_correspondences(corner_storage[id_1], corner_storage[id_2]) points, ids, _ = triangulate_correspondences(correspondences, view_mats[id_1], view_mats[id_2], intrinsic_mat, params) point_cloud_builder = PointCloudBuilder(ids.reshape(-1), points) while True: update = False for i in range(frame_count): if view_mats[i] is not None: continue corners = corner_storage[i] ids, ids1, ids2 = np.intersect1d(corners.ids, point_cloud_builder.ids, return_indices=True) points_2d, points_3d = corners.points[ ids1], point_cloud_builder.points[ids2] if len(ids) < 6: continue retval, rvec, tvec, inliers = solvePnPRansac(points_3d, points_2d, intrinsic_mat, distCoeffs=None) if not retval: continue retval, rvec, tvec = solvePnP(points_3d[inliers], points_2d[inliers], intrinsic_mat, distCoeffs=None, rvec=rvec, tvec=tvec, useExtrinsicGuess=True) if not retval: continue view_mats[i] = rodrigues_and_translation_to_view_mat3x4(rvec, tvec) update = True outliers = np.delete(ids, inliers) for j in range(i): if view_mats[j] is None: continue correspondences = build_correspondences(corners, corner_storage[j], ids_to_remove=outliers) points, ids, _ = triangulate_correspondences( correspondences, view_mats[i], view_mats[j], intrinsic_mat, params) point_cloud_builder.add_points(ids.reshape(-1), points) click.echo("Process frame %d/%d. %d 3D points found. inliners=%d" % (i + 1, frame_count, len( point_cloud_builder.points), len(inliers))) if not update: break calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud
def track_and_calc_colors(camera_parameters: CameraParameters, corner_storage: CornerStorage, frame_sequence_path: str, known_view_1: Optional[Tuple[int, Pose]] = None, known_view_2: Optional[Tuple[int, Pose]] = None) \ -> Tuple[List[Pose], PointCloud]: if known_view_1 is None or known_view_2 is None: raise NotImplementedError() rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path) intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters, rgb_sequence[0].shape[0]) frames_cnt = len(rgb_sequence) unused = np.array([ i for i in range(0, frames_cnt) if i not in [known_view_1[0], known_view_2[0]] ]) used = np.array([known_view_1[0], known_view_2[0]]) used_pose = [known_view_1[1], known_view_2[1]] points, ids, cos = triangulate(known_view_1[0], known_view_2[0], known_view_1[1], known_view_2[1], corner_storage, intrinsic_mat, None) point_cloud_builder = PointCloudBuilder(ids, points) # print("Add frames", *known_ids) while unused.shape[0] > 0: added = [] for i in range(len(unused)): pose_i, ids_to_remove = camera_pose(unused[i], corner_storage, point_cloud_builder, intrinsic_mat) if pose_i is None: continue for j in range(len(used)): points, ids, cos = triangulate(unused[i], used[j], pose_i, used_pose[j], corner_storage, intrinsic_mat, ids_to_remove) point_cloud_builder.add_points(ids, points) used = np.append(used, [unused[i]]) used_pose.append(pose_i) added.append(unused[i]) # print("Frame", unused[i], "done!!") if len(added) == 0: break unused = np.setdiff1d(unused, added) used_pose = recalculate_poses(used, point_cloud_builder, corner_storage, intrinsic_mat) view_mats = [None for i in range(frames_cnt)] for i in range(len(used)): view_mats[used[i]] = pose_to_view_mat3x4(used_pose[i]) calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats, intrinsic_mat, corner_storage, 5.0) point_cloud = point_cloud_builder.build_point_cloud() poses = list(map(view_mat3x4_to_pose, view_mats)) return poses, point_cloud