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 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 _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 track_camera(corner_storage, intrinsic_mat, known_view1, known_view2):
n = len(corner_storage)
point_cloud_builder = PointCloudBuilder()
view_mat = [eye3x4()] * n
view_mat[known_view1[0]] = pose_to_view_mat3x4(known_view1[1])
view_mat[known_view2[0]] = pose_to_view_mat3x4(known_view2[1])
min_angle = 1.0
max_error = 1.0
new_points = []
ids = []
correspondences = build_correspondences(corner_storage[known_view1[0]],
corner_storage[known_view2[0]])
while len(new_points) < 10:
new_points, ids, _ = triangulate_correspondences(
correspondences, pose_to_view_mat3x4(known_view1[1]),
pose_to_view_mat3x4(known_view2[1]), intrinsic_mat,
TriangulationParameters(max_error, min_angle, 0))
min_angle -= 0.2
max_error += 0.4
point_cloud_builder.add_points(ids, new_points)
for i in range(known_view1[0] - 1, -1, -1):
solve_frame(i, corner_storage, point_cloud_builder, view_mat,
intrinsic_mat, 1)
for i in range(known_view1[0] + 1, n):
solve_frame(i, corner_storage, point_cloud_builder, view_mat,
intrinsic_mat, -1)
return view_mat, point_cloud_builder
def track_and_calc_colors(camera_parameters: CameraParameters,
corner_storage: CornerStorage,
frame_sequence_path: str,
known_view_1: Optional[Tuple[int, Pose]] = None,
known_view_2: Optional[Tuple[int, Pose]] = None) \
-> Tuple[List[Pose], PointCloud]:
rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path)
intrinsic_mat = to_opencv_camera_mat3x3(
camera_parameters,
rgb_sequence[0].shape[0]
)
view_mats, point_cloud_builder = [eye3x4()] * len(corner_storage), PointCloudBuilder()
try:
known_view_1, known_view_2 = _find_pos_pair(intrinsic_mat, corner_storage)
tracker = CameraTracker(intrinsic_mat, corner_storage, known_view_1, known_view_2)
view_mats, point_cloud_builder = tracker.track()
except TrackingError as error:
print(error)
print('Poses and point cloud are not calculated')
calc_point_cloud_colors(
point_cloud_builder,
rgb_sequence,
view_mats,
intrinsic_mat,
corner_storage,
5.0
)
point_cloud = point_cloud_builder.build_point_cloud()
poses = list(map(view_mat3x4_to_pose, view_mats))
return poses, point_cloud
def 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 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 find_best_start_poses(frames_n, corner_storage, intrinsic_mat):
best_frames = (0, 0)
best_snd_pose = None
best_pose_score = 0
for first_frame in range(frames_n):
for second_frame in range(first_frame + 5, frames_n):
pose, pose_score = get_pose_with_score(first_frame, second_frame,
corner_storage,
intrinsic_mat)
if pose_score > best_pose_score:
best_frames = (first_frame, second_frame)
best_snd_pose = pose
best_pose_score = pose_score
return best_frames, view_mat3x4_to_pose(eye3x4()), best_snd_pose
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 find_best_start_poses(self):
best_i = 0
best_j = 0
best_j_pose = None
best_pose_points = 0
step = self.frame_count // 20
for i in range(self.frame_count):
for j in range(i + 5, self.frame_count, step):
pose, pose_points = self.get_pose(i, j)
if pose_points > best_pose_points:
best_i = i
best_j = j
best_j_pose = pose
best_pose_points = pose_points
return best_i, eye3x4(), best_j, pose_to_view_mat3x4(best_j_pose)
def 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 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 _find_pos_pair(intrinsic_mat: np.ndarray,
corner_storage: CornerStorage,
param_koeff: float = 1) -> Tuple[Tuple[int, Pose], Tuple[int, Pose]]:
best_num_pts = 0
best_frame = 1
best_pose = None
for frame_number in range(1, len(corner_storage)):
print(frame_number)
num_pts, pose = _test_frame_pair(intrinsic_mat, corner_storage[0], corner_storage[frame_number])
if num_pts > best_num_pts:
best_num_pts = num_pts
best_pose = pose
best_frame = frame_number
if best_pose is None:
if param_koeff < 4:
return _find_pos_pair(intrinsic_mat, corner_storage, param_koeff * 2)
raise TrackingError("Failed to initialize tracking")
return (0, view_mat3x4_to_pose(eye3x4())), (best_frame, best_pose)
def __init__(self, intrinsic_mat, corner_storage, known_view_1, known_view_2):
self.intrinsic_mat = intrinsic_mat
self.corner_storage = corner_storage
frame_num_1 = known_view_1[0]
frame_num_2 = known_view_2[0]
self.pc_builder = PointCloudBuilder()
view_matr_1 = pose_to_view_mat3x4(known_view_1[1])
view_matr_2 = pose_to_view_mat3x4(known_view_2[1])
self.tracked_views = [eye3x4()] * len(self.corner_storage)
self.tracked_views[frame_num_1] = view_matr_1
self.tracked_views[frame_num_2] = view_matr_2
self.initial_baseline = get_baseline(view_matr_1, view_matr_2)
print("initial baseline =", self.initial_baseline)
self._add_points_from_frame(frame_num_1, frame_num_2, initial_triangulation=True)
self.min_init_frame = min(frame_num_1, frame_num_2)
self.max_init_frame = max(frame_num_1, frame_num_2)
self.outliers = set()
def initialize_known_views(self):
median_corners = np.median(np.array([len(self.corner_storage[i].ids) for i in range(self.frame_count)]))
def score(result):
_, _, _, inliers_count, median_cos = result
return (inliers_count / median_corners) ** 2 + (1 - median_cos ** 2)
leniency = 1.0
while True:
print('Trying to initialize views...')
best_view_result = max(self.initial_views_generator(leniency), key=score, default=None)
if best_view_result is None:
print('Initialization failed, relaxing constraints...')
leniency *= 1.05
continue
frame_1, frame_2, view, _, _ = best_view_result
print('Found initial poses for frames {} and {}'.format(frame_1, frame_2))
self.update_view(frame_1, eye3x4())
self.update_view(frame_2, view)
return
def initial_camera_views(
corner_storage: CornerStorage, intrinsic_mat: np.ndarray
) -> Tuple[Tuple[int, Pose], Tuple[int, Pose]]:
max_pose = None
max_npoints = 0
maxj = -1
maxi = -1
enum_corner_storage = list(enumerate(corner_storage))
for j, base_frame in tqdm(enum_corner_storage[::32]):
base_frame = corner_storage[0]
for i, frame in enum_corner_storage[j + 1:j + 32]:
pose, npoints = pose_by_frames(base_frame, frame, intrinsic_mat)
print(i, j, npoints)
if npoints > max_npoints:
max_npoints = npoints
maxi = i
maxj = j
max_pose = pose
print(max_npoints)
return (maxj, view_mat3x4_to_pose(eye3x4())), (maxi, max_pose)
def get_pose(self, frame_1, frame_2):
p1 = self.corner_storage[frame_1]
p2 = self.corner_storage[frame_2]
corresp = build_correspondences(p1, p2)
if len(corresp.ids) < 6:
return None, 0
E, mask_essential = cv2.findEssentialMat(corresp[1], corresp[2], self.intrinsic_mat, method=cv2.RANSAC,
threshold=1.0)
_, mask_homography = cv2.findHomography(corresp[1], corresp[2], method=cv2.RANSAC)
if mask_essential is None or mask_homography is None:
return None, 0
essential_inliers = mask_essential.flatten().sum()
homography_inliers = mask_homography.flatten().sum()
if homography_inliers / essential_inliers > 0.5:
return None, 0
corresp = _remove_correspondences_with_ids(corresp, np.argwhere(mask_essential == 0))
R1, R2, t = cv2.decomposeEssentialMat(E)
possible_poses = [Pose(R1.T, R1.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ t), Pose(R2.T, R2.T @ (-t))]
best_pose_points = 0
best_pose = None
for pose in possible_poses:
p, ids, med = triangulate_correspondences(corresp, eye3x4(), pose_to_view_mat3x4(pose), self.intrinsic_mat, self.triangulation_parameters)
if len(p) > best_pose_points:
best_pose_points = len(p)
best_pose = pose
return best_pose, best_pose_points
def track(self):
step_num = 1
num_of_defined_poses = np.sum([
track_pos_info is not None for track_pos_info in self.tracked_poses
])
while num_of_defined_poses != self.num_of_frames:
unsolved_frames = [
i for i in range(self.num_of_frames)
if self.tracked_poses[i] is None
]
new_cams_info = []
for frame, cam_info in zip(unsolved_frames,
map(self._get_pos, unsolved_frames)):
if cam_info is not None:
new_cams_info.append((frame, cam_info))
if len(new_cams_info) == 0:
print(f'Can not get more camera positions, '
f'{self.num_of_frames - num_of_defined_poses}'
f' frames left without defined camera position')
self.tracked_poses = [
CameraInfo(view_mat3x4_to_pose(eye3x4()), 0)
if pos is None else pos for pos in self.tracked_poses
]
break
best_frame = None
best_new_cam_info = None
for frame, cam_info in new_cams_info:
if best_new_cam_info is None or best_new_cam_info.inliers < cam_info.inliers:
best_new_cam_info = cam_info
best_frame = frame
print('Added camera position for frame ', best_frame)
print('Number of inliers: ', best_new_cam_info.inliers)
self.tracked_poses[best_frame] = best_new_cam_info
self._retriangulate_3d_points(best_frame)
if step_num % 4 == 0:
self._update_camera_poses()
step_num += 1
num_of_defined_poses = np.sum([
tracked_pos_info is not None
for tracked_pos_info in self.tracked_poses
])
print(
f'{num_of_defined_poses}/{self.num_of_frames} camera positions found, {len(self.point_cloud)} points in cloud'
)
self._update_camera_poses()
ids, cloud_points = [], []
for pt_id, clout_pt_info in self.point_cloud.items():
ids.append(pt_id)
cloud_points.append(clout_pt_info.pos)
return list(map(lambda tracked_pos_info: tracked_pos_info.pos, self.tracked_poses)), \
PointCloudBuilder(np.array(ids), np.array(cloud_points))
