def test_triangulate_directional_relative_pair():
np.random.seed(0)
poses = [SE3.identity(), SE3.from_tangent(np.random.randn(6)*.1)]
point = np.random.randn(3) + [0, 0, 10]
features = [pr(np.dot(pose.orientation, point - pose.position)) for pose in poses]
estimated = triangulate_directional_relative_pair(features[0], features[1], poses[1])
assert_arrays_almost_equal(estimated, point)
def test_triangulate_directional_relative_pair():
np.random.seed(0)
poses = [SE3.identity(), SE3.from_tangent(np.random.randn(6) * .1)]
point = np.random.randn(3) + [0, 0, 10]
features = [
pr(np.dot(pose.orientation, point - pose.position)) for pose in poses
]
estimated = triangulate_directional_relative_pair(features[0], features[1],
poses[1])
assert_arrays_almost_equal(estimated, point)
def generate_features(num_frames, noise=0):
ps = np.random.randn(num_frames, 3)
rs = map(rotation.exp, np.random.randn(num_frames, 3)*.1)
poses = [SE3(r, p) for r, p in zip(rs, ps)]
point = np.random.randn(3) + [0, 0, 10]
features = []
for pose in poses:
z = pr(np.dot(pose.orientation, point - pose.position))
if noise > 0:
z += np.random.randn(2) * noise
features.append(z)
return features, poses, point
def generate_features(num_frames, noise=0):
ps = np.random.randn(num_frames, 3)
rs = map(rotation.exp, np.random.randn(num_frames, 3) * .1)
poses = [SE3(r, p) for r, p in zip(rs, ps)]
point = np.random.randn(3) + [0, 0, 10]
features = []
for pose in poses:
z = pr(np.dot(pose.orientation, point - pose.position))
if noise > 0:
z += np.random.randn(2) * noise
features.append(z)
return features, poses, point
