Triangulation
High quality implementations of triangulation algorithms in pure python.
Installation
pip install triangulation
Usage
from rigidbody import pr, rotation, SE3
from triangulation import triangulate
noise = 1e-3
true_point = np.random.randn(3) + [0, 0, 10]
positions = np.random.randn(num_frames, 3)
orientations = map(rotation.exp, np.random.randn(num_frames, 3)*.1)
poses = [SE3(r, p) for r, p in zip(orientations, positions)]
features = []
for pose in poses:
z = pr(np.dot(pose.orientation, point - pose.position))
features.append(z + np.random.randn(2) * noise)
triangulated_point = triangulate(features, poses, algorithm="midpoint")
Algorithms
triangulate_midpoint
Finds the 3D point minimizing the sum of squared distances to the rays from each camera.
triangulate_linear
Finds the 3D point minimizing the sum of squared alebraic errors.
triangulate_directional
Finds the 3D point minimizing the squared directional error in the first view and the view with longest baseline to the first view.
triangulate_directional_pair
Finds the 3D point minimizing the squared directional error in two views.
triangulate_infnorm
Finds the 3D point minimizing the infinity norm of the vector of reprojection errors in all views.
triangulate_infnorm_fixed
Finds any 3D point such that all reprojection errors are no greater than a given threshold.