def test_frames(intrinsic_mat, fr1, fr2):
corr = build_correspondences(fr1, fr2)
if corr.points_1.shape[0] < 5:
return None, 0
mat, mask = cv2.findEssentialMat(corr.points_1,
corr.points_2,
intrinsic_mat,
method=cv2.RANSAC,
prob=0.999,
threshold=1.0)
if mat is None or mat.shape != (3, 3) or not validate_mat(corr, mask):
return None, 0
# https://kite.com/python/docs/cv2.decomposeEssentialMat
R1, R2, t = cv2.decomposeEssentialMat(mat)
poses = [
Pose(R1.T, R1.T @ t),
Pose(R2.T, R2.T @ t),
Pose(R1.T, R1.T @ (-t)),
Pose(R2.T, R2.T @ (-t))
]
best_size, best_idx = -1, 0
for i, pose in enumerate(poses):
points, _, _ = triangulate_correspondences(
remove_correspondences_with_ids(corr, np.argwhere(mask == 0)),
eye3x4(), pose_to_view_mat3x4(pose), intrinsic_mat, params)
if points.shape[0] > best_size:
best_size, best_idx = points.shape[0], i
return poses[best_idx], best_size
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 _two_frame_initialization(self, frame1: FrameCorners,
frame2: FrameCorners):
correspondences = build_correspondences(frame1, frame2)
if correspondences.points_1.shape[0] < 5:
return None, 0
essential_mat, mask_essential = cv2.findEssentialMat(
correspondences.points_1,
correspondences.points_2,
self._intrinsic_mat,
method=cv2.RANSAC,
prob=0.9999,
threshold=1.)
_, mask_homography = cv2.findHomography(
correspondences.points_1,
correspondences.points_2,
method=cv2.RANSAC,
confidence=0.9999,
ransacReprojThreshold=self._triangulation_parameters.
max_reprojection_error)
# zeros in mask correspond to outliers
essential_inliers = np.count_nonzero(mask_essential)
homography_inliers = np.count_nonzero(mask_homography)
if essential_inliers < 1. * homography_inliers:
return None, 0
correspondences_filtered = remove_correspondences_with_ids(
correspondences, np.argwhere(mask_essential == 0))
R1, R2, t = cv2.decomposeEssentialMat(essential_mat)
possible_poses = [
Pose(R1.T, R1.T @ t),
Pose(R2.T, R2.T @ t),
Pose(R1.T, R1.T @ (-t)),
Pose(R2.T, R2.T @ (-t))
]
poses2points = [0] * 4
for i, pose in enumerate(possible_poses):
points, ids = triangulate_correspondences(
correspondences_filtered, eye3x4(), pose_to_view_mat3x4(pose),
self._intrinsic_mat, self._triangulation_parameters)
poses2points[i] = points.shape[0]
best_pose = np.argmax(poses2points)
return possible_poses[best_pose], poses2points[best_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 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 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 _initialize_with_two_frames(corners_1, corners_2, intrinsic_mat,
triangulation_parameters):
correspondences = build_correspondences(corners_1, corners_2)
if len(correspondences.ids) <= 5:
return None, None, None
E, mask_E = cv2.findEssentialMat(correspondences.points_1,
correspondences.points_2,
intrinsic_mat,
method=cv2.RANSAC,
prob=0.999,
threshold=1.0)
if E is None or E.shape != (3, 3):
return None, None, None
fundamental_inliers = np.sum(mask_E)
H, mask_H = cv2.findHomography(correspondences.points_1,
correspondences.points_2,
method=cv2.RANSAC,
ransacReprojThreshold=1.0,
confidence=0.999)
homography_inliers = np.sum(mask_H)
if fundamental_inliers / homography_inliers < 1.2:
return None, None, None
correspondences = _remove_correspondences_with_ids(
correspondences,
np.where(mask_E == 0)[0])
best_points, best_ids, best_pose = None, None, None
R1, R2, t_d = cv2.decomposeEssentialMat(E)
for pose in [
Pose(R1.T, R1.T @ t_d),
Pose(R2.T, R2.T @ t_d),
Pose(R1.T, R1.T @ -t_d),
Pose(R2.T, R2.T @ -t_d)
]:
points, ids = triangulate_correspondences(
correspondences,
eye3x4(),
pose_to_view_mat3x4(pose),
intrinsic_mat,
triangulation_parameters,
)
if best_ids is None or len(ids) > len(best_ids):
best_points, best_ids, best_pose = points, ids, pose
return best_points, best_ids, best_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 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 _init_on_two_frames(frame_corners_0, frame_corners_1, intrinsic_mat, triang_params) \
-> Tuple[Pose, np.ndarray, np.ndarray]:
best_pose, best_pts, best_ids = None, np.array([]), np.array([])
correspondences = build_correspondences(frame_corners_0, frame_corners_1)
if len(correspondences.ids) < 5:
return best_pose, best_pts, best_ids
pts_0 = correspondences.points_1
pts_1 = correspondences.points_2
# Check if the E matrix will be well-defined based on homography matrix
H, h_mask = cv2.findHomography(pts_0,
pts_1,
method=cv2.RANSAC,
ransacReprojThreshold=1.0,
confidence=0.999)
if np.nonzero(h_mask)[0].size / len(pts_0) > 0.9:
return best_pose, best_pts, best_ids
E, e_mask = cv2.findEssentialMat(pts_0,
pts_1,
cameraMatrix=intrinsic_mat,
method=cv2.RANSAC,
prob=0.999,
threshold=1.0)
if E is None or E.shape != (3, 3):
return best_pose, best_pts, best_ids
correspondences = remove_correspondences_with_ids(correspondences,
np.where(e_mask == 0)[0])
R1, R2, t_d = cv2.decomposeEssentialMat(E)
for R, t in [(R1, t_d), (R1, -t_d), (R2, t_d), (R2, -t_d)]:
pose_1 = Pose(R.T, R.T @ -t)
view_1 = pose_to_view_mat3x4(pose_1)
view_0 = eye3x4()
pts, ids = triangulate_correspondences(correspondences, view_0, view_1,
intrinsic_mat, triang_params)
if len(best_ids) < len(ids):
best_pose, best_pts, best_ids = pose_1, pts, ids
return best_pose, best_pts, best_ids
def _initialize_with_history(corners_prev, corners, intrinsic_mat, triang_pars):
correspondences = build_correspondences(corners_prev, corners)
if len(correspondences.ids) <= 5:
return None, np.array([], dtype=np.int32), np.array([], dtype=np.int32)
E, mask = cv2.findEssentialMat(correspondences.points_1, correspondences.points_2,
cameraMatrix=intrinsic_mat, method=cv2.RANSAC, prob=0.99, threshold=1.)
H, hmask = cv2.findHomography(correspondences.points_1, correspondences.points_2,
method=cv2.RANSAC, ransacReprojThreshold=1., confidence=0.99)
if np.sum(mask) / np.sum(hmask) < 0.8:
return None, np.array([], dtype=np.int32), np.array([], dtype=np.int32)
r1, r2, t = cv2.decomposeEssentialMat(E)
correspondences = remove_correspondences_with_ids(correspondences, np.where(mask == 0)[0])
view_mat, points, ids = None, np.array([], dtype=np.int32), np.array([], dtype=np.int32)
for r in [r1, r2]:
for tt in [t.reshape(-1), -t.reshape(-1)]:
view_ = pose_to_view_mat3x4(Pose(r.T, r.T @ tt))
pts_, ids_ = triangulate_correspondences(correspondences, eye3x4(), view_, intrinsic_mat, triang_pars)
if pts_.size > points.size:
view_mat, points, ids = view_, pts_, ids_
return view_mat, points, ids
def view_mat3x4_to_pose(view_mat: np.ndarray) -> Pose:
r_mat = view_mat[:, :3]
t_vec = view_mat[:, 3]
return Pose(r_mat.T, r_mat.T @ -t_vec)
def _to_pose_from_mat4x4(mat):
return Pose(mat[:3, :3], mat[:3, 3].flatten())
def _rescale_track(cam_track, scale):
rescaled_cam_track = []
for pose in cam_track:
rescaled_cam_track.append(Pose(pose.r_mat, pose.t_vec * scale))
return rescaled_cam_track
