def track_camera(corner_storage: CornerStorage,
intrinsic_mat: np.ndarray,
triangulation_parameters: TriangulationParameters,
known_view_1: Tuple[int, Pose],
known_view_2: Tuple[int, Pose]) \
-> Tuple[np.ndarray, PointCloudBuilder]:
track = [None for _ in range(len(corner_storage))]
points = [None for _ in range(corner_storage.max_corner_id() + 1)]
init_from_known_views(known_view_1, known_view_2, track, points,
corner_storage, intrinsic_mat,
triangulation_parameters)
has_new_track = True
while has_new_track:
has_new_track = False
for i in range(len(track)):
if track[i] is None:
is_successful = track_frame(i, track, points, corner_storage,
intrinsic_mat,
triangulation_parameters)
if is_successful:
has_new_track = True
for i in range(1, len(track)):
if track[i] is None:
track[i] = track[i - 1]
view_mats = np.array(track)
point_cloud_builder = PointCloudBuilder(
ids=np.array([i for i in range(len(points)) if points[i] is not None]),
points=np.array(
[points[i] for i in range(len(points)) if points[i] is not None]))
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])
known_view_1 = None
known_view_2 = None
if known_view_1 is None or known_view_2 is None:
track = [None] * len(corner_storage)
i, j = initialize(corner_storage, track, intrinsic_mat)
poses = add_points(corner_storage, track, intrinsic_mat,
[None] * (corner_storage.max_corner_id() + 1), i, j)
else:
track = init_track_with_known_views(corner_storage, known_view_1,
known_view_2)
poses = add_points(corner_storage, track, intrinsic_mat,
[None] * (corner_storage.max_corner_id() + 1),
known_view_1[0], known_view_2[0])
track, poses = track_(len(corner_storage), track, poses, intrinsic_mat,
corner_storage)
point_cloud_builder = PointCloudBuilder(
ids=np.array([i for i, point in enumerate(poses)
if point is not None]),
points=np.array([point for point in poses if point is not None]))
view_mats = np.array(track)
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 calc_point_cloud_colors(pc_builder: PointCloudBuilder,
rgb_sequence: pims.FramesSequence,
view_mats: List[np.ndarray],
intrinsic_mat: np.ndarray,
corner_storage: CornerStorage,
max_reproj_error: float) -> None:
# pylint:disable=too-many-arguments
# pylint:disable=too-many-locals
point_cloud_points = np.zeros((corner_storage.max_corner_id() + 1, 3))
point_cloud_points[pc_builder.ids.flatten()] = pc_builder.points
color_sums = np.zeros_like(point_cloud_points)
color_counts = np.zeros_like(color_sums)
with click.progressbar(zip(rgb_sequence, view_mats, corner_storage),
label='Calculating colors',
length=len(view_mats)) as progress_bar:
for image, view, corners in progress_bar:
try:
proj_mat = intrinsic_mat @ view
except Exception as e:
print(intrinsic_mat)
print(view)
raise e
points3d = point_cloud_points[corners.ids.flatten()]
with np.errstate(invalid='ignore'):
errors = compute_reprojection_errors(points3d, corners.points,
proj_mat)
errors = np.nan_to_num(errors)
consistency_mask = (
(errors <= max_reproj_error) & (corners.points[:, 0] >= 0) &
(corners.points[:, 1] >= 0) &
(corners.points[:, 0] < image.shape[1] - 0.5) &
(corners.points[:, 1] < image.shape[0] - 0.5)).flatten()
ids_to_process = corners.ids[consistency_mask].flatten()
corner_points = np.round(corners.points[consistency_mask]).astype(
np.int32)
rows = corner_points[:, 1].flatten()
cols = corner_points[:, 0].flatten()
color_sums[ids_to_process] += image[rows, cols]
color_counts[ids_to_process] += 1
nonzero_mask = (color_counts[:, 0] != 0).flatten()
color_sums[nonzero_mask] /= color_counts[nonzero_mask]
colors = color_sums[pc_builder.ids.flatten()]
pc_builder.set_colors(colors)
def __init__(self, corner_storage: CornerStorage,
intrinsic_mat: np.ndarray,
parameters: TriangulationParameters):
self._corner_storage = corner_storage
self._intrinsic_mat = intrinsic_mat
self._triangulation_parameters = parameters
self._n_frames = len(corner_storage)
self._track = [None] * self._n_frames
self._point_positions = [None] * (corner_storage.max_corner_id() + 1)
frame_ind1, frame_ind2 = self._initialization()
# print(frame_ind1, frame_ind2)
# print(self._n_frames)
self._add_cloud_points(frame_ind1, frame_ind2)
self._tracking()
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])
view_mats = np.zeros((len(corner_storage), 3, 4), dtype=np.float64)
processed_frames = np.zeros(len(corner_storage), dtype=np.bool)
points3d = np.zeros((corner_storage.max_corner_id() + 1, 3),
dtype=np.float64)
added_points = np.zeros(corner_storage.max_corner_id() + 1, dtype=np.bool)
print('Trying to extract 3d points from known frames...')
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])
processed_frames[known_view_1[0]] = processed_frames[
known_view_2[0]] = True
extracted_points = extract_points(corner_storage[known_view_1[0]],
corner_storage[known_view_2[0]],
view_mats[known_view_1[0]],
view_mats[known_view_2[0]],
intrinsic_mat)
if not extracted_points:
print(
'Extracting 3d points from known frames failed: '
'either there are no common points, or triangulation angle between frames is too small.\n'
'Try to choose another initial frames.',
file=sys.stderr)
exit(0)
print('Succeeded! Trying to build point cloud...')
added_points[extracted_points[0]] = True
points3d[extracted_points[0]] = extracted_points[1]
was_updated = True
while was_updated:
was_updated = False
unprocessed_frames = np.arange(len(corner_storage),
dtype=np.int32)[~processed_frames]
for frame in unprocessed_frames:
points3d_ids = np.arange(corner_storage.max_corner_id() + 1,
dtype=np.int32)[added_points]
common, (indices1, indices2) = snp.intersect(
points3d_ids,
corner_storage[frame].ids.flatten(),
indices=True)
if len(common) <= 5:
continue
try:
print(f'Processing frame {frame}: ', end='')
retval, rvec, tvec, inliers = cv2.solvePnPRansac(
objectPoints=points3d[common],
imagePoints=corner_storage[frame].points[indices2],
cameraMatrix=intrinsic_mat,
distCoeffs=None)
except:
retval = False
if not retval:
print(f'failed to solve PnP RANSAC, trying another frame')
continue
print(f'succeeded, found {len(inliers)} inliers')
was_updated = True
view_mats[frame] = rodrigues_and_translation_to_view_mat3x4(
rvec, tvec)
for processed_frame in np.arange(len(corner_storage),
dtype=np.int32)[processed_frames]:
extracted_points = extract_points(
corner_storage[frame], corner_storage[processed_frame],
view_mats[frame], view_mats[processed_frame],
intrinsic_mat, points3d_ids)
if extracted_points:
added_points[extracted_points[0]] = True
points3d[extracted_points[0]] = extracted_points[1]
processed_frames[frame] = True
print(f'Current point cloud contains {sum(added_points)} points')
for _ in range(2):
for i in range(1, len(corner_storage)):
if not processed_frames[i]:
processed_frames[i] = True
view_mats[i] = view_mats[i - 1]
processed_frames = processed_frames[::-1]
view_mats = view_mats[::-1]
print(
f'Finished building point cloud, now it contains {sum(added_points)} points'
)
point_cloud_builder = PointCloudBuilder(ids=np.arange(
corner_storage.max_corner_id() + 1, dtype=np.int32)[added_points],
points=points3d[added_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
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 = initial_camera_views(
corner_storage, intrinsic_mat)
print(known_view_1[1])
print(known_view_2[1])
print(known_view_1[0], known_view_2[0])
print(intrinsic_mat)
points_3d, points_ids = calc_starting_points(intrinsic_mat, corner_storage,
known_view_1, known_view_2)
#initial_points_3d, initial_points_ids = points_3d.copy(), points_ids.copy()
n_points = corner_storage.max_corner_id() + 1
res_points_3d = np.full((n_points, 3), None)
#res_points_3d[points_ids] = points_3d
view_mats = [np.full((3, 4), None) for _ in range(len(corner_storage))]
view_mats[known_view_1[0]], view_mats[
known_view_2[0]] = pose_to_view_mat3x4(
known_view_1[1]), pose_to_view_mat3x4(known_view_2[1])
print(f'len(corner_storage):{len(corner_storage)}')
id1 = known_view_1[0]
for n_frame, corners in enumerate(corner_storage[id1 + 1:], id1 + 1):
common_obj, common_img, common_ids = get_common_points(
corners, points_ids, points_3d)
mat, points_3d, points_ids, reprojection_error = build_view_mat(
common_obj, common_img, common_ids, intrinsic_mat)
points_ids = np.array(points_ids)
view_mats[n_frame] = mat
is_null = np.array([(x[0] is None) for x in res_points_3d[points_ids]])
#res_points_3d[points_ids[is_null]] = points_3d[is_null]
n_inliers = len(points_3d)
points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat,
corner_storage, view_mats,
points_3d, points_ids)
print(
f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. "
f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}"
)
for n_frame, corners in list(enumerate(corner_storage))[::-1]:
common_obj, common_img, common_ids = get_common_points(
corners, points_ids, points_3d)
mat, points_3d, points_ids, reprojection_error = build_view_mat(
common_obj, common_img, common_ids, intrinsic_mat)
view_mats[n_frame] = mat
is_null = np.array([(x[0] is None) for x in res_points_3d[points_ids]])
#res_points_3d[points_ids[is_null]] = points_3d[is_null]
n_inliers = len(points_3d)
points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat,
corner_storage, view_mats,
points_3d, points_ids)
print(
f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. "
f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}"
)
# Approximating
n_iter = 1
for iter in range(n_iter):
for n_frame, corners in enumerate(corner_storage):
common_obj, common_img, common_ids = get_common_points(
corners, points_ids, points_3d)
mat, points_3d, points_ids, reprojection_error = build_view_mat(
common_obj, common_img, common_ids, intrinsic_mat)
view_mats[n_frame] = mat
if iter == n_iter - 1:
res_points_3d[points_ids] = points_3d
n_inliers = len(points_3d)
points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat,
corner_storage, view_mats,
points_3d, points_ids)
print(
f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. "
f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}"
)
for n_frame, corners in list(enumerate(corner_storage))[::-1]:
common_obj, common_img, common_ids = get_common_points(
corners, points_ids, points_3d)
mat, points_3d, points_ids, reprojection_error = build_view_mat(
common_obj, common_img, common_ids, intrinsic_mat)
view_mats[n_frame] = mat
if iter == n_iter - 1:
res_points_3d[points_ids] = points_3d
n_inliers = len(points_3d)
points_3d, points_ids = calc_new_points(n_frame, intrinsic_mat,
corner_storage, view_mats,
points_3d, points_ids)
print(
f"Processing frame #{n_frame}. Number of inliers: {n_inliers}. "
f"Reprojection error: {reprojection_error}. Tracking points: {len(common_img)}"
)
res_points_ids = np.array(
[i for i, x in enumerate(res_points_3d) if x[0] is not None])
res_points_3d = np.array(res_points_3d[res_points_ids], dtype=float)
point_cloud_builder = PointCloudBuilder(ids=res_points_ids,
points=res_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