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 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 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 _track_camera(corner_storage: CornerStorage,
intrinsic_mat: np.ndarray,
known_view_1: Tuple[int, Pose],
known_view_2: Tuple[int, Pose]) \
-> Optional[Tuple[List[np.ndarray], PointCloudBuilder]]:
parameters = TriangulationParameters(max_reprojection_error=1.,
min_triangulation_angle_deg=2.,
min_depth=0.1)
return CameraTracker(corner_storage, intrinsic_mat, parameters,
known_view_1, known_view_2).track()
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 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 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 __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 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 _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 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_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)
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 = get_matrix_poses(corner_storage,
intrinsic_mat)
video_size = len(rgb_sequence)
frame_trackers = [
FrameTrack(i, corner_storage[i]) for i in range(video_size)
]
known_tracker_creator = lambda x: FrameTrack(x[0], corner_storage[x[0]],
pose_to_view_mat3x4(x[1]))
frame_trackers[known_view_1[0]] = known_tracker_creator(known_view_1)
frame_trackers[known_view_2[0]] = known_tracker_creator(known_view_2)
init_params = triang_params
for angle in range(90, 0, -2):
params = TriangulationParameters(
max_reprojection_error=MAX_REPROJECTION_ERROR,
min_triangulation_angle_deg=angle,
min_depth=0.001)
_, points = triangulate_trackers(frame_trackers[known_view_1[0]],
frame_trackers[known_view_2[0]],
intrinsic_mat, params)
if len(points) > 100:
print(f"Chosen init angle: {angle}")
init_params = params
break
ids, points = triangulate_trackers(frame_trackers[known_view_1[0]],
frame_trackers[known_view_2[0]],
intrinsic_mat, init_params)
point_cloud_builder = PointCloudBuilder(ids, points)
if len(points) < MIN_STARTING_POINTS:
print(
f"Not enough starting points ({len(points)}), please choose another initial frames pair"
f"\n0, 20 is a good pair for short fox, ")
return [], PointCloudBuilder()
frame_trackers[known_view_1[0]].update_reproj_error(
point_cloud_builder, intrinsic_mat)
frame_trackers[known_view_2[0]].update_reproj_error(
point_cloud_builder, intrinsic_mat)
point_cloud_builder = track(ITERATIONS, frame_trackers,
point_cloud_builder, intrinsic_mat)
view_mats = [x.mtx for x in frame_trackers]
for i in range(1, len(view_mats)):
if view_mats[i] is None:
view_mats[i] = view_mats[i - 1]
for i in range(len(view_mats) - 2, -1, -1):
if view_mats[i] is None:
view_mats[i] = view_mats[i + 1]
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
from corners import CornerStorage
from data3d import CameraParameters, PointCloud, Pose
import frameseq
from _camtrack import (PointCloudBuilder, create_cli, calc_point_cloud_colors,
to_opencv_camera_mat3x3, view_mat3x4_to_pose,
build_correspondences, pose_to_view_mat3x4,
TriangulationParameters, triangulate_correspondences,
rodrigues_and_translation_to_view_mat3x4, eye3x4,
_remove_correspondences_with_ids)
MAX_REPROJ_ERR = 7.9
MIN_TRIANG_ANGLE = 5.2
MIN_DEPTH = 0.3
TRIANG_PARAMS = TriangulationParameters(
max_reprojection_error=MAX_REPROJ_ERR,
min_triangulation_angle_deg=MIN_TRIANG_ANGLE,
min_depth=MIN_DEPTH)
HALF_FRAME_DIST = 180
def add_points(point_cloud_builder, corner_storage, i, view_mats,
intrinsic_mat):
cloud_changed = False
for d in range(-HALF_FRAME_DIST, HALF_FRAME_DIST):
if d == 0 or i + d < 0 or i + d >= len(view_mats) or view_mats[
i + d] is None:
continue
correspondences = build_correspondences(corner_storage[i + d],
corner_storage[i],
point_cloud_builder.ids)
def track_and_find_point_cloud(intrinsic_mat, corner_storage, known_view_1, known_view_2):
num_frames = len(corner_storage)
view_mats = [None] * num_frames
point_cloud_builder = PointCloudBuilder()
print(f'Frames with known views: {known_view_1[0]} and {known_view_2[0]}')
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])
triang_params = TriangulationParameters(max_reprojection_error=MAX_REPR_ERR,
min_triangulation_angle_deg=MIN_TRIANG_ANGLE_DEG,
min_depth=MIN_DEPTH)
add_points_to_cloud(point_cloud_builder,
corner_storage[known_view_1[0]],
corner_storage[known_view_2[0]],
view_mats[known_view_1[0]],
view_mats[known_view_2[0]],
intrinsic_mat,
triang_params)
while True:
was_updated = False
for i in range(num_frames):
if view_mats[i] is not None:
continue
print(f"\nCurrent frame: {i}")
# find intersection of current point cloud and current frame
corners = corner_storage[i]
corner_ids = []
points_3d = []
points_2d = []
for id, corner in zip(corners.ids, corners.points):
if id not in point_cloud_builder.ids:
continue
ind_in_builder, _ = np.nonzero(point_cloud_builder.ids == id)
corner_ids.append(id)
points_3d.append(point_cloud_builder.points[ind_in_builder[0]])
points_2d.append(corner)
print(f"Size of intersection of current point cloud and current frame: {len(corner_ids)}")
if len(corner_ids) < 5:
continue
points_3d = np.array(points_3d)
points_2d = np.array(points_2d)
retval, rvec, tvec, inliers = cv2.solvePnPRansac(points_3d.reshape((-1, 1, 3)),
points_2d.reshape((-1, 1, 2)),
cameraMatrix=intrinsic_mat,
distCoeffs=None,
reprojectionError=PNP_REPROJ_ERROR)
if not retval:
print("Unsuccessful solution of PnP")
continue
print(f"Position found by {len(inliers)} inliers")
view_mat = rodrigues_and_translation_to_view_mat3x4(rvec, tvec)
view_mats[i] = view_mat
for j in range(num_frames):
if i != j and view_mats[j] is not None:
points_added = add_points_to_cloud(point_cloud_builder,
corner_storage[i],
corner_storage[j],
view_mats[i],
view_mats[j],
intrinsic_mat,
triang_params)
if points_added > 0:
was_updated = True
print(f"Current size of point cloud: {point_cloud_builder.points.size}")
if was_updated is False:
break
for i in range(num_frames):
if view_mats[i] is None:
view_mats[i] = view_mats[i - 1]
return view_mats, 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])
if known_view_1 is None or known_view_2 is None:
known_view_1, known_view_2 = calc_known_views(corner_storage,
intrinsic_mat)
# TODO: implement
num_frames_retr = 10
triang_params = TriangulationParameters(8.0, 1.0, .1)
num_frames = len(corner_storage)
frame_1 = known_view_1[0]
frame_2 = known_view_2[0]
print(f'{frame_1}, {frame_2}')
tvecs = [None] * num_frames
tvecs[frame_1] = known_view_1[1].t_vec
tvecs[frame_2] = known_view_2[1].t_vec
view_mat_1 = pose_to_view_mat3x4(known_view_1[1])
view_mat_2 = pose_to_view_mat3x4(known_view_2[1])
view_mats = [None] * num_frames
view_mats[frame_1] = view_mat_1
view_mats[frame_2] = view_mat_2
cloud = {}
corrs = build_correspondences(corner_storage[frame_1],
corner_storage[frame_2])
points3d, ids, _ = triangulate_correspondences(corrs, view_mat_1,
view_mat_2, intrinsic_mat,
triang_params)
for point3d, id in zip(points3d, ids):
cloud[id] = point3d
current_corners_occurences = {}
corner_to_frames = {}
for i, corners in enumerate(corner_storage):
for j, id in enumerate(corners.ids.flatten()):
current_corners_occurences[id] = 0
if id not in corner_to_frames.keys():
corner_to_frames[id] = [[i, j]]
else:
corner_to_frames[id].append([i, j])
while len(untracked_frames(view_mats)) > 0:
untr_frames = untracked_frames(view_mats)
max_num_inl = -1
best_frame = -1
best_rvec = None
best_tvec = None
for frame in untr_frames:
rvec, tvec, num_inl = calc_camera_pose(frame, corner_storage,
cloud, intrinsic_mat)
if rvec is not None:
if num_inl > max_num_inl:
best_frame = frame
max_num_inl = num_inl
best_rvec = rvec
best_tvec = tvec
if max_num_inl == -1:
break
corners_to_add = []
for id in corner_storage[best_frame].ids.flatten():
if id in current_corners_occurences.keys():
current_corners_occurences[id] += 1
if current_corners_occurences[id] >= 2:
corners_to_add.append(id)
for corner in corners_to_add:
add_new_point(corner, corner_to_frames, view_mats, tvecs,
corner_storage, cloud, current_corners_occurences,
intrinsic_mat)
print(f"Frame: {best_frame}, Inliers: {max_num_inl}")
print(f"Cloud size: {len(cloud)}")
view_mats[best_frame] = rodrigues_and_translation_to_view_mat3x4(
best_rvec, best_tvec)
tvecs[best_frame] = best_tvec
last_mat = None
for frame in range(num_frames):
if view_mats[frame] is None:
view_mats[frame] = last_mat
else:
last_mat = view_mats[frame]
ids = []
points = []
for id in cloud.keys():
ids.append(id)
points.append(cloud[id])
point_cloud_builder = PointCloudBuilder(np.array(ids, dtype=np.int),
np.array(points))
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
triangulate_correspondences,
rodrigues_and_translation_to_view_mat3x4
)
EPS = 1e-8
MAX_EPIPOL_LINE_DIST = 1
MAX_REPROJECTION_ERROR = 1
MIN_TRIANGULATION_ANGLE_DEG = 1
RANSAC_P = 0.999
MAX_POINT_CLOUD_SIZE = 200000
TRIANGULATION_PARAMETERS = TriangulationParameters(
max_reprojection_error=MAX_REPROJECTION_ERROR,
min_triangulation_angle_deg=MIN_TRIANGULATION_ANGLE_DEG,
min_depth=0
)
class PointStorage():
def __init__(self,
corner_storage: Optional[CornerStorage] = None,
n_frames: Optional[int] = None,
n_points: Optional[int] = None):
if corner_storage is not None:
self._init_by_corner_storage(corner_storage)
else:
self._init_data(n_frames, n_points)
self._init_events()
TRIANGULATION_MIN_DEPTH = 0.1
PNP_RANSAC_INLIERS_MAX_REPROJECTION_ERROR = 2.0
PNP_RANSAC_CONFIDENCE = 0.999
INIT_RANSAC_CONFIDENCE = 0.9999
INIT_MIN_FRAME_DISTANCE = 3
INIT_MAX_FRAME_DISTANCE = 75
INIT_MAX_LINE_DISTANCE = 1.0
INIT_HOMOGRAPHY_MAX_REPROJECTION_ERROR = 1.0
INIT_MAX_HOMOGRAPHY_INLIER_RATIO = 0.5
TRIANGULATION_PARAMS = TriangulationParameters(
TRIANGULATION_MAX_REPROJECTION_ERROR,
TRIANGULATION_MIN_ANGLE_DEG,
TRIANGULATION_MIN_DEPTH
)
class CameraTracker:
def __init__(self,
camera_parameters: CameraParameters,
corner_storage: CornerStorage,
frame_sequence_path: str,
known_view_1: Optional[Tuple[int, Pose]],
known_view_2: Optional[Tuple[int, Pose]]
):
self.rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path)
self.frame_count = len(self.rgb_sequence)
from _camtrack import (PointCloudBuilder, create_cli, calc_point_cloud_colors,
to_opencv_camera_mat3x3, view_mat3x4_to_pose,
pose_to_view_mat3x4,
rodrigues_and_translation_to_view_mat3x4,
triangulate_correspondences, TriangulationParameters,
build_correspondences, _remove_correspondences_with_ids,
compute_reprojection_errors, eye3x4)
from _corners import FrameCorners
from corners import CornerStorage
from data3d import CameraParameters, PointCloud, Pose
RANSAC_REPROJECTION_ERROR = 10
TRIANGULATION_REPROJECTION_ERROR = 10
MIN_TRISNGULATION_ANGLE = 1.5
INITIAL_TRIANGULATION_PARAMETERS = TriangulationParameters(
max_reprojection_error=TRIANGULATION_REPROJECTION_ERROR,
min_triangulation_angle_deg=1,
min_depth=0)
NEW_TRIANGULATION_PARAMETERS = {
1:
TriangulationParameters(
max_reprojection_error=TRIANGULATION_REPROJECTION_ERROR,
min_triangulation_angle_deg=MIN_TRISNGULATION_ANGLE,
min_depth=0),
2:
TriangulationParameters(
max_reprojection_error=TRIANGULATION_REPROJECTION_ERROR,
min_triangulation_angle_deg=MIN_TRISNGULATION_ANGLE,
min_depth=0),
4:
TriangulationParameters(
max_reprojection_error=5,
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])
frame_count = len(corner_storage)
view_mats = [None] * frame_count
if known_view_1 is None or known_view_2 is None:
known_view_1, known_view_2 = init_first_camera_positions(
intrinsic_mat, corner_storage)
id_1, view_mat_1 = known_view_1
id_2, view_mat_2 = known_view_2
view_mats[id_1], view_mats[id_2] = view_mat_1, view_mat_2
else:
id_1, pose_1 = known_view_1
id_2, pose_2 = known_view_2
view_mats[id_1] = pose_to_view_mat3x4(pose_1)
view_mats[id_2] = pose_to_view_mat3x4(pose_2)
params = TriangulationParameters(max_reprojection_error=2,
min_triangulation_angle_deg=1,
min_depth=0.5)
correspondences = build_correspondences(corner_storage[id_1],
corner_storage[id_2])
points, ids, _ = triangulate_correspondences(correspondences,
view_mats[id_1],
view_mats[id_2],
intrinsic_mat, params)
point_cloud_builder = PointCloudBuilder(ids.reshape(-1), points)
while True:
update = False
for i in range(frame_count):
if view_mats[i] is not None:
continue
corners = corner_storage[i]
ids, ids1, ids2 = np.intersect1d(corners.ids,
point_cloud_builder.ids,
return_indices=True)
points_2d, points_3d = corners.points[
ids1], point_cloud_builder.points[ids2]
if len(ids) < 6:
continue
retval, rvec, tvec, inliers = solvePnPRansac(points_3d,
points_2d,
intrinsic_mat,
distCoeffs=None)
if not retval:
continue
retval, rvec, tvec = solvePnP(points_3d[inliers],
points_2d[inliers],
intrinsic_mat,
distCoeffs=None,
rvec=rvec,
tvec=tvec,
useExtrinsicGuess=True)
if not retval:
continue
view_mats[i] = rodrigues_and_translation_to_view_mat3x4(rvec, tvec)
update = True
outliers = np.delete(ids, inliers)
for j in range(i):
if view_mats[j] is None:
continue
correspondences = build_correspondences(corners,
corner_storage[j],
ids_to_remove=outliers)
points, ids, _ = triangulate_correspondences(
correspondences, view_mats[i], view_mats[j], intrinsic_mat,
params)
point_cloud_builder.add_points(ids.reshape(-1), points)
click.echo("Process frame %d/%d. %d 3D points found. inliners=%d" %
(i + 1, frame_count, len(
point_cloud_builder.points), len(inliers)))
if not update:
break
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
from tqdm import tqdm as tqdm
from data3d import CameraParameters, PointCloud, Pose
import frameseq
import cv2
import pims
from sklearn.preprocessing import normalize
import sortednp as snp
from _camtrack import (PointCloudBuilder, create_cli, calc_point_cloud_colors,
to_opencv_camera_mat3x3, view_mat3x4_to_pose,
build_correspondences, pose_to_view_mat3x4,
triangulate_correspondences, TriangulationParameters,
rodrigues_and_translation_to_view_mat3x4)
INLIERS_MIN_SIZE = 0
TRIANG_PARAMS = TriangulationParameters(4, 1e-2, 1e-2)
DELTA = 7
MIN_SIZE = 20
def get_ransac(point_cloud_builder, corners_i, intrinsic_mat):
intersection, (indexes_cloud, indexes_corners) = snp.intersect(
point_cloud_builder.ids.flatten(),
corners_i.ids.flatten(),
indices=True)
if len(intersection) < 6:
return False, None, None, None
try:
res_code, rvec, tvec, inliers = cv2.solvePnPRansac(
point_cloud_builder.points[indexes_cloud],
corners_i.points[indexes_corners],
filter_frame_corners,
)
INLINER_FREQUENCY_TRASHHOLD = 0.9
MIN_INLINER_FRAMES = 5
MIN_STARTING_POINTS = 5
ITERATIONS = 5
MAX_REPROJECTION_ERROR = 8
MAX_TRANSLATION = 3
FRAMES_STEP = 10
FRAMES_MIN_WINDOW = 5
FRAMES_MAX_WINDOW = 100
triang_params = TriangulationParameters(
max_reprojection_error=MAX_REPROJECTION_ERROR,
min_triangulation_angle_deg=1.,
min_depth=0.001)
def build_index_intersection(ids1, ids2):
_, intersection = snp.intersect(ids1.flatten(),
ids2.flatten(),
indices=True)
return intersection
def build_index_difference(ids1, ids2):
_, intersection = snp.intersect(ids1.flatten(),
ids2.flatten(),
indices=True)
mask = np.ones_like(ids1).astype(bool)
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]:
if known_view_1 is None or known_view_2 is None:
raise NotImplementedError()
rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path)
intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters,
rgb_sequence[0].shape[0])
i_1 = known_view_1[0]
i_2 = known_view_2[0]
print('Known frames are', i_1, 'and', i_2)
global INLIERS_MIN_SIZE, DELTA, MIN_SIZE, TRIANG_PARAMS
view_mat3x4_1 = pose_to_view_mat3x4(known_view_1[1])
view_mat3x4_2 = pose_to_view_mat3x4(known_view_2[1])
correspondences = build_correspondences(corner_storage[i_1],
corner_storage[i_2])
print(len(correspondences.ids))
points3d, corr_ids, median_cos = triangulate_correspondences(
correspondences, view_mat3x4_1, view_mat3x4_2, intrinsic_mat,
TRIANG_PARAMS)
view_mats, point_cloud_builder = [view_mat3x4_1 for _ in corner_storage
], PointCloudBuilder(
corr_ids.astype(np.int64), points3d)
err_indexes = set(range(len(corner_storage)))
err_indexes.remove(i_1)
err_indexes.remove(i_2)
res_code, rodrig, inliers_1, cloud_points = get_ransac(
point_cloud_builder, corner_storage[i_1], intrinsic_mat)
view_mats[i_1] = rodrig
res_code, rodrig, inliers_2, cloud_points = get_ransac(
point_cloud_builder, corner_storage[i_2], intrinsic_mat)
view_mats[i_2] = rodrig
INLIERS_MIN_SIZE = min(len(inliers_1), len(inliers_2)) - 20 * max(
len(corner_storage) // 100, 1)
descr_template = 'Point cloud calc - {} inliers, {} points found, cloud size is {}'
tqdm_iterator = tqdm(range(len(corner_storage) - 2),
total=len(corner_storage),
desc=descr_template.format('?', '?', '?'))
params = [
corner_storage, view_mats, point_cloud_builder, intrinsic_mat,
err_indexes, descr_template, tqdm_iterator
]
DELTA = 5 * max(len(corner_storage) // 100, 1)
MIN_SIZE = 20 * max(len(corner_storage) // 100, 1)
TRIANG_PARAMS = TriangulationParameters(2, 1e-2, 1e-2)
for i in range(i_1 + 1, i_2):
update(i, *params)
print('Points between handled')
DELTA = 20 * max(len(corner_storage) // 100, 1)
MIN_SIZE = 20 * max(len(corner_storage) // 100, 1)
TRIANG_PARAMS = TriangulationParameters(4, 1e-2, 1e-2)
for i in range(i_1, -1, -1):
if i in err_indexes:
update(i, *params)
for i in range(i_2, len(corner_storage)):
if i in err_indexes:
update(i, *params)
for i in range(i_1, -1, -1):
if i in err_indexes:
view_mats[i] = view_mats[i + 1]
for i in range(i_2, len(corner_storage)):
if i in err_indexes:
view_mats[i] = view_mats[i - 1]
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
pose_to_view_mat3x4,
to_opencv_camera_mat3x3,
view_mat3x4_to_pose,
TriangulationParameters,
triangulate_correspondences,
build_correspondences,
rodrigues_and_translation_to_view_mat3x4,
Correspondences,
project_points,
calc_inlier_indices,
compute_reprojection_errors,
)
from _corners import FrameCorners
triang_params = TriangulationParameters(max_reprojection_error=7.5,
min_triangulation_angle_deg=1.0,
min_depth=0.1)
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:
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]:
if known_view_1 is None or known_view_2 is None:
raise NotImplementedError()
rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path)
intrinsic_mat = to_opencv_camera_mat3x3(
camera_parameters,
rgb_sequence[0].shape[0]
)
# TODO: implement
frame_count = len(corner_storage)
view_mats = [None] * frame_count
view_1_id, view_1 = known_view_1[0], pose_to_view_mat3x4(known_view_1[1])
view_2_id, view_2 = known_view_2[0], pose_to_view_mat3x4(known_view_2[1])
view_mats[view_1_id] = view_1
view_mats[view_2_id] = view_2
correspondences = build_correspondences(corner_storage[view_1_id], corner_storage[view_2_id], None)
triangulation_parameters = TriangulationParameters(1, 5, 0)
points, ids, _ = triangulate_correspondences(correspondences, view_1, view_2,
intrinsic_mat, TriangulationParameters(0.1, 0.1, 0))
#corners_0 = corner_storage[0]
point_cloud_builder = PointCloudBuilder(ids, points)
unknown_frames = frame_count - 2
known_frames = {view_1_id, view_2_id}
while unknown_frames > 0:
for i in range(frame_count):
if view_mats[i] is None:
corners = corner_storage[i]
interesting_ids, in_corners, in_cloud = np.intersect1d(corners.ids.flatten(), point_cloud_builder.ids.flatten(), return_indices=True)
points_2d = corners.points[in_corners]
points_3d = point_cloud_builder.points[in_cloud]
if len(ids) < 3:
continue
method = cv2.SOLVEPNP_EPNP
if len(ids) == 3:
method = cv2.SOLVEPNP_P3P
retval, rvec, tvec, inliers = cv2.solvePnPRansac(points_3d, points_2d, intrinsic_mat, None,
flags=method)
if not retval:
continue
retval, rvec, tvec = cv2.solvePnP(points_3d[inliers], points_2d[inliers], intrinsic_mat, None,
rvec=rvec, tvec=tvec, useExtrinsicGuess=True)
if not retval:
continue
view_mats[i] = rodrigues_and_translation_to_view_mat3x4(rvec, tvec)
unknown_frames -= 1
known_frames.add(i)
outliers = np.delete(interesting_ids, inliers)
for frame in known_frames:
correspondences = build_correspondences(corners, corner_storage[frame], outliers)
points_3d, corner_ids, _ = triangulate_correspondences(
correspondences, view_mats[i], view_mats[frame], intrinsic_mat, triangulation_parameters)
point_cloud_builder.add_points(corner_ids, points_3d)
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