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
