def pyransaclib_localize_from_loaded_data(
kapture_data: kapture.Kapture, kapture_path: str,
tar_handlers: TarCollection, kapture_query_data: kapture.Kapture,
output_path: str, pairsfile_path: str, inlier_threshold: float,
number_lo_steps: int, min_num_iterations: int, max_num_iterations: int,
refine_poses: bool, keypoints_type: Optional[str],
duplicate_strategy: DuplicateCorrespondencesStrategy,
rerank_strategy: RerankCorrespondencesStrategy,
write_detailed_report: bool, force: bool) -> None:
"""
Localize images using pyransaclib.
:param kapture_data: loaded kapture data (incl. points3d)
:param kapture_path: path to the kapture to use
:param tar_handlers: collection of pre-opened tar archives
:param kapture_data: loaded kapture data (mapping and query images)
:param output_path: path to the write the localization results
:param pairsfile_path: pairs to use
:param inlier_threshold: RANSAC inlier threshold in pixel
:param number_lo_steps: number of local optimization iterations in LO-MSAC. Use 0 to use MSAC
:param min_num_iterations: minimum number of ransac loops
:param max_num_iterations: maximum number of ransac loops
:param refine_poses: refine poses with pycolmap
:param keypoints_type: types of keypoints (and observations) to use
:param force: Silently overwrite kapture files if already exists.
"""
assert has_pyransaclib
if refine_poses:
assert has_pycolmap
if not (kapture_data.records_camera and kapture_data.sensors
and kapture_data.keypoints and kapture_data.matches
and kapture_data.points3d and kapture_data.observations):
raise ValueError('records_camera, sensors, keypoints, matches, '
'points3d, observations are mandatory for map+query')
if not (kapture_query_data.records_camera and kapture_query_data.sensors):
raise ValueError('records_camera, sensors are mandatory for query')
if keypoints_type is None:
keypoints_type = try_get_only_key_from_collection(
kapture_data.keypoints)
assert keypoints_type is not None
assert keypoints_type in kapture_data.keypoints
assert keypoints_type in kapture_data.matches
if kapture_data.rigs is not None and kapture_data.trajectories is not None:
# make sure, rigs are not used in trajectories.
logger.info('remove rigs notation.')
rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs)
kapture_data.rigs.clear()
if kapture_query_data.trajectories is not None:
logger.warning(
"Input query data contains trajectories: they will be ignored")
kapture_query_data.trajectories.clear()
os.umask(0o002)
os.makedirs(output_path, exist_ok=True)
delete_existing_kapture_files(output_path, force_erase=force)
# load pairsfile
pairs = {}
with open(pairsfile_path, 'r') as fid:
table = kapture.io.csv.table_from_file(fid)
for img_query, img_map, _ in table:
if img_query not in pairs:
pairs[img_query] = []
pairs[img_query].append(img_map)
kapture_data.matches[keypoints_type].normalize()
keypoints_filepaths = keypoints_to_filepaths(
kapture_data.keypoints[keypoints_type], keypoints_type, kapture_path,
tar_handlers)
obs_for_keypoints_type = {
point_id: per_keypoints_type_subdict[keypoints_type]
for point_id, per_keypoints_type_subdict in
kapture_data.observations.items()
if keypoints_type in per_keypoints_type_subdict
}
point_id_from_obs = {
(img_name, kp_id): point_id
for point_id in obs_for_keypoints_type.keys()
for img_name, kp_id in obs_for_keypoints_type[point_id]
}
query_images = [(timestamp, sensor_id, image_name)
for timestamp, sensor_id, image_name in kapture.flatten(
kapture_query_data.records_camera)]
# kapture for localized images + pose
trajectories = kapture.Trajectories()
progress_bar = tqdm(total=len(query_images),
disable=logging.getLogger().level >= logging.CRITICAL)
for timestamp, sensor_id, image_name in query_images:
if image_name not in pairs:
continue
keypoints_filepath = keypoints_filepaths[image_name]
kapture_keypoints_query = image_keypoints_from_file(
filepath=keypoints_filepath,
dsize=kapture_data.keypoints[keypoints_type].dsize,
dtype=kapture_data.keypoints[keypoints_type].dtype)
query_cam = kapture_query_data.sensors[sensor_id]
assert isinstance(query_cam, kapture.Camera)
num_keypoints = kapture_keypoints_query.shape[0]
kapture_keypoints_query, K, distortion = get_camera_matrix_from_kapture(
kapture_keypoints_query, query_cam)
kapture_keypoints_query = kapture_keypoints_query.reshape(
(num_keypoints, 2))
cv2_keypoints_query = np.copy(kapture_keypoints_query)
if np.count_nonzero(distortion) > 0:
epsilon = np.finfo(np.float64).eps
stop_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
cv2.TERM_CRITERIA_EPS, 500, epsilon)
cv2_keypoints_query = cv2.undistortPointsIter(
cv2_keypoints_query,
K,
distortion,
R=None,
P=K,
criteria=stop_criteria)
cv2_keypoints_query = cv2_keypoints_query.reshape((num_keypoints, 2))
# center keypoints
for i in range(cv2_keypoints_query.shape[0]):
cv2_keypoints_query[i, 0] = cv2_keypoints_query[i, 0] - K[0, 2]
cv2_keypoints_query[i, 1] = cv2_keypoints_query[i, 1] - K[1, 2]
kpts_query = kapture_keypoints_query if (
refine_poses or write_detailed_report) else None
points2D, points2D_undistorted, points3D, stats = get_correspondences(
kapture_data, keypoints_type, kapture_path, tar_handlers,
image_name, pairs[image_name], point_id_from_obs, kpts_query,
cv2_keypoints_query, duplicate_strategy, rerank_strategy)
# compute absolute pose
# inlier_threshold - RANSAC inlier threshold in pixels
# answer - dictionary containing the RANSAC output
ret = pyransaclib.ransaclib_localization(image_name, K[0, 0], K[1, 1],
points2D_undistorted,
points3D, inlier_threshold,
number_lo_steps,
min_num_iterations,
max_num_iterations)
# add pose to output kapture
if ret['success'] and ret['num_inliers'] > 0:
pose = kapture.PoseTransform(ret['qvec'], ret['tvec'])
if refine_poses:
inlier_mask = np.zeros((len(points2D), ), dtype=bool)
inlier_mask[ret['inliers']] = True
inlier_mask = inlier_mask.tolist()
col_cam_id, width, height, params, _ = get_colmap_camera(
query_cam)
cfg = {
'model': CAMERA_MODEL_NAME_ID[col_cam_id][0],
'width': int(width),
'height': int(height),
'params': params
}
ret_refine = pycolmap.pose_refinement(pose.t_raw, pose.r_raw,
points2D, points3D,
inlier_mask, cfg)
if ret_refine['success']:
pose = kapture.PoseTransform(ret_refine['qvec'],
ret_refine['tvec'])
logger.debug(
f'{image_name} refinement success, new pose: {pose}')
if write_detailed_report:
reprojection_error = compute_reprojection_error(
pose, ret['num_inliers'], ret['inliers'], points2D,
points3D, K, distortion)
cache = {
"num_correspondences": len(points3D),
"num_inliers": ret['num_inliers'],
"inliers": ret['inliers'],
"reprojection_error": reprojection_error,
"stats": stats
}
cache_path = os.path.join(
output_path, f'pyransaclib_cache/{image_name}.json')
save_to_json(cache, cache_path)
trajectories[timestamp, sensor_id] = pose
progress_bar.update(1)
progress_bar.close()
kapture_data_localized = kapture.Kapture(
sensors=kapture_query_data.sensors,
trajectories=trajectories,
records_camera=kapture_query_data.records_camera,
rigs=kapture_query_data.rigs)
kapture.io.csv.kapture_to_dir(output_path, kapture_data_localized)
# Adding New axis to the matched points. This is done so that the points have a dimension of 1 x N x 2.
# This dimension is required because the next opencv function used in line 70 takes these points as the input with this
# particular dimension of 1 x N x 2
matched_L_na = matched_L[:, np.newaxis, :]
matched_R_na = matched_R[:, np.newaxis, :]
# Undistorting the matched key points
# The next step is to multiply the matched keypoints with the inverse camera matrix, undidtort the points and then
# multiply again with the camera matrix.
# In the case of lens distortion, the equations are non-linear and depend on 3 to 8 parameters (k1 to k6, p1 and p2).
# Hence, it would normally require a non-linear solving algorithm (e.g. Newton's method, Levenberg-Marquardt algorithm,
# etc) to inverse such a model and estimate the undistorted coordinates from the distorted ones.
# And this is what is used behind function undistortPoints, with tuned parameters making the optimization fast but a
# little inaccurate.
# The explaination of the function and its input arguments are given in the report
undist_pts_L = cv.undistortPointsIter(matched_L_na, k, dist, R = None, P = newK, criteria=crit_cal)
undist_pts_R = cv.undistortPointsIter(matched_R_na, k, dist, R = None, P = newK, criteria=crit_cal)
print('Shape of the returned Undistorted points: ', undist_pts_L.shape)
# Computing the Fundamental Matrix.
# The fundamental matrix maps points in one stereo image to epipolar lines in the other. It can be computed usingcorresponding
# points in two images recovered in the feature matching stage. In particular, cv2.findFundamentalMat() implements just
# this approach.
# The undistorted keypoints are used for calculating the fundamental matrix. This not only reverses the effect of lens
# distortion, but also transforms the coordinates to normalized image coordinates. Normalized image coordinates
# (not to be confused with the normalization done in the 8-point algorithm) are camera agnostic coordinates that do
# not depend on any of the camera intrinsic parameters. They represent the angle of the bearing vector to the point
# in the real world. For example, a normalized image coordinate of (1, 0) would correspond to a bearing angle of 45
# N x 1 x 2
matched_kps_left = [kp_left[mat.queryIdx].pt for mat in matches]
matched_kps_right = [kp_right[mat.trainIdx].pt for mat in matches]
matched_kps_left = np.array(matched_kps_left)
matched_kps_right = np.array(matched_kps_right)
matched_kps_left = matched_kps_left[:, np.newaxis, :]
matched_kps_right = matched_kps_right[:, np.newaxis, :]
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
# Undistort the image points
pts_left_undistorted = cv2.undistortPointsIter(matched_kps_left,
cameraMatrix,
distCoeffs,
R=None,
P=newK,
criteria=criteria)
pts_right_undistorted = cv2.undistortPointsIter(matched_kps_right,
cameraMatrix,
distCoeffs,
R=None,
P=newK,
criteria=criteria)
# Compute the fundamental matrix
F, mask = cv2.findFundamentalMat(pts_left_undistorted,
pts_right_undistorted,
method=cv2.RANSAC,
ransacReprojThreshold=0.5,
confidence=.99)
def create_3D_model_from_depth_from_loaded_data(
kdata: kapture.Kapture, input_path: str, tar_handlers: TarCollection,
output_path: str, keypoints_type: Optional[str], depth_sensor_id: str,
topk: int, method: Method, cellsizes: List[str], force: bool):
"""
Create 3D model from a kapture dataset that has registered depth data
Assumes the kapture data is already loaded
"""
logger.info(f'create 3D model using depth data')
if os.path.exists(output_path) and not force:
print(f'outpath already exists, use --force to overwrite')
return -1
if kdata.rigs is not None:
assert kdata.trajectories is not None
kapture.rigs_remove_inplace(kdata.trajectories, kdata.rigs)
if keypoints_type is None:
keypoints_type = try_get_only_key_from_collection(kdata.keypoints)
assert keypoints_type is not None
assert kdata.keypoints is not None
assert keypoints_type in kdata.keypoints
if method == Method.voxelgrid:
vg = VoxelGrid(cellsizes)
# add all 3D points to map that correspond to a keypoint
logger.info('adding points from scan to kapture')
points3d = []
observations = kapture.Observations()
progress_bar = tqdm(total=len(
list(kapture.flatten(kdata.records_camera, is_sorted=True))),
disable=logger.level >= logging.CRITICAL)
for timestamp, sensor_id, sensing_filepath in kapture.flatten(
kdata.records_camera, is_sorted=True):
logger.info(
f'total 3d points: {len(points3d)}, processing {sensing_filepath}')
# check if images have a pose
if timestamp not in kdata.trajectories:
logger.info('{} does not have a pose. skipping ...'.format(
sensing_filepath))
continue
# check if depth map exists
depth_map_record = ''
if timestamp in kdata.records_depth:
if depth_sensor_id is None:
depth_id = sensor_id + '_depth'
else:
depth_id = depth_sensor_id
if depth_id in kdata.records_depth[timestamp]:
depth_map_record = kdata.records_depth[timestamp][depth_id]
depth_map_size = tuple(
[int(x) for x in kdata.sensors[depth_id].camera_params[0:2]])
depth_path = get_depth_map_fullpath(input_path, depth_map_record)
if not os.path.exists(depth_path):
logger.info('no 3D data found for {}. skipping ...'.format(
sensing_filepath))
continue
depth_map = depth_map_from_file(depth_path, depth_map_size)
img = Image.open(get_image_fullpath(input_path,
sensing_filepath)).convert('RGB')
assert img.size[0] == depth_map_size[0]
assert img.size[1] == depth_map_size[1]
kps_raw = load_keypoints(keypoints_type, input_path, sensing_filepath,
kdata.keypoints[keypoints_type].dtype,
kdata.keypoints[keypoints_type].dsize,
tar_handlers)
_, camera_sensor_C, camera_dist = get_camera_matrix_from_kapture(
np.zeros((1, 0, 2), dtype=np.float64), kdata.sensors[sensor_id])
cv2_keypoints, depth_sensor_C, depth_dist = get_camera_matrix_from_kapture(
kps_raw, kdata.sensors[depth_id])
assert np.isclose(depth_sensor_C, camera_sensor_C).all()
assert np.isclose(depth_dist, camera_dist).all()
if np.count_nonzero(camera_dist) > 0:
epsilon = np.finfo(np.float64).eps
stop_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
cv2.TERM_CRITERIA_EPS, 500, epsilon)
undistorted_cv2_keypoints = cv2.undistortPointsIter(
cv2_keypoints,
camera_sensor_C,
camera_dist,
R=None,
P=camera_sensor_C,
criteria=stop_criteria)
else:
undistorted_cv2_keypoints = cv2_keypoints
cv2_keypoints = cv2_keypoints.reshape((kps_raw.shape[0], 2))
undistorted_cv2_keypoints = undistorted_cv2_keypoints.reshape(
(kps_raw.shape[0], 2))
points3d_img = []
rgb_img = []
kp_idxs = []
for idx_kp, kp in enumerate(cv2_keypoints[0:topk]):
u = round(kp[0])
v = round(kp[1])
undist_kp = undistorted_cv2_keypoints[idx_kp]
undist_u = round(undist_kp[0])
undist_v = round(undist_kp[1])
if u >= 0 and u < depth_map_size[
0] and v >= 0 and v < depth_map_size[1]:
if depth_map[v, u] == 0:
continue
pt3d = project_kp_to_3D(undist_u, undist_v, depth_map[v, u],
depth_sensor_C[0,
2], depth_sensor_C[1,
2],
depth_sensor_C[0,
0], depth_sensor_C[1,
1])
points3d_img.append(pt3d)
rgb_img.append(img.getpixel((u, v)))
kp_idxs.append(idx_kp)
# transform to world coordinates (pt3d from a depth map is in camera coordinates)
# we use sensor_id here because we assume that the image and the corresponding depthmap have the same pose
# and sometimes, the pose might only be provided for the images
cam_to_world = kdata.trajectories[timestamp][sensor_id].inverse()
if len(points3d_img) == 0:
continue
points3d_img = cam_to_world.transform_points(np.array(points3d_img))
for idx_kp, pt3d, rgb in zip(kp_idxs, points3d_img, rgb_img):
if not np.isnan(pt3d).any():
# apply transform (alignment)
if method == Method.voxelgrid:
assert vg is not None
if not vg.exists(pt3d):
# add 3D point
points3d.append(list(pt3d) + list(rgb))
# add observation
observations.add(
len(points3d) - 1, keypoints_type,
sensing_filepath, idx_kp)
vg.add(pt3d, len(points3d) - 1, sensing_filepath)
else:
ret = vg.append(pt3d, sensing_filepath)
if ret is not None:
observations.add(ret[0], keypoints_type,
sensing_filepath, idx_kp)
elif method == Method.all:
# add 3D point
points3d.append(list(pt3d) + list(rgb))
# add observation
observations.add(
len(points3d) - 1, keypoints_type, sensing_filepath,
idx_kp)
# save_3Dpts_to_ply(points3d, os.path.join(output_path, 'map.ply'))
progress_bar.update(1)
progress_bar.close()
kdata.points3d = kapture.Points3d(np.array(points3d))
kdata.observations = observations
logger.info('saving ...')
kapture_to_dir(output_path, kdata)
# save_3Dpts_to_ply(points3d, os.path.join(output_path, 'map.ply'))
logger.info('all done')
i1_udist = img_undistort(f_camera, i1)[1]
i2_udist = img_undistort(f_camera, i2)[1]
i1_udistcp = img_undistort(f_camera, i1)[1]
i2_udistcp = img_undistort(f_camera, i2)[1]
# # Undistort matched kps, adopt the procedure shown in class by prof.Peters
cdt_kp1 = cdt_kp1[:, np.
newaxis, :] # Possibly convert kp array format to 40(n) x 1 x 2
cdt_kp2 = cdt_kp2[:, np.
newaxis, :] # I have to use subpix cdt of kps (no 'int'), otherwise I will meet depth error
criteria_cal = (cv.TERM_CRITERIA_EPS + cv.TermCriteria_MAX_ITER, 30, 1e-6)
cdt_kp1_udist = cv.undistortPointsIter(cdt_kp1,
new_kmat,
dist,
R=None,
P=new_kmat,
criteria=criteria_cal)
cdt_kp2_udist = cv.undistortPointsIter(cdt_kp2,
new_kmat,
dist,
R=None,
P=new_kmat,
criteria=criteria_cal)
# It seems that printed cdts are reasonable
# print(cdt_kp1_udist)
# print(cdt_kp2_udist)
# auxiliary function to draw undistorted key pts from coordinates
i1_udist_temp = img_undistort(f_camera, i1)[1]
i2_udist_temp = img_undistort(f_camera, i2)[1]