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 Pose_test(self, frame1, frame2):
feature1 = self._config.LoadFeature(frame1)
feature2 = self._config.LoadFeature(frame2)
[loc1, des1] = [feature1['location'], feature1['descriptor']]
[loc2, des2] = [feature2['location'], feature2['descriptor']]
flann = pyflann.FLANN()
result, dist = flann.nn(des2,
des1,
2,
algorithm="kmeans",
branching=32,
iterations=10,
checks=200)
index1 = np.arange(loc1.shape[0])
compare = (dist[:, 0].astype(np.float32) /
dist[:, 1]) < self._config.Get('flann_threshold')
index1 = index1[compare]
index2 = result[:, 0][compare]
loc1 = loc1[index1, :]
loc2 = loc2[index2, :]
[F1, M1] = cv2.findFundamentalMat(loc1, loc2, cv2.FM_RANSAC)
[F2, M2] = cv2.findFundamentalMat(loc2, loc1, cv2.FM_RANSAC)
M = M1 * M2
M = np.reshape(M, [-1])
loc1 = loc1[M == 1, :]
loc2 = loc2[M == 1, :]
loc1 = normalized(loc1, 720, 1280)
loc2 = normalized(loc2, 720, 1280)
a = np.ones([loc1.shape[0], 3], np.float)
a[:, :2] = loc1
b = np.ones([loc1.shape[0], 3], np.float)
b[:, :2] = loc2
# print a
# eight point is better
M = pyopengv.relative_pose_eightpt(a, b)
# print M
i = 20
print a[i, :]
print b[i, :]
print np.dot(np.dot(a[i, :], M), b[i, :].T)
'''