def Rt_from_E(E, pts1, pts2, K1, K2, refine_Rt=False, refine_iter=300):
cam1 = build_perspective_cam(0, K1)
cam2 = build_perspective_cam(1, K2)
bts1 = cam1.pixel_bearing_many(pts1[:, :2])
bts2 = cam2.pixel_bearing_many(pts2[:, :2])
Rt = pygeometry.relative_pose_from_essential(E, bts1, bts2)
if refine_Rt is True:
Rt = pygeometry.relative_pose_refinement(Rt, bts1, bts2, refine_iter)
return Rt
def relative_pose_optimize_nonlinear(b1, b2, t, R, iterations):
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
def relative_pose_optimize_nonlinear(b1: np.ndarray, b2: np.ndarray,
t: np.ndarray, R: np.ndarray,
iterations: int) -> np.ndarray:
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
def test_relative_pose_refinement(one_pair_and_its_E):
f1, f2, _, pose = one_pair_and_its_E
pose = copy.deepcopy(pose)
pose.translation /= np.linalg.norm(pose.translation)
noisy_pose = copy.deepcopy(pose)
noisy_pose.translation += np.random.rand(3)*1e-2
noisy_pose.rotation += np.random.rand(3)*1e-2
result = pygeometry.relative_pose_refinement(noisy_pose.get_Rt(), f1, f2, 1000)
expected = pose.get_Rt()
assert np.linalg.norm(expected-result) < 5e-2
def relative_pose_from_essential_mat(E,
cam1,
cam2,
pts1,
pts2,
refine_iter=600):
# pts1_normalized = features.normalized_image_coordinates(pts1[:, :2].copy(), width=cam1.width, height=cam1.height)
# pts2_normalized = features.normalized_image_coordinates(pts2[:, :2].copy(), width=cam2.width, height=cam2.height)
bts1 = cam1.pixel_bearing_many(pts1.copy())
bts2 = cam2.pixel_bearing_many(pts2.copy())
Rt = pygeometry.relative_pose_from_essential(E, bts1, bts2)
Rt = pygeometry.relative_pose_refinement(Rt, bts1, bts2, refine_iter)
return Rt
def Refine_Rt_from_E(E, pts1, pts2, K1, K2, refine_Rt=False, refine_iter=300):
E = E.reshape(3, 3).astype(np.float64)
pts1 = pts1.astype(np.float64)
pts2 = pts2.astype(np.float64)
cam1 = build_perspective_cam(0, K1)
cam2 = build_perspective_cam(1, K2)
bts1 = cam1.pixel_bearing_many(pts1[:, :2])
bts2 = cam2.pixel_bearing_many(pts2[:, :2])
Rt = pygeometry.relative_pose_from_essential(E, bts1, bts2)
if refine_Rt is True:
r_Rt = pygeometry.relative_pose_refinement(Rt, bts1, bts2, refine_iter)
else:
r_Rt = None
return Rt, r_Rt
def test_relative_pose_refinement(pairs_and_their_E):
exact_found = 0
for f1, f2, _, pose in pairs_and_their_E:
pose = copy.deepcopy(pose)
pose.translation /= np.linalg.norm(pose.translation)
noisy_pose = copy.deepcopy(pose)
noisy_pose.translation += np.random.rand(3)*1e-2
noisy_pose.rotation += np.random.rand(3)*1e-2
result = pygeometry.relative_pose_refinement(noisy_pose.get_Rt(), f1, f2, 1000)
expected = pose.get_Rt()
exact_found += np.linalg.norm(expected-result) < 1.8e-1
exacts = len(pairs_and_their_E)-1
assert exact_found >= exacts
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)