def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] points = cv2.goodFeaturesToTrack(image_0, maxCorners=5000, qualityLevel=0.001, minDistance=10, blockSize=10).squeeze(1) ids = np.array(range(len(points))); sizes = np.array([10] * len(points)) corners = FrameCorners(ids, points, sizes) builder.set_corners_at_frame(0, corners) corners_count = len(points) for frame, image_1 in enumerate(frame_sequence[1:], 1): i0 = cv2.convertScaleAbs(image_0, alpha=255) i1 = cv2.convertScaleAbs(image_1, alpha=255) tracked_points, status, err = cv2.calcOpticalFlowPyrLK(i0, i1, points, None) status = status.squeeze(1) tracked_points = tracked_points[status == 1] tracked_ids = ids[status == 1] new_points = cv2.goodFeaturesToTrack(image_1, maxCorners=5000, qualityLevel=0.001, minDistance=10, blockSize=10).squeeze(1) dist = np.linalg.norm(tracked_points[None, :] - new_points[:, None], axis=2) new_points = new_points[np.min(dist, axis=1) >= 10, :] new_ids = np.array(range(corners_count, corners_count + len(new_points)), dtype=np.int32); corners_count += len(new_points) tracked_points = np.concatenate((tracked_points, new_points)) points = tracked_points[:min(5000, len(tracked_points))] ids = np.append(tracked_ids, new_ids, axis=0)[:len(points)] sizes = np.array([10] * len(points)) corners = FrameCorners(ids, points, sizes) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: frame_sequence = list( map(lambda t: (np.array(t) * 255.0).astype(np.uint8), frame_sequence)) cur_image = frame_sequence[0] initial_points = cv2.goodFeaturesToTrack(cur_image, **feature_params).squeeze(axis=1) ptr = len(initial_points) params = CornersParams(np.arange(ptr), initial_points, np.full(ptr, 10)) builder.set_corners_at_frame(0, FrameCorners(*params.get())) idx = 0 for next_image in frame_sequence[1:]: idx += 1 points_ = params.get_good_tracks_masked(cur_image, next_image, *params.get()) if len(points_) < maxCorners: next_features = cv2.goodFeaturesToTrack(next_image, mask=mask_current_corners( points_, next_image), **feature_params) next_features = next_features.squeeze( axis=1) if next_features is not None else [] for pnt in next_features[:maxCorners - len(points_)]: params.extend(ptr, pnt) ptr += 1 builder.set_corners_at_frame(idx, FrameCorners(*params.get())) cur_image = next_image
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] maxCorners = (image_0.shape[0] * image_0.shape[1]) // 2000 qualityLevel = 0.001 minDistance = 20 blockSize = 9 winSize = (20, 20) corner_points = cv2.goodFeaturesToTrack(image_0, maxCorners, qualityLevel, minDistance, blockSize=blockSize, useHarrisDetector=False).reshape( (-1, 2)) max_id = len(corner_points) ids = np.arange(0, max_id) corners = FrameCorners(ids, corner_points, np.full(corner_points.shape, blockSize)) builder.set_corners_at_frame(0, corners) max_id = ids.max() for frame, image_1 in enumerate(frame_sequence[1:], 1): nextPts, status, _ = cv2.calcOpticalFlowPyrLK( np.asarray(image_0 * 256, np.uint8), np.asarray(image_1 * 256, np.uint8), corner_points.astype(np.float32), None, winSize=winSize) ptid = np.c_[nextPts, ids][status.reshape(-1) == 1] b = np.fromiter( (id_closer_than(ptid, xi, minDistance) >= xi[2] for xi in ptid), bool, count=len(ptid)) ptid = ptid[b] corner_points, ids = np.hsplit(ptid, [2]) ids = ids.reshape(-1).astype(int) new_corner_points = cv2.goodFeaturesToTrack( image_1, maxCorners, qualityLevel, minDistance, blockSize=blockSize, useHarrisDetector=False).reshape((-1, 2)) b = np.fromiter((id_closer_than(ptid, xi, minDistance) == ptid.shape[0] for xi in new_corner_points), bool, count=len(new_corner_points)) new_corner_points = new_corner_points[b][:maxCorners - corner_points.shape[0]] new_ids = np.arange(max_id, max_id + new_corner_points.shape[0]) max_id += new_corner_points.shape[0] corner_points = np.concatenate((corner_points, new_corner_points)) ids = np.concatenate((ids, new_ids)) corners = FrameCorners(ids, corner_points, np.full(corner_points.shape, blockSize)) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] i0 = image_to_uint8(image_0) num_of_pix = frame_sequence.frame_shape[0] * frame_sequence.frame_shape[1] gf_params["minDistance"] = int((num_of_pix / max_corners)**0.5) init_corners = cv2.goodFeaturesToTrack(image_0, maxCorners=max_corners, **gf_params) corners = FrameCorners(np.array(range(len(init_corners))), np.array(init_corners), np.array([block_size] * len(init_corners))) builder.set_corners_at_frame(0, corners) for frame, image_1 in enumerate(frame_sequence[1:], 1): i1 = image_to_uint8(image_1) flow, status, _ = cv2.calcOpticalFlowPyrLK(i0, i1, np.float32(corners.points), None) r_flow, r_status, _ = cv2.calcOpticalFlowPyrLK(i1, i0, flow, None) status = status.squeeze().astype(np.bool) bumped_corners = ((np.float32(corners.points) - r_flow)**2).reshape( -1, 2).max(-1) < 0.25 corners = FrameCorners(corners.ids, flow, corners.sizes) corners = filter_frame_corners(corners, np.logical_and(status, bumped_corners)) if len(corners.points) < max_corners: mask = create_points_mask(i1, corners) diff = max_corners - len(corners.points) new_corners = cv2.goodFeaturesToTrack(image_1, mask=mask, maxCorners=diff, **gf_params) if new_corners is None: new_corners = [] new_id = corners.ids[-1][0] + 1 new_corners = FrameCorners( np.array(range(new_id, new_id + len(new_corners)), dtype=np.int64), np.array(new_corners), np.array([block_size] * len(new_corners))) corners = concat_corners(corners, new_corners) builder.set_corners_at_frame(frame, corners) i0 = i1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: min_distance = 10 corners_cnt = 500 quality_level = 0.01 params_dict = dict(maxCorners=corners_cnt, qualityLevel=quality_level, minDistance=min_distance, useHarrisDetector=False, blockSize=min_distance) prev_frame = frame_sequence[0] prev_frame *= 255 prev_frame = prev_frame.astype(np.uint8) corners = cv2.goodFeaturesToTrack(image=prev_frame, **params_dict) ids = np.arange(len(corners)) sizes = np.full(len(corners), min_distance) frame_corners = FrameCorners(ids, corners, sizes) builder.set_corners_at_frame(0, frame_corners) for frame, cur_frame in enumerate(frame_sequence[1:], 1): cur_frame *= 255 cur_frame = cur_frame.astype(np.uint8) corners, status, _ = cv2.calcOpticalFlowPyrLK(prev_frame, cur_frame, corners, None, winSize=(min_distance, min_distance)) status = status.reshape(-1).astype(np.bool) corners = corners[status] ids = ids[status] sizes = np.full(len(corners), min_distance) if len(corners) < corners_cnt: mask = np.full_like(cur_frame, 255) for x, y in corners.reshape(-1, 2): cv2.circle(mask, (x, y), min_distance, 0, -1) params_dict['maxCorners'] = corners_cnt - len(corners) new_corners = cv2.goodFeaturesToTrack(cur_frame, mask=mask, **params_dict) corners = np.append(corners, new_corners).reshape((-1, 1, 2)) ids = np.arange(len(corners)) sizes = np.full(len(corners), min_distance) frame_corners = FrameCorners(ids, corners, sizes) builder.set_corners_at_frame(frame, frame_corners) prev_frame = cur_frame
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: # params for ShiTomasi corner detection feature_params = dict(maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7) # Parameters for lucas kanade optical flow lk_params = dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) image_0 = frame_sequence[0] p0 = cv2.goodFeaturesToTrack(image_0, mask=None, **feature_params).squeeze(-2) radius = 11 ids = np.arange(len(p0)) next_id = len(p0) corners = FrameCorners(ids, p0, np.full(len(p0), radius)) builder.set_corners_at_frame(0, corners) for frame, image_1 in enumerate(frame_sequence[1:], 1): # calculate optical flow p1, st, err = cv2.calcOpticalFlowPyrLK( (image_0 * 255).astype(np.uint8), (255 * image_1).astype(np.uint8), p0, cv2.OPTFLOW_USE_INITIAL_FLOW, **lk_params) idx = (st == 1).reshape(-1) p0 = p1[idx] ids = ids[idx] if len(p0) < feature_params['maxCorners']: mask = np.full(image_1.shape, 255, dtype=np.uint8) for arr in p0: x, y = arr cv2.circle(mask, (x, y), feature_params['minDistance'], 0, -1) new_centers = cv2.goodFeaturesToTrack(image_1, mask=mask, **feature_params) if new_centers is not None: add_length = min(feature_params['maxCorners'] - len(ids), len(new_centers)) ids = np.concatenate( [ids, np.arange(next_id, next_id + add_length)]) p0 = np.concatenate([p0, new_centers[:add_length].squeeze(-2)]) next_id += add_length corners = FrameCorners(ids, p0, np.full(len(p0), radius)) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def add_new_corners(self, new_corner_points): if new_corner_points is None: return new_corner_points = np.array(new_corner_points, dtype=np.int32).reshape(-1, 2) if self.corners is None: self.corners = FrameCorners( np.array(range(new_corner_points.shape[0])), new_corner_points, np.array([self.circle_size] * new_corner_points.shape[0])) return self.corners.add_corners(new_corner_points, self.circle_size)
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: tracker = Tracker() img_prev = None last_id = 0 for frame, img_curr in enumerate(frame_sequence): if img_prev is None: coords, sizes, ids = tracker.find_corners(img_curr, 0) last_id += len(coords) if img_prev is not None: coords, sizes, ids = tracker.tracking(img_prev, img_curr, corners.points, corners.sizes[:, 0], corners.ids[:, 0]) new_coords, new_sizes, new_ids = tracker.find_corners( img_curr, last_id) last_id += len(new_coords) coords, sizes, ids = tracker.addCorners(img_curr.shape[0], img_curr.shape[1], coords, sizes, ids, new_coords, new_sizes, new_ids) corners = FrameCorners(ids, coords, sizes) corners = tracker.filterCorners(corners, img_curr.shape[0], img_curr.shape[1]) builder.set_corners_at_frame(frame, corners) img_prev = img_curr
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] corner_tracker = CornerTracker(image_0) corners, corners_ids, radiuses = corner_tracker.get_corners() builder.set_corners_at_frame(0, FrameCorners(corners_ids, corners, radiuses)) for frame, image_1 in enumerate(frame_sequence[1:], 1): corner_tracker.process_frame(image_1) corners, corners_ids, radiuses = corner_tracker.get_corners() builder.set_corners_at_frame( frame, FrameCorners(corners_ids, corners, radiuses))
def set_corners(frame, _ids, _corner_points): builder.set_corners_at_frame( frame, FrameCorners(ids=_ids, points=_corner_points, sizes=np.full(len(_ids), feature_params['blockSize'])))
def move_to_frame(self, frame: int): corners = FrameCorners( np.array(self.corners_indices).astype(np.int32), np.array(self.corners_pos).astype(np.float32), np.array(self.corners_radius).astype(np.float32)) self.frames[frame] = corners self.builder.set_corners_at_frame(frame, corners)
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] corners = FrameCorners(np.array([0]), np.array([[0, 0]]), np.array([55])) builder.set_corners_at_frame(0, corners) for frame, image_1 in enumerate(frame_sequence[1:], 1): builder.set_corners_at_frame(frame, corners) image_0 = image_1
def get_corners(self, corner_size): sizes_list = [ np.full(points.shape[0], corner_size * 2**i) for i, points in enumerate(self.points_list) ] return FrameCorners(np.concatenate(self.ids_list), np.concatenate(self.points_list), np.concatenate(sizes_list))
def get_corners(self, new_img, old_img = None, old_corners=None): if old_img is None: points, sizes = self.find_new_corners(new_img) ids = np.arange(len(points)) points = points.reshape((-1, 2)) self.total_corners = len(points) return FrameCorners(ids, points, sizes) else: ids = old_corners.ids points = old_corners.points sizes = old_corners.sizes nextPts, status, err = cv2.calcOpticalFlowPyrLK(to_uint8_image(old_img), to_uint8_image(new_img), prevPts=points, nextPts=None, winSize=(self.CIRCLE_SIZE, self.CIRCLE_SIZE), maxLevel=self.MAX_LEVEL_LK, criteria=self.TERM_CRITERIA) status = status.squeeze() found = np.where(status == 1) ids = ids[found] points = nextPts[found] sizes = sizes[found] mask = self.get_circles_mask(new_img.shape, points) if len(points) < self.MAX_CORNERS: new_points, new_sizes = self.find_new_corners(new_img, self.MAX_CORNERS - len(points), mask, self.QUALITY_LEVEL) if new_points is not None: new_ids = np.arange(self.total_corners, self.total_corners + len(new_points)) new_ids = new_ids.reshape((-1, 1)) new_points = new_points.reshape((-1, 2)) new_sizes = new_sizes.reshape((-1, 1)) self.total_corners += len(new_points) ids = np.concatenate([ids, new_ids]) points = np.concatenate([points, new_points]) sizes = np.concatenate([sizes, new_sizes]) points = points.reshape((-1, 2)) return FrameCorners(ids, points, sizes)
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: lk_params = dict(winSize=(15, 15), maxLevel=6, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.02)) # Create corners on two levels image_0 = frame_sequence[0] ids, points, sizes = calc_corners(image_0) corners = FrameCorners( ids, points, sizes ) for frame, image_1 in enumerate(frame_sequence[1:], 1): next_points, st, err = cv2.calcOpticalFlowPyrLK(np.uint8(image_0 * 255), np.uint8(image_1 * 255), corners.points, None, **lk_params) # backtrack # corners_0_lvl1r, st, err = cv2.calcOpticalFlowPyrLK(np.uint8(image_1 * 255), np.uint8(image_0 * 255), corners_1_lvl1, None, **lk_params) # error = abs(corners_0_lvl1 - corners_0_lvl1r).reshape(-1, 2).max(-1) # corners_1_lvl1 = corners_1_lvl1[error < 1] tracked_corners = filter_frame_corners(corners, st.reshape(-1) == 1) builder.set_corners_at_frame(frame - 1, tracked_corners) prev_points = FrameCorners( tracked_corners.ids, next_points[st.reshape(-1) == 1], tracked_corners.sizes ) ids, points, sizes = calc_corners(image_1, old_corners=prev_points) ids = np.append(prev_points.ids.reshape(-1), ids, axis=0) points = np.append(prev_points.points.reshape(-1, 2), points.reshape(-1, 2), axis=0) sizes = np.append(prev_points.sizes.reshape(-1), sizes, axis=0) corners = FrameCorners( ids, points, sizes ) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder, config: {}) -> None: image_0 = frame_sequence[0] points, sizes = get_new_points(image_0, config) corners = FrameCorners( np.arange(len(points)), points, sizes ) n_tracks = len(points) builder.set_corners_at_frame(0, corners) for frame, image_1 in enumerate(frame_sequence[1:], 1): next_points, status = calc_corners_flow(image_0, image_1, corners.points, config) # print(len(corners_), len(status)) corners = FrameCorners(corners.ids, next_points, corners.sizes) corners = filter_frame_corners(corners, np.squeeze(status == 1)) new_points, new_sizes = get_new_points(image_1, config, corners.points) # print(maxCorners - len(corners.points), len(new_points)) if new_points is not None: new_ids = \ np.arange(n_tracks, n_tracks + len(new_points)) n_tracks += len(new_points) corners = FrameCorners( np.concatenate([corners.ids, new_ids.reshape(-1, 1)], axis=0), np.concatenate([corners.points, new_points], axis=0), np.concatenate([corners.sizes, new_sizes], axis=0), ) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] p0 = cv2.goodFeaturesToTrack(image_0, mask=None, **feature_params).squeeze(axis=1) p0_ids = np.array(list(range(len(p0)))) corners = CornerUpdater(p0_ids, p0, np.array([P_SIZE] * len(p0))) last_id = len(p0) builder.set_corners_at_frame(0, FrameCorners(*corners.get())) for frame, image_1 in enumerate(frame_sequence[1:], 1): p1, st, err = cv2.calcOpticalFlowPyrLK(to_int_gray(image_0), to_int_gray(image_1), corners.points, None, **lk_params) corners = CornerUpdater(corners.ids[st.flatten() == 1], p1[st.flatten() == 1], corners.sizes[st.flatten() == 1]) update_with = cv2.goodFeaturesToTrack(image_1, mask=None, **feature_params).squeeze(axis=1) last_id = corners.update(update_with, last_id) builder.set_corners_at_frame(frame, FrameCorners(*corners.get())) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] image_0 = image_0 * 255 image_0 = image_0.astype(np.uint8) min_dim = min(image_0.shape[0], image_0.shape[1]) min_feat_dist = int(min_dim * 0.015) max_feats = _get_max_num_pts(image_0.shape, min_feat_dist, 0.25) cv_corners = cv2.goodFeaturesToTrack(image_0, max_feats, 0.01, min_feat_dist, blockSize=11, useHarrisDetector=False) max_id = 0 ids = np.arange(max_id, max_id + len(cv_corners), dtype=np.int64) max_id = max(max_id, ids[-1]) corners = FrameCorners( ids, cv_corners, np.ones(shape=(len(cv_corners),)) * 5 ) builder.set_corners_at_frame(0, corners) for frame, image_1 in enumerate(frame_sequence[1:], 1): image_1 = image_1 * 255 image_1 = image_1.astype(np.uint8) next_corners = np.zeros(shape=np.shape(cv_corners), dtype=cv_corners.dtype) _, status, err = cv2.calcOpticalFlowPyrLK(image_0, image_1, cv_corners, next_corners, winSize=(15, 15)) tracked_cv_corners, tracked_ids = filter_tracked_corners(ids, next_corners, status, err) max_id = max(max_id, ids[-1]) new_cv_corners = cv2.goodFeaturesToTrack(image_1, max_feats, 0.01, min_feat_dist, blockSize=11, useHarrisDetector=False) cv_corners, ids = _filter_new_corners(tracked_cv_corners, tracked_ids, new_cv_corners, max_id, min_feat_dist) max_id = max(max_id, ids[-1]) cv_corners = cv_corners.astype(np.float32) corners = FrameCorners( ids, cv_corners, np.ones(shape=(len(cv_corners),)) * 5 ) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: max_corners = 700 frame = list( map(lambda t: (np.array(t) * 255.0).astype(np.uint8), frame_sequence)) image_0 = frame[0] block_size = int(len(image_0) / 50) feature_params, lk_params = init_params(block_size, max_corners) corners = cv2.goodFeaturesToTrack(image_0, **feature_params).squeeze(axis=1) points_end = len(corners) ids = np.arange(points_end) sizes = np.full(points_end, 10) builder.set_corners_at_frame(0, FrameCorners(ids, corners, sizes)) for idx, image_1 in enumerate(frame[1:]): next, next_st, _ = cv2.calcOpticalFlowPyrLK(image_0, image_1, corners, None, **lk_params) mask = next_st.reshape(-1).astype(np.bool) ids, corners, sizes = ids[mask], next[mask], sizes[mask] if len(corners) < max_corners: mask = np.ones_like(image_1, dtype=np.uint8) for x, y in corners: cv2.circle(mask, (x, y), block_size, 0, -1) features = cv2.goodFeaturesToTrack(image_1, mask=mask * 225, **feature_params) features = features.squeeze(axis=1) if features is not None else [] for corner in features[:max_corners - len(corners)]: ids = np.concatenate([ids, [points_end]]) points_end += 1 corners = np.concatenate([corners, [corner]]) sizes = np.concatenate([sizes, [block_size]]) builder.set_corners_at_frame(idx, FrameCorners(ids, corners, sizes)) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = None depth, image_0_pyramid = PYRAMID_DEPTH, None ids = [] points = [] sizes = [] img_shape = frame_sequence[0].shape next_id = 0 for frame, image_1 in enumerate(frame_sequence): if len(points) > 0: ids, points, sizes = remove_old_points(image_0, image_1, ids, points, sizes, PYRAMID_DEPTH) mask = get_points_mask(img_shape, points, sizes) ids, points, sizes, next_id = add_new_points(image_1, ids, points, sizes, next_id, mask, frame == 0) builder.set_corners_at_frame(frame, FrameCorners(np.array(ids), np.array(points), np.array(sizes))) image_0 = image_1
def frame_corners(self, frame: int) -> FrameCorners: corners = np.zeros((0, 2), dtype=np.float32) ids = np.zeros(0, dtype=np.int32) radiuses = np.zeros(0, dtype=np.int32) for level in range(self._levels): if frame not in self._ids[level]: continue corners = np.concatenate((corners, self._corners[level][frame])) ids = np.concatenate((ids, self._ids[level][frame])) n = len(self._corners[level][frame]) radius = (self._base_radius - 1) * 2**level + 1 radiuses = np.concatenate((radiuses, np.ones(n) * radius)) return FrameCorners(ids, corners, radiuses)
def build_corner_storage(self) -> CornerStorage: self.process_all_events() frame_corners = [] for frame_id in range(self._n_frames): ids = self._frame_point_ids[frame_id] sizes = self._point_sizes[ids] points2d = [] for point_id in ids: i = frame_id - self._point_first_frames[point_id] points2d += [np.expand_dims(self._points2d[point_id][i], axis=0)] points2d = np.concatenate(points2d, axis=0) frame_corners += [FrameCorners(ids, points2d, sizes)] return StorageImpl(frame_corners)
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: N = 5000 block_size = 7 quality = 0.01 min_distance = 8 win_size = (15, 15) max_level = 2 criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03) feature_params = dict(qualityLevel=quality, minDistance=min_distance, blockSize=block_size) lk_params = dict(winSize=win_size, maxLevel=max_level, criteria=criteria) image_0 = frame_sequence[0] old_frame_corners = cv2.goodFeaturesToTrack(image_0, mask=None, maxCorners=N, **feature_params) old_corners = FrameCorners( np.array([i for i in range(len(old_frame_corners))]), old_frame_corners, np.array([block_size] * len(old_frame_corners))) last_id = len(old_frame_corners) - 1 image_0 = (image_0 * 256).astype(np.uint8) builder.set_corners_at_frame(0, old_corners) for frame, image_1 in enumerate(frame_sequence[1:], 1): image_1 = (image_1 * 256).astype(np.uint8) new_corners, status, err = cv2.calcOpticalFlowPyrLK( image_0, image_1, old_frame_corners, None, **lk_params) good_new = new_corners[status == 1] size = len(good_new) if len(good_new) < N: mask = np.ones_like(image_1) for new_corner in good_new: x = new_corner[0] y = new_corner[1] mask = cv2.circle(mask, (int(round(x)), int(round(y))), radius=min_distance, color=0, thickness=cv2.FILLED) delta = N - len(good_new) new_frame_corners = cv2.goodFeaturesToTrack(image_1.astype( np.float32), maxCorners=delta, mask=mask, **feature_params) if new_frame_corners is not None: new_frame_corners = np.array([[x[0][0], x[0][1]] for x in new_frame_corners]) good_new = np.concatenate((good_new, new_frame_corners)) size = min(N, len(good_new)) good_new = good_new[:size] ids = np.array([ old_corners.ids[i][0] for i in range(len(status)) if status[i] == 1 ], dtype=np.int32) old_len = len(ids) new_ids = np.array( [i for i in range(last_id + 1, last_id + 1 + size - old_len)], dtype=np.int32) last_id = last_id + size - old_len ids = np.concatenate((ids, new_ids)) corners = FrameCorners(ids, good_new, np.array([block_size] * size)) builder.set_corners_at_frame(frame, corners) image_0 = image_1 old_frame_corners = good_new.reshape(-1, 1, 2) old_corners = corners
def concat_corners(corners, new_corners): result = FrameCorners(np.concatenate((corners.ids, new_corners.ids)), np.concatenate((corners.points, new_corners.points)), np.concatenate((corners.sizes, new_corners.sizes))) return result
def get_current_corners(self): return FrameCorners( np.array([corner.id for corner in self.corners]), np.array([corner.get_position() for corner in self.corners]), np.array([self.block_size] * len(self.corners)))
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: image_0 = frame_sequence[0] maxCorners = image_0.shape[0] * image_0.shape[1] // 1000 qualityLevel = 0.01 minDistance = 5 blockSize = 10 corner_points = cv2.goodFeaturesToTrack(image_0, maxCorners, qualityLevel, minDistance, blockSize=blockSize) ids = np.arange(0, corner_points.shape[0], 1).reshape((-1, 1)) sizes = np.full((corner_points.shape[0], 2), blockSize) corners = FrameCorners(ids, corner_points, sizes) builder.set_corners_at_frame(0, corners) maxLevel = 3 maxCount = 10 epsilon = 0.03 criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, maxCount, epsilon) for frame, image_1 in enumerate(frame_sequence[1:], 1): image_0_8u = (image_0 * 255).astype(np.uint8) image_1_8u = (image_1 * 255).astype(np.uint8) nextPts, status, err = cv2.calcOpticalFlowPyrLK(image_0_8u, image_1_8u, corner_points, None, winSize=(blockSize, blockSize), maxLevel=maxLevel, criteria=criteria) corner_points = nextPts[status == 1] ids = ids[status == 1] new_corner_points = cv2.goodFeaturesToTrack(image_1, maxCorners, qualityLevel, minDistance, blockSize=blockSize) corner_points = _merge_corners(corner_points, new_corner_points, maxCorners, minDistance) sizes = np.full((corner_points.shape[0], 2), blockSize) new_points_num = corner_points.shape[0] - ids.shape[0] new_ids = np.arange( np.max(ids) + 1, np.max(ids) + 1 + new_points_num, 1).reshape((-1, 1)) ids = np.concatenate((ids.reshape((-1, 1)), new_ids)) corners = FrameCorners(ids, corner_points, sizes) corner_points = corner_points.reshape((-1, 1, 2)) builder.set_corners_at_frame(frame, corners) image_0 = image_1
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: height, width = frame_sequence.frame_shape[:2] max_levels = min(np.log2(height), np.log2(width)).astype(np.int8) MAX_CORNERS = 2000 WINDOW_SIZE = (15, 15) MIN_DIST = 10 BLOCK_SIZE = 3 QUALITY_LEVEL = 0.075 pyramid = None prev_pyramid = None levels = 0 prev_levels = 0 pts_id = 0 corner_pts = np.ndarray((0, 2), dtype=np.float32) corner_ids = np.ndarray(0, dtype=np.int32) corner_szs = np.ndarray(0, dtype=np.float32) for frame, image in enumerate(frame_sequence): levels, pyramid = cv2.buildOpticalFlowPyramid(img=(image * 255).astype( np.uint8), winSize=WINDOW_SIZE, maxLevel=max_levels, pyramid=None, withDerivatives=False) if corner_pts.size > 0: prev_corners = corner_pts cur_corners = None min_levels = min(levels, prev_levels) for level in reversed(range(min_levels)): cur_corners, status, err = cv2.calcOpticalFlowPyrLK( prevImg=prev_pyramid[level], nextImg=pyramid[level], prevPts=(prev_corners / (2**level)).astype(np.float32), nextPts=cur_corners * 2 if cur_corners is not None else None, winSize=WINDOW_SIZE) mask = status[:, 0] == 1 corner_pts = cur_corners[mask] corner_ids = corner_ids[mask] corner_szs = corner_szs[mask] def prohibit_circle(mask, x, y, r): mask = cv2.circle(img=mask, center=(np.int(x), np.int(y)), radius=np.int(r), color=0, thickness=-1) n = corner_szs.size if n >= MAX_CORNERS: prev_pyramid = pyramid prev_levels = levels builder.set_corners_at_frame( frame, FrameCorners(corner_ids, corner_pts, corner_szs)) continue new_pts = [] new_ids = [] new_szs = [] mask = np.full((height, width), 255, dtype=np.uint8) for x, y, r in np.column_stack((corner_pts, corner_szs)): prohibit_circle(mask, x, y, r) for i in range(levels): if i > 0: mask = cv2.pyrDown(mask).astype(np.uint8) new_corners = cv2.goodFeaturesToTrack(image=pyramid[i], maxCorners=max( 0, MAX_CORNERS - n), qualityLevel=QUALITY_LEVEL, minDistance=MIN_DIST, mask=mask, blockSize=BLOCK_SIZE) if new_corners is None: continue for x, y in new_corners[:, 0, :]: if not mask[np.int(y), np.int(x)]: continue new_pts.append((x * (2.0**i), y * (2.0**i))) new_ids.append(pts_id) new_szs.append(BLOCK_SIZE * (2.0**i)) pts_id += 1 prohibit_circle(mask, x, y, BLOCK_SIZE) if new_pts: corner_pts = np.concatenate((corner_pts, new_pts)) corner_ids = np.concatenate((corner_ids, new_ids)) corner_szs = np.concatenate((corner_szs, new_szs)) prev_pyramid = pyramid prev_levels = levels builder.set_corners_at_frame( frame, FrameCorners(corner_ids, corner_pts, corner_szs))
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: idss, cornerss, radiii = get_corners_video(frame_sequence, 2) for i, (ids, corners, radii) in enumerate(zip(idss, cornerss, radiii)): frame_corners = FrameCorners(ids, corners, radii) builder.set_corners_at_frame(i, frame_corners)
def _build_impl(frame_sequence: pims.FramesSequence, builder: _CornerStorageBuilder) -> None: """ Params section: 0) Common parameters """ MAX_CORNERS = 2000 EPS = 1e-9 MAX_SHIFT = 1. """ 1) Shi-Tomasi method: a) Number of strongest corners to detect b) Quality level (0..1). All corners with quality below it will be ignored c) Minimum euclidean distance between corners detected """ N_STRONGEST = 1500 QULITY_LEVEL = 0.11 MIN_DISTANCE = 15 get_corners = lambda img: cv2.goodFeaturesToTrack( image_0, N_STRONGEST, QULITY_LEVEL, MIN_DISTANCE) """ 2) Lucas-Kanade optical Flow: a) Size of the search window at each pyramid level b) Maximal pyramid level (0-based, number of layers - 1) c) Termination criterion """ WIN_SIZE = (10, 10) MAX_LEVEL = 2 CRITERIA = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03) get_next_points = lambda first_frame, second_frame, first_frame_corners: \ cv2.calcOpticalFlowPyrLK(first_frame, second_frame, first_frame_corners, winSize=WIN_SIZE, maxLevel=MAX_LEVEL, criteria=CRITERIA, nextPts=None) """ End of params section. Some initial parameter values have been collected from the corresponding opencv guide. """ frame_sequence = [(frame * 255).astype(np.uint8) for frame in frame_sequence] image_0 = frame_sequence[0] corners_0 = get_corners(image_0) n_corners = corners_0.shape[0] ids = np.arange(n_corners) points = corners_0.squeeze() sizes = np.full(n_corners, 5) corners = FrameCorners(ids, points, sizes) builder.set_corners_at_frame(0, corners) id_counter = n_corners for frame, image_1 in enumerate(frame_sequence[1:], 1): points_1, st, err = get_next_points(image_0, image_1, points) points_1 = points_1.squeeze() mask = st.squeeze() == 1 # Additional check points_0, st, err = get_next_points(image_1, image_0, points_1) mask = mask & (st.squeeze() == 1) mask = mask & (np.linalg.norm(points_0 - points, axis=1) < MAX_SHIFT) ids = ids[mask] points = points_1[mask] sizes = sizes[mask] n_corners = ids.shape[0] corners_1 = get_corners(image_1) new_points = corners_1.squeeze() # Heuristics: let's iteratively select the furthest new corner. min_dists = {} for ind, new_point in enumerate(new_points): min_dist = None for point in points: dist = np.linalg.norm(point - new_point) if min_dist is None or dist < min_dist: min_dist = dist min_dists[ind] = min_dist while n_corners < MAX_CORNERS and len(min_dists) > 0: max_min_dist = None next_point_ind = -1 for ind, dist in min_dists.items(): if dist is None or max_min_dist is None or max_min_dist < dist: max_min_dist = dist next_point_ind = ind if next_point_ind == -1 or max_min_dist < MIN_DISTANCE + EPS: break ids = np.concatenate([ids, [id_counter]]) points = np.concatenate([points, [new_points[next_point_ind]]]) sizes = np.concatenate([sizes, [5]]) min_dists.pop(next_point_ind) for ind in min_dists.keys(): min_dists[ind] = min( min_dists[ind], np.linalg.norm(new_points[ind] - new_points[next_point_ind])) n_corners += 1 id_counter += 1 corners = FrameCorners(ids, points, sizes) builder.set_corners_at_frame(frame, corners) image_0 = image_1
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