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