def main(setup='setup.json',
relative_poses='relative_poses.json',
method='cross-ratios',
dump_images=True,
output_path="./multiview_calib/examples/pose/output/relative_poses/"):
utils.config_logger(os.path.join(".", "concat_relative_poses.log"))
setup = utils.json_read(setup)
relative_poses = utils.json_read(relative_poses)
relative_poses = utils.dict_keys_from_literal_string(relative_poses)
if not verify_view_tree(setup['minimal_tree']):
raise ValueError("minimal_tree is not a valid tree!")
poses, triang_points = concatenate_relative_poses(setup['minimal_tree'],
relative_poses, method)
path = output_path if dump_images else None
visualise_cameras_and_triangulated_points(setup['views'],
setup['minimal_tree'],
poses,
triang_points,
max_points=100,
path=path)
utils.json_write("./multiview_calib/examples/pose/input/poses.json", poses)
triang_points = utils.dict_keys_to_string(triang_points)
utils.json_write(
"./multiview_calib/examples/pose/input/triang_points.json",
triang_points)
def main(setup='setup.json',
intrinsics='intrinsics.json',
landmarks='landmarks.json',
filenames='filenames.json',
method='8point',
th=20,
max_paths=5,
method_scale='cross-ratios',
dump_images=True,
output_path="output/relative_poses/"):
utils.config_logger(os.path.join(".", "relative_poses_robust.log"))
setup = utils.json_read(setup)
intrinsics = utils.json_read(intrinsics)
landmarks = utils.json_read(landmarks)
if not verify_view_tree(setup['minimal_tree']):
raise ValueError("minimal_tree is not a valid tree!")
res, msg = verify_landmarks(landmarks)
if not res:
raise ValueError(msg)
relative_poses = compute_relative_poses_robust(setup['views'],
setup['minimal_tree'],
intrinsics,
landmarks,
method=method,
th=th,
max_paths=max_paths,
verbose=1)
if dump_images:
visualise_epilines(setup['minimal_tree'],
relative_poses,
intrinsics,
landmarks,
filenames,
output_path=output_path)
relative_poses = utils.dict_keys_to_string(relative_poses)
utils.json_write("relative_poses.json", relative_poses)
def main(setup='setup.json',
intrinsics='intrinsics.json',
landmarks='landmarks.json',
filenames='filenames.json',
method='8point',
dump_images=True,
th=20,
output_path="./multiview_calib/examples/pose/relative_poses/"):
utils.config_logger(os.path.join(".", "relative_poses.log"))
setup = utils.json_read(setup)
intrinsics = utils.json_read(intrinsics)
landmarks = utils.json_read(landmarks)
if not verify_view_tree(setup['minimal_tree']):
raise ValueError("minimal_tree is not a valid tree!")
res, msg = verify_landmarks(landmarks)
if not res:
raise ValueError(msg)
relative_poses = compute_relative_poses(setup['minimal_tree'],
intrinsics,
landmarks,
method,
th,
verbose=2)
if dump_images:
visualise_epilines(setup['minimal_tree'],
relative_poses,
intrinsics,
landmarks,
filenames,
output_path=output_path)
relative_poses = utils.dict_keys_to_string(relative_poses)
utils.json_write(
"./multiview_calib/examples/pose/input/relative_poses.json",
relative_poses)
def main(setup='setup.json',
ba_poses='ba_poses.json',
ba_points='ba_points.json',
landmarks='landmarks.json',
landmarks_global='landmarks_global.json',
dump_images=True,
filenames="filenames.json",
output_path="output/global_registration"):
utils.config_logger(os.path.join(".", "global_registration.log"))
setup = utils.json_read(setup)
ba_poses = utils.json_read(ba_poses)
ba_points = utils.json_read(ba_points)
landmarks = utils.json_read(landmarks)
landmarks_global = utils.json_read(landmarks_global)
global_poses, global_triang_points = global_registration(ba_poses, ba_points, landmarks_global)
if dump_images:
filenames = utils.json_read(filenames)
visualise_global_registration(global_poses, landmarks_global, ba_poses, ba_points,
filenames, output_path=output_path)
utils.json_write("global_poses.json", global_poses)
utils.json_write("global_triang_points.json", global_triang_points)
avg_dist, std_dist, median_dist = error_measure(setup, landmarks, global_poses, global_triang_points,
scale=1, view_limit_triang=5)
logging.info("Per pair of view average error:")
logging.info("\t mean+-std: {:0.3f}+-{:0.3f} [unit of destination (dst) point set]".format(avg_dist, std_dist))
logging.info("\t median: {:0.3f} [unit of destination (dst) point set]".format(median_dist))
def main(poses='poses.json', landmarks='landmarks.json', dump_images=True):
if dump_images:
utils.mkdir(__config__["output_path"])
poses = utils.json_read(poses)
landmarks = utils.json_read(landmarks)
intrinsics = {
view: {
'K': data['K'],
'dist': data['dist']
}
for view, data in poses.items()
}
extrinsics = {
view: {
'R': data['R'],
't': data['t']
}
for view, data in poses.items()
}
res, msg = verify_landmarks(landmarks)
if not res:
raise ValueError(msg)
views = list(landmarks.keys())
print("-" * 20)
print("Views: {}".format(views))
print(
"Triangulate image points...(this can take a while depending on the number of points to triangulate)"
)
start = time.time()
camera_params, points_3d, points_2d,\
camera_indices, point_indices, \
n_cameras, n_points, ids = build_input(views, intrinsics, extrinsics,
landmarks, each=1,
view_limit_triang=4)
print("Elapsed time: {:0.2f}s".format(time.time() - start))
print("Least-Squares optimization of the 3D points:")
print("\t ftol={:0.3e}".format(__config__["ftol"]))
print("\t xtol={:0.3e}".format(__config__["xtol"]))
print("\t loss={} f_scale={:0.2f}".format(__config__["loss"],
__config__['f_scale']))
print("\t max_nfev={}".format(__config__["max_nfev"]))
points_3d_ref = bundle_adjustment(camera_params,
points_3d,
points_2d,
camera_indices,
point_indices,
n_cameras,
n_points,
optimize_camera_params=False,
optimize_points=True,
ftol=__config__["ftol"],
xtol=__config__["xtol"],
loss=__config__['loss'],
f_scale=__config__['f_scale'],
max_nfev=__config__["max_nfev"],
bounds=True,
bounds_cp=__config__["bounds_cp"],
bounds_pt=__config__["bounds_pt"],
verbose=True,
eps=1e-12)
f1 = evaluate(camera_params, points_3d_ref, points_2d, camera_indices,
point_indices, n_cameras, n_points)
avg_abs_res = np.abs(f1).mean()
print("Average absolute residual: {:0.2f} over {} points.".format(
avg_abs_res,
len(f1) / 2))
if dump_images:
plt.figure()
plt.plot(f1)
plt.title("Residuals after optimization")
plt.ylabel("Residual [pixels]")
plt.xlabel("X and Y coordinates")
plt.show()
plt.savefig(os.path.join(__config__["output_path"],
"optimized_residuals.jpg"),
bbox_inches='tight')
plt.ylim(-10, 10)
plt.savefig(os.path.join(__config__["output_path"],
"optimized_residuals_ylim.jpg"),
bbox_inches='tight')
print("Reprojection errors (mean+-std pixels):")
for i, (view, cp) in enumerate(zip(views, camera_params)):
K, R, t, dist = unpack_camera_params(cp)
points3d = points_3d_ref[point_indices[camera_indices == i]]
points2d = points_2d[camera_indices == i]
mean_error, std_error = reprojection_error(R, t, K, dist, points3d,
points2d, 'mean')
print("\t {} n_points={}: {:0.3f}+-{:0.3f}".format(
view, len(points3d), mean_error, std_error))
print("Reprojection errors (median pixels):")
for i, (view, cp) in enumerate(zip(views, camera_params)):
K, R, t, dist = unpack_camera_params(cp)
points3d = points_3d_ref[point_indices[camera_indices == i]]
points2d = points_2d[camera_indices == i]
mean_error, std_error = reprojection_error(R, t, K, dist, points3d,
points2d, 'median')
print("\t {} n_points={}: {:0.3f}".format(view, len(points3d),
mean_error))
points = {
"points_3d": points_3d_ref.tolist(),
"ids": np.array(ids).tolist()
}
utils.json_write("triangulated_points.json", points)
def main(config=None,
setup='setup.json',
intrinsics='intrinsics.json',
extrinsics='poses.json',
landmarks='landmarks.json',
filenames='filenames.json',
iter1=200,
iter2=200,
dump_images=True,
name='default'):
utils.config_logger(os.path.join(".", "bundle_adjustment.log"))
if config is not None:
__config__ = utils.json_read(config)
if iter1 is not None:
__config__["max_nfev"] = iter1
if iter2 is not None:
__config__["max_nfev2"] = iter2
if dump_images:
utils.mkdir(__config__["output_path"])
setup = utils.json_read(setup)
intrinsics = utils.json_read(intrinsics)
extrinsics = utils.json_read(extrinsics)
landmarks = utils.json_read(landmarks)
filenames_images = utils.json_read(filenames)
if not verify_view_tree(setup['minimal_tree']):
raise ValueError("minimal_tree is not a valid tree!")
res, msg = verify_landmarks(landmarks)
if not res:
raise ValueError(msg)
views = setup['views']
logging.info("-"*20)
logging.info("Views: {}".format(views))
if __config__["each_training"]<2 or __config__["each_training"] is None:
logging.info("Use all the landmarks.")
else:
logging.info("Subsampling the landmarks to 1 every {}.".format(__config__["each_training"]))
logging.info("Preparing the input data...(this can take a while depending on the number of points to triangulate)")
start = time.time()
camera_params, points_3d, points_2d,\
camera_indices, point_indices, \
n_cameras, n_points, ids = build_input(views, intrinsics, extrinsics,
landmarks, each=__config__["each_training"],
view_limit_triang=4)
logging.info("The preparation of the input data took: {:0.2f}s".format(time.time()-start))
logging.info("Sizes:")
logging.info("\t camera_params: {}".format(camera_params.shape))
logging.info("\t points_3d: {}".format(points_3d.shape))
logging.info("\t points_2d: {}".format(points_2d.shape))
f0 = evaluate(camera_params, points_3d, points_2d,
camera_indices, point_indices,
n_cameras, n_points)
if dump_images:
plt.figure()
plt.plot(f0)
plt.title("Residuals at initialization")
plt.ylabel("Residual [pixels]")
plt.xlabel("X and Y coordinates")
plt.show()
plt.savefig(os.path.join(__config__["output_path"], "initial_residuals.jpg"), bbox_inches='tight')
if __config__["th_outliers_early"]==0 or __config__["th_outliers_early"] is None:
logging.info("No early outlier rejection.")
else:
logging.info("Early Outlier rejection:")
logging.info("\t threshold outliers: {}".format(__config__["th_outliers_early"]))
f0_ = np.abs(f0.reshape(-1,2))
mask_outliers = np.logical_or(f0_[:,0]>__config__["th_outliers_early"],f0_[:,1]>__config__["th_outliers_early"])
point_indices = point_indices[~mask_outliers]
camera_indices = camera_indices[~mask_outliers]
points_2d = points_2d[~mask_outliers]
optimized_points = np.int32(list(set(point_indices)))
logging.info("\t Number of points considered outliers: {}".format(sum(mask_outliers)))
if sum(mask_outliers)/len(mask_outliers)>0.5:
logging.info("!"*20)
logging.info("More than half of the data points have been considered outliers! Something may have gone wrong.")
logging.info("!"*20)
if dump_images:
f01 = evaluate(camera_params, points_3d, points_2d,
camera_indices, point_indices,
n_cameras, n_points)
plt.figure()
plt.plot(f01)
plt.title("Residuals after early outlier rejection")
plt.ylabel("Residual [pixels]")
plt.xlabel("X and Y coordinates")
plt.show()
plt.savefig(os.path.join(__config__["output_path"], "early_outlier_rejection_residuals.jpg"), bbox_inches='tight')
if __config__["bounds"]:
logging.info("Bounded optimization:")
logging.info("\t LB(x)=x-bound; UB(x)=x+bound")
logging.info("\t rvec bounds=({},{},{})".format(*__config__["bounds_cp"][:3]))
logging.info("\t tvec bounds=({},{},{})".format(*__config__["bounds_cp"][3:6]))
logging.info("\t k bounds=(fx={},fy={},c0={},c1={})".format(*__config__["bounds_cp"][6:10]))
logging.info("\t dist bounds=({},{},{},{},{})".format(*__config__["bounds_cp"][10:]))
logging.info("\t 3d points bounds=(x={},y={},z={})".format(*__config__["bounds_pt"]))
else:
logging.info("Unbounded optimization.")
logging.info("Least-Squares optimization of the 3D points:")
logging.info("\t optimize camera parameters: {}".format(False))
logging.info("\t optimize 3d points: {}".format(True))
logging.info("\t ftol={:0.3e}".format(__config__["ftol"]))
logging.info("\t xtol={:0.3e}".format(__config__["xtol"]))
logging.info("\t loss={} f_scale={:0.2f}".format(__config__["loss"], __config__['f_scale']))
logging.info("\t max_nfev={}".format(__config__["max_nfev"]))
points_3d_ref = bundle_adjustment(camera_params, points_3d, points_2d, camera_indices,
point_indices, n_cameras, n_points,
optimize_camera_params=False,
optimize_points=True,
ftol=__config__["ftol"], xtol=__config__["xtol"],
loss=__config__['loss'], f_scale=__config__['f_scale'],
max_nfev=__config__["max_nfev"],
bounds=__config__["bounds"],
bounds_cp = __config__["bounds_cp"],
bounds_pt = __config__["bounds_pt"],
verbose=True, eps=1e-12)
logging.info("Least-Squares optimization of 3D points and camera parameters:")
logging.info("\t optimize camera parameters: {}".format(True))
logging.info("\t optimize 3d points: {}".format(True))
logging.info("\t ftol={:0.3e}".format(__config__["ftol"]))
logging.info("\t xtol={:0.3e}".format(__config__["xtol"]))
logging.info("\t loss={} f_scale={:0.2f}".format(__config__["loss"], __config__['f_scale']))
logging.info("\t max_nfev={}".format(__config__["max_nfev"]))
new_camera_params, new_points_3d = bundle_adjustment(camera_params, points_3d_ref, points_2d, camera_indices,
point_indices, n_cameras, n_points,
optimize_camera_params=__config__["optimize_camera_params"],
optimize_points=__config__["optimize_points"],
ftol=__config__["ftol"], xtol=__config__["xtol"],
loss=__config__['loss'], f_scale=__config__['f_scale'],
max_nfev=__config__["max_nfev"],
bounds=__config__["bounds"],
bounds_cp = __config__["bounds_cp"],
bounds_pt = __config__["bounds_pt"],
verbose=True, eps=1e-12)
f1 = evaluate(new_camera_params, new_points_3d, points_2d,
camera_indices, point_indices,
n_cameras, n_points)
avg_abs_res = np.abs(f1).mean()
logging.info("Average absolute residual: {:0.2f} over {} points.".format(avg_abs_res, len(f1)/2))
if avg_abs_res>15:
logging.info("!"*20)
logging.info("The average absolute residual error is high! Something may have gone wrong.".format(avg_abs_res))
logging.info("!"*20)
if dump_images:
plt.figure()
plt.plot(f1)
plt.title("Residuals after optimization")
plt.ylabel("Residual [pixels]")
plt.xlabel("X and Y coordinates")
plt.show()
plt.savefig(os.path.join(__config__["output_path"], "optimized_residuals.jpg"), bbox_inches='tight')
# Find ouliers points and remove them form the optimization.
# These might be the result of inprecision in the annotations.
# in this case we remove the resduals higher than 20 pixels.
if __config__["th_outliers"]==0 or __config__["th_outliers"] is None:
logging.info("No outlier rejection.")
else:
logging.info("Outlier rejection:")
logging.info("\t threshold outliers: {}".format(__config__["th_outliers"]))
logging.info("\t max_nfev={}".format(__config__["max_nfev2"]))
f1_ = np.abs(f1.reshape(-1,2))
mask_outliers = np.logical_or(f1_[:,0]>__config__["th_outliers"],f1_[:,1]>__config__["th_outliers"])
point_indices = point_indices[~mask_outliers]
camera_indices = camera_indices[~mask_outliers]
points_2d = points_2d[~mask_outliers]
optimized_points = np.int32(list(set(point_indices)))
logging.info("\t Number of points considered outliers: {}".format(sum(mask_outliers)))
if sum(mask_outliers)==0:
logging.info("\t Exit.")
else:
if sum(mask_outliers)/len(mask_outliers)>0.5:
logging.info("!"*20)
logging.info("More than half of the data points have been considered outliers! Something may have gone wrong.")
logging.info("!"*20)
logging.info("\t New sizes:")
logging.info("\t\t camera_params: {}".format(camera_params.shape))
logging.info("\t\t points_3d: {}".format(points_3d.shape))
logging.info("\t\t points_2d: {}".format(points_2d.shape))
if len(points_2d)==0:
logging.info("No points left! Exit.")
return
new_camera_params, new_points_3d = bundle_adjustment(camera_params, points_3d_ref, points_2d, camera_indices,
point_indices, n_cameras, n_points,
optimize_camera_params=__config__["optimize_camera_params"],
optimize_points=__config__["optimize_points"],
ftol=__config__["ftol"], xtol=__config__["xtol"],
loss=__config__['loss'], f_scale=__config__['f_scale'],
max_nfev=__config__["max_nfev2"],
bounds=__config__["bounds"],
bounds_cp = __config__["bounds_cp"],
bounds_pt = __config__["bounds_pt"],
verbose=True, eps=1e-12)
f2 = evaluate(new_camera_params, new_points_3d, points_2d,
camera_indices, point_indices,
n_cameras, n_points)
avg_abs_res = np.abs(f2).mean()
logging.info("Average absolute residual: {:0.2f} over {} points.".format(avg_abs_res, len(f1)/2))
if avg_abs_res>15:
logging.info("!"*20)
logging.info("The average absolute residual error (after outlier removal) is high! Something may have gone wrong.".format(avg_abs_res))
logging.info("!"*20)
if dump_images:
plt.figure()
plt.plot(f2)
plt.title("Residuals after outlier removal")
plt.ylabel("Residual [pixels]")
plt.xlabel("X and Y coordinates")
plt.show()
plt.savefig(os.path.join(__config__["output_path"], "optimized_residuals_outliers_removal.jpg"),
bbox_inches='tight')
logging.info("Reprojection errors (mean+-std pixels):")
ba_poses = {}
for i,(view, cp) in enumerate(zip(views, new_camera_params)):
K, R, t, dist = unpack_camera_params(cp)
ba_poses[view] = {"R":R.tolist(), "t":t.tolist(), "K":K.tolist(), "dist":dist.tolist()}
points3d = new_points_3d[point_indices[camera_indices==i]]
points2d = points_2d[camera_indices==i]
mean_error, std_error = reprojection_error(R, t, K, dist, points3d, points2d)
logging.info("\t {} n_points={}: {:0.3f}+-{:0.3f}".format(view, len(points3d), mean_error, std_error))
logging.info("Reprojection errors (median pixels):")
ba_poses = {}
for i,(view, cp) in enumerate(zip(views, new_camera_params)):
K, R, t, dist = unpack_camera_params(cp)
ba_poses[view] = {"R":R.tolist(), "t":t.tolist(), "K":K.tolist(), "dist":dist.tolist()}
points3d = new_points_3d[point_indices[camera_indices==i]]
points2d = points_2d[camera_indices==i]
mean_error, std_error = reprojection_error(R, t, K, dist, points3d, points2d, 'median')
logging.info("\t {} n_points={}: {:0.3f}".format(view, len(points3d), mean_error))
ba_points = {"points_3d": new_points_3d[optimized_points].tolist(),
"ids":np.array(ids)[optimized_points].tolist()}
if __config__["each_visualisation"]<2 or __config__["each_visualisation"] is None:
logging.info("Visualise all the annotations.")
else:
logging.info("Subsampling the annotations to visualise to 1 every {}.".format(__config__["each_visualisation"]))
path = __config__['output_path'] if dump_images else None
visualisation(setup, landmarks, filenames_images,
new_camera_params, new_points_3d,
points_2d, camera_indices, each=__config__["each_visualisation"], path=path)
utils.json_write(f"./multiview_calib/examples/pose/ba/ba_poses_{name}.json", ba_poses)
utils.json_write("./multiview_calib/examples/pose/ba_points.json", ba_points)