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.