def test_relative_pose_from_essential(one_pair_and_its_E):
f1, f2, E, pose = one_pair_and_its_E
result = pygeometry.relative_pose_from_essential(E, f1, f2)
pose.translation /= np.linalg.norm(pose.translation)
expected = pose.get_Rt()
assert np.allclose(expected, result, rtol=1e-10)
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 test_relative_pose_from_essential(pairs_and_their_E):
for f1, f2, E, pose in pairs_and_their_E:
result = pygeometry.relative_pose_from_essential(E, f1, f2)
pose = copy.deepcopy(pose)
pose.translation /= np.linalg.norm(pose.translation)
expected = pose.get_world_to_cam()[:3]
assert np.allclose(expected, result, rtol=1e-10)
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