def test_triangulation():
print "Testing triangulation"
d = RelativePoseDataset(10, 0.0, 0.0)
points1 = pyopengv.triangulation_triangulate(
d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation)
assert np.allclose(d.points, points1)
points2 = pyopengv.triangulation_triangulate2(
d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation)
assert np.allclose(d.points, points2)
print "Done testing triangulation"
def test_triangulation():
print("Testing triangulation")
d = RelativePoseDataset(10, 0.0, 0.0)
points1 = pyopengv.triangulation_triangulate(
d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation)
assert np.allclose(d.points, points1)
points2 = pyopengv.triangulation_triangulate2(
d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation)
assert np.allclose(d.points, points2)
print("Done testing triangulation")
def test_relative_pose_triangulated():
# Creating Random Poses / Points
d = RelativePoseDataset(500, 0.002, 0.15)
result = pyopengv.relative_pose_ransac_optimize(d.bearing_vectors1,
d.bearing_vectors2,
"NISTER", 4., 1000)
E = result[0]
print("Testing mini Odom\n")
print("Relative Pose Computed : \n")
R = E[:, :3]
t = normalized(E[:, 3])
print("Truth : \n %s \n Normalized Truth : %s \n" %
(d.position, normalized(d.position)))
print("Got : \n %s \n Normalized translation : \n %s \n" % (E, t))
print("%s inliers / %s points \n" % (len(result[1]), len(d.points)))
bearing_vectors_to_erase = []
inliers = result[1]
for i in xrange(len(d.bearing_vectors1)):
if inliers != []:
if inliers[0] == i:
inliers.pop(0)
else:
bearing_vectors_to_erase.append(i)
else:
bearing_vectors_to_erase.append(i)
bv_1 = np.delete(d.bearing_vectors1, bearing_vectors_to_erase, axis=0)
bv_2 = np.delete(d.bearing_vectors2, bearing_vectors_to_erase, axis=0)
pts_3d = pyopengv.triangulation_triangulate2(bv_1, bv_2, t, R)
abs_pose = pyopengv.absolute_pose_ransac_optimize(bv_2, pts_3d, "KNEIP",
2., 500)
print("Absolute Pose Computed : \n %s \n" % abs_pose[0])
print("%s inliers / %s points \n" % (len(abs_pose[1]), len(pts_3d)))
# for i in xrange(len(d.points)):
# d.bearing_vectors2[i] = R.T.dot(d.points[i] - t)
result_ = pyopengv.absolute_pose_ransac(d.bearing_vectors2, d.points,
"KNEIP", 0.001, 1000)
print("\n----------------------\n")
print("TEST \n")
print("Truth : \n")
print(np.hstack((d.rotation, d.position.reshape(3, 1))))
print("Essential M. : \n")
print(E)
print()
print(np.hstack((R, t.reshape(3, 1))))
print()
print(np.hstack((R, t.reshape(3, 1))) / E)
print("\n Truth : \n")
print(d.points[:5])
print()
print(d.bearing_vectors1[:5])
print()
print((d.bearing_vectors1 / d.points)[:5])
print()
print("\nAbsolute Pose : \n")
print(normalized(result_[0][:, 3]))
print()
print(t)
def triangulationTest(src, dst, position, rotation): points1 = gv.triangulation_triangulate(src, dst, position, rotation) # print points1 points2 = gv.triangulation_triangulate2(src, dst, position, rotation)