def relative_pose_optimize_nonlinear(b1, b2, t, R, iterations):
try:
return pyopengv.relative_pose_optimize_nonlinear(
b1, b2, t, R, iterations)
except Exception:
# Current master of pyopengv do not accept the iterations argument.
return pyopengv.relative_pose_optimize_nonlinear(b1, b2, t, R)
def robust_match_calibrated(p1, p2, camera1, camera2, matches, config):
"""Filter matches by estimating the Essential matrix via RANSAC."""
if len(matches) < 8:
return np.array([])
p1 = p1[matches[:, 0]][:, :2].copy()
p2 = p2[matches[:, 1]][:, :2].copy()
b1 = camera1.pixel_bearing_many(p1)
b2 = camera2.pixel_bearing_many(p2)
threshold = config['robust_matching_calib_threshold']
T = multiview.relative_pose_ransac(b1, b2, b"STEWENIUS",
1 - np.cos(threshold), 1000, 0.999)
for relax in [4, 2, 1]:
inliers = _compute_inliers_bearings(b1, b2, T, relax * threshold)
if sum(inliers) < 8:
return np.array([])
T = pyopengv.relative_pose_optimize_nonlinear(b1[inliers], b2[inliers],
T[:3, 3], T[:3, :3])
inliers = _compute_inliers_bearings(b1, b2, T, threshold)
return matches[inliers]
def test_relative_pose():
print("Testing relative pose")
d = RelativePoseDataset(10, 0.0, 0.0)
# running experiments
twopt_translation = pyopengv.relative_pose_twopt(d.bearing_vectors1,
d.bearing_vectors2,
d.rotation)
fivept_nister_essentials = pyopengv.relative_pose_fivept_nister(
d.bearing_vectors1, d.bearing_vectors2)
fivept_kneip_rotations = pyopengv.relative_pose_fivept_kneip(
d.bearing_vectors1, d.bearing_vectors2)
sevenpt_essentials = pyopengv.relative_pose_sevenpt(
d.bearing_vectors1, d.bearing_vectors2)
eightpt_essential = pyopengv.relative_pose_eightpt(d.bearing_vectors1,
d.bearing_vectors2)
t_perturbed, R_perturbed = getPerturbedPose(d.position, d.rotation, 0.01)
eigensolver_rotation = pyopengv.relative_pose_eigensolver(
d.bearing_vectors1, d.bearing_vectors2, R_perturbed)
t_perturbed, R_perturbed = getPerturbedPose(d.position, d.rotation, 0.1)
nonlinear_transformation = pyopengv.relative_pose_optimize_nonlinear(
d.bearing_vectors1, d.bearing_vectors2, t_perturbed, R_perturbed)
assert proportional(d.position, twopt_translation)
assert matrix_in_list(d.essential, fivept_nister_essentials)
assert matrix_in_list(d.rotation, fivept_kneip_rotations)
assert matrix_in_list(d.essential, sevenpt_essentials)
assert proportional(d.essential, eightpt_essential)
assert proportional(d.rotation, eigensolver_rotation)
assert same_transformation(d.position, d.rotation,
nonlinear_transformation)
print("Done testing relative pose")
def test_relative_pose():
print "Testing relative pose"
d = RelativePoseDataset(10, 0.0, 0.0)
# running experiments
twopt_translation = pyopengv.relative_pose_twopt(d.bearing_vectors1, d.bearing_vectors2, d.rotation)
fivept_nister_essentials = pyopengv.relative_pose_fivept_nister(d.bearing_vectors1, d.bearing_vectors2)
fivept_kneip_rotations = pyopengv.relative_pose_fivept_kneip(d.bearing_vectors1, d.bearing_vectors2)
sevenpt_essentials = pyopengv.relative_pose_sevenpt(d.bearing_vectors1, d.bearing_vectors2)
eightpt_essential = pyopengv.relative_pose_eightpt(d.bearing_vectors1, d.bearing_vectors2)
t_perturbed, R_perturbed = getPerturbedPose( d.position, d.rotation, 0.01)
eigensolver_rotation = pyopengv.relative_pose_eigensolver(d.bearing_vectors1, d.bearing_vectors2, R_perturbed)
t_perturbed, R_perturbed = getPerturbedPose( d.position, d.rotation, 0.1)
nonlinear_transformation = pyopengv.relative_pose_optimize_nonlinear(d.bearing_vectors1, d.bearing_vectors2, t_perturbed, R_perturbed)
assert proportional(d.position, twopt_translation)
assert matrix_in_list(d.essential, fivept_nister_essentials)
assert matrix_in_list(d.rotation, fivept_kneip_rotations)
assert matrix_in_list(d.essential, sevenpt_essentials)
assert proportional(d.essential, eightpt_essential)
assert proportional(d.rotation, eigensolver_rotation)
assert same_transformation(d.position, d.rotation, nonlinear_transformation)
print "Done testing relative pose"
def robust_match_calibrated(p1, p2, camera1, camera2, matches, config):
"""Filter matches by estimating the Essential matrix via RANSAC."""
if len(matches) < 8:
return np.array([])
p1 = p1[matches[:, 0]][:, :2].copy()
p2 = p2[matches[:, 1]][:, :2].copy()
b1 = camera1.pixel_bearing_many(p1)
b2 = camera2.pixel_bearing_many(p2)
threshold = config['robust_matching_calib_threshold']
T = multiview.relative_pose_ransac(
b1, b2, b"STEWENIUS", 1 - np.cos(threshold), 1000, 0.999)
for relax in [4, 2, 1]:
inliers = _compute_inliers_bearings(b1, b2, T, relax * threshold)
if sum(inliers) < 8:
return np.array([])
T = pyopengv.relative_pose_optimize_nonlinear(
b1[inliers], b2[inliers], T[:3, 3], T[:3, :3])
inliers = _compute_inliers_bearings(b1, b2, T, threshold)
return matches[inliers]
def relative_pose_optimize_nonlinear(b1, b2, method, t, R, iterations):
# in-house refinement
if method == 'mapillary':
Rt = np.zeros((3, 4))
Rt[:3, :3] = R.T
Rt[:, 3] = -R.T.dot(t)
Rt_refined = pygeometry.relative_pose_refinement(Rt, b1, b2, iterations)
R, t = Rt_refined[:3, :3].copy(), Rt_refined[:, 3].copy()
Rt[:3, :3] = R.T
Rt[:, 3] = -R.T.dot(t)
return Rt
else:
try:
return pyopengv.relative_pose_optimize_nonlinear(b1, b2, t, R, iterations)
except Exception:
# Current master of pyopengv do not accept the iterations argument.
return pyopengv.relative_pose_optimize_nonlinear(b1, b2, t, R)
def run_relative_pose_optimize_nonlinear(b1, b2, t, R):
return pyopengv.relative_pose_optimize_nonlinear(b1, b2, t, R)
def run_relative_pose_optimize_nonlinear(b1, b2, t, R):
return pyopengv.relative_pose_optimize_nonlinear(b1, b2, t, R)