# Triangulation methods and uncertainty

$$\DeclareMathOperator*{\argmin}{argmin} \DeclareMathOperator*{\Var}{Var}$$

A very common thing to want to do with a calibrated camera system is to convert a pair of pixel observations of a feature to a point in space that produced these observations, a process known as triangulation. mrcal supports both sparse triangulation (processing a small number of discrete pixel observations) and dense triangulation (processing every pixel in a pair of images; stereo vision). This can be sensitive to noise, creating a strong need for proper error modeling and propagation.

Here I describe mrcal's sparse triangulation capabilities: the mrcal-triangulate tool and the mrcal.triangulate() Python routine.

## Overview

Let's say we have an idealized geometry:

Let $$b \equiv \mathrm{baseline}$$ and $$r \equiv \mathrm{range}$$. Two cameras are looking at a point in space. Given two camera models and a pair of pixel observations we can compute the range to the point. Basic geometry tells us that

$\frac{r}{\sin \phi} = \frac{b}{\sin \theta}$

When looking far away, straight ahead, we have $$\theta \approx 0$$ and $$\phi \approx 90^\circ$$, so

$r \approx \frac{b}{\theta}$

Differentiating, we get

$\frac{\mathrm{d}r}{\mathrm{d}\theta} \propto \frac{b}{\theta^2} \propto \frac{r^2}{b}$

Thus a small error in $$\theta$$ causes an error in the computed range that is proportional to the square of $$r$$. This relationship sets the fundamental limit for the ranging capabilities of stereo systems: if you try to look out too far, the precision of $$\theta$$ required to get a precise-enough $$r$$ becomes unattainable. And because we have $$r^2$$, this range limit is approached very quickly. A bigger baseline helps, but does so only linearly.

The angle $$\theta$$ comes from the extrinsics and intrinsics in the camera model, so the noise modeling and uncertainty propagation in mrcal are essential to a usable long-range stereo system.

## Triangulation routines

Before we can talk about quantifying the uncertainty of a triangulation operation, we should define what that operation is. Each triangulation operation takes as input

• Two camera models. Intrinsics (lens behavior) and extrinsics (geometry) are required for both
• Pixel coordinates $$\vec q$$ of the same observed feature in the two images captured by each camera

And it outputs

• A point $$\vec p$$ in space that produced the given pixel observations

The "right" way to implement this operation is to minimize the reprojection error:

$E\left(\vec p\right) \equiv \left\lVert \vec q_0 - \mathrm{project}_\mathrm{cam0}\left(\vec p\right) \right\rVert^2 + \left\lVert \vec q_1 - \mathrm{project}_\mathrm{cam1}\left(\vec p\right) \right\rVert^2$

$\vec p^* \equiv \argmin{E\left(\vec p\right)}$

This is correct, but it's complex and requires a nonlinear optimization, which limits the usefulness of this approach. mrcal implements several slightly-imprecise but much faster methods to compute a triangulation. All of these precompute $$\vec v \equiv \mathrm{unproject} \left( \vec q \right)$$, and then operate purely geometrically. The methods are described in these papers, listed in chronological order:

• "Triangulation Made Easy", Peter Lindstrom. IEEE Conference on Computer Vision and Pattern Recognition, 2010
• "Closed-Form Optimal Two-View Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. https://arxiv.org/abs/1903.09115
• "Triangulation: Why Optimize?", Seong Hun Lee and Javier Civera https://arxiv.org/abs/1907.11917

The last paper compares the available methods from all the papers. A triangulation study is available to evaluate the precision and accuracy of the existing methods. Currently leecivera_mid2 is recommended for most usages. The triangulation methods available in mrcal:

### geometric

This is the basic midpoint method: it computes the point in space that minimizes the distance between the two observation rays. This is the simplest method, but also produces the most bias. Not recommended. Implemented in mrcal.triangulate_geometric() (in Python) and mrcal_triangulate_geometric() (in C).

### lindstrom

Described in the "Triangulation Made Easy" paper above. The method is a close approximation to a reprojection error minimization (the "right" approach above) if we have pinhole lenses. Implemented in mrcal.triangulate_lindstrom() (in Python) and mrcal_triangulate_lindstrom() (in C).

### leecivera_l1

Described in the "Closed-Form Optimal Two-View Triangulation Based on Angular Errors" paper above. Minimizes the L1 norm of the observation angle error. Implemented in mrcal.triangulate_leecivera_l1() (in Python) and mrcal_triangulate_leecivera_l1() (in C).

### leecivera_linf

Described in the "Closed-Form Optimal Two-View Triangulation Based on Angular Errors" paper above. Minimizes the L-infinity norm of the observation angle error. Implemented in mrcal.triangulate_leecivera_linf() (in Python) and mrcal_triangulate_leecivera_linf() (in C).

### leecivera_mid2

Described in the "Triangulation: Why Optimize?" paper above: this is the "Mid2" method. Doesn't explicitly minimize anything, but rather is a heuristic that works well in practice. Implemented in mrcal.triangulate_leecivera_mid2() (in Python) and mrcal_triangulate_leecivera_mid2() (in C).

### leecivera_wmid2

Described in the "Triangulation: Why Optimize?" paper above: this is the "wMid2" method. Doesn't explicitly minimize anything, but rather is a heuristic that works well in practice. Similar to leecivera_mid2, but contains a bit of extra logic to improve the behavior for points very close to the cameras (not satisfying $$r \gg b$$). Implemented in mrcal.triangulate_leecivera_wmid2() (in Python) and mrcal_triangulate_leecivera_wmid2() (in C).

## Triangulation uncertainty

We compute the uncertainty of a triangulation operation using the usual error-propagation technique:

• We define the input noise
• We compute the operation through which we're propagating this input noise, evaluating the gradients of the output in respect to all the noisy inputs
• We assume the behavior is locally linear and that the input noise is Gaussian, which allows us to easily compute the output noise using the usual noise-propagation relationship

### Noise sources

We want to capture the effect of two different sources of error:

• Calibration-time noise. We propagate the noise in chessboard observations obtained during the chessboard dance. This is the noise that we propagate when evaluating projection uncertainty. This is specified in the --q-calibration-stdev argument to mrcal-triangulate or in the q_calibration_stdev argument to mrcal.triangulate(). This is usually known from the calibration, and we can request the calibrated value by passing a stdev of -1. See the relevant interface documentation (just-mentioned links) for details.
• Observation-time noise. Each triangulation processes observations $$\vec q$$ of a feature in space. These are noisy, and we propagate the noise. As with the calibration-time noise, this noise is assumed to be normally distributed, independent in $$x$$ and $$y$$. This is specified in the --q-observation-stdev argument to mrcal-triangulate or in the q_observation_stdev argument to mrcal.triangulate(). A common source of these pixel observations is a pixel correlation operation where a patch in one image is matched against the second image. Corresponding pixel observations observed this way are correlated: the noise in $$\vec q_0$$ not independent of the noise in $$\vec q_1$$. I do not yet know how to estimate this correlation, but the tools are able to ingest and propagate such an estimate: using the --q-observation-stdev-correlation commandline option to mrcal-triangulate or the q_observation_stdev_correlation argument to mrcal.triangulate().

A big point to note here is that repeated observations of the same feature have independent observation-time noise. So these observation-time errors average out with multiple observations. This is not true of the calibration-time noise however. Using the same calibration to observe a feature multiple times will produce correlated triangulation results. So calibration-time noise is biased, and it is thus essential to make and use low-uncertainty calibrations to minimize this effect.

### Sample uncertainties

The test-triangulation-uncertainty.py test generates synthetic models and triangulation scenarios. It can be used to produce an illustrative diagram:

test/test-triangulation-uncertainty.py  \
--do-sample                           \
--cache write                         \
--observed-point -2 0 10              \
--fixed cam0                          \
--Nsamples 200                        \
--Ncameras 2                          \
--q-observation-stdev-correlation 0.5 \
--q-calibration-stdev 0.2             \
--q-observation-stdev 0.2             \
--make-documentation-plots ''


Here we have two cameras arranged in the usual left/right stereo configuration, looking at two points somewhere ahead. We generate calibration and observation noise, and display the results in the horizontal plane. The vertical dimension is insignificant here, so it is not shown, even though all the computations are performed in full 3D. For each of the two observed points we display:

• The empirical noise samples, and the 1-sigma ellipse they represent
• The predicted 1-sigma ellipse for the calibration-time noise
• The predicted 1-sigma ellipse for the observation-time noise
• The predicted 1-sigma ellipse for the joint noise

We can see that the observed and predicted covariances line up nicely. We can also see that the observation-time noise acts primarily in the forward/backward direction, while the calibration-time noise has a much larger lateral effect. This pattern varies greatly depending on the lenses and the calibration and the geometry. As we get further out, the uncertainty in the forward/backward direction dominates for both noise sources, as expected.

### Stabilization

In the above plot, the uncertainties are displayed in the coordinate system of the left camera. But, as described on the projection uncertainty page, the origin and orientation of each camera's coordinate system is subject to calibration noise:

So what we usually want to do is to consider the covariance of the triangulation in the coordinates of the camera housing, not the camera coordinate system. We achieve this with "stabilization", computed exactly as described on the projection uncertainty page. We can recompute the triangulation uncertainty in the previous example (same geometry, lens, etc), but with stabilization enabled:

test/test-triangulation-uncertainty.py  \
--do-sample                           \
--cache write                         \
--observed-point -2 0 10              \
--fixed cam0                          \
--Nsamples 200                        \
--Ncameras 2                          \
--q-observation-stdev-correlation 0.5 \
--q-calibration-stdev 0.2             \
--q-observation-stdev 0.2             \
--stabilize                           \
--make-documentation-plots ''


We can now clearly see that the forward/backward uncertainty was a real effect, but the lateral uncertainty was largely due to the moving camera coordinate system.

### Calibration-time noise produces correlated estimates

As mentioned above, the calibration-time noise produces correlations (and thus biases) in the triangulated measurements. Since the test-triangulation-uncertainty.py command triangulates two different points, we can directly observe these correlations. Let's look at the magnitude of each element of $$\Var {\vec p_{01}}$$ where $$\vec p_{01}$$ is a 6-dimensional vector that contains both the triangulated 3D points: $$\vec p_{01} \equiv \left[ \begin{array}{cc} \vec p_0 \\ \vec p_1 \end{array} \right]$$. If we had only observation-time noise, $$\vec p_0$$ and $$\vec p_1$$ would be independent, and the off-diagonal terms in the covariance matrix would be 0. However, we also have calibration-time noise, so the errors are correlated:

As before, the exact pattern varies greatly depending on the lenses and the calibration and the geometry, but calibration-time noise always creates these correlations. To reduce these correlations and the biases they cause: lower the uncertainty of your calibrations by dancing better

### Assumptions break down at infinity

When propagating noise, mrcal makes the very common assumption that everything is locally linear. This makes things simple, and is right most of the time. However, when running the triangulation routines with near-parallel rays, this assumptions can break down.

Let's run another simulation, but observing a more distant point, with more observation-time noise, no calibration-time noise, and gathering more samples:

test/test-triangulation-uncertainty.py  \
--do-sample                           \
--cache write                         \
--observed-point -200 0 2000          \
--fixed cam0                          \
--Nsamples 2000                       \
--Ncameras 2                          \
--q-observation-stdev-correlation 0.5 \
--q-observation-stdev 0.4             \
--stabilize                           \
--make-documentation-plots ''


The range to the observed point:

The two points in the synthetic world are at $$(\pm 200, 0, 2000)m$$ so the true range is ~ $$2010m$$. We see that the calibration-time noise has little effect here. More importantly, we also see that the predicted distribution of the range to the point is gaussian (as we assume), but the empirical distribution is not gaussian: there's a much more significant tail on the long end. This makes sense. If the observation rays are near-parallel, small errors that make the rays more parallel push the range to infinity; while small errors that bring the rays together have a more modest, finite effect.

Similarly, when we look at the distance between our two points we get this distribution:

We see the same asymmetric non-gaussian distribution. Empirically I observe this distance-between-points distribution become more non-gaussian, faster than the range-to-point distribution.

At this time I do not know how much this matters or what to do about it, but these limitations are good to keep in mind.

## Applications

Visual tracking of an object over time is one application that would benefit from a more complete error model of its input. Repeated noisy observations of a moving object $$\vec q_{01}(t)$$ can be triangulated into a noisy estimate of the object motion $$\vec p(t)$$. If for each point in time $$t$$ we have $$\Var \vec p(t)$$, we can combine everything into an estimate $$\hat p(t)$$. The better our covariances, the closer the estimate. The mrcal.triangulate() routine can be used to compute the triangulations, and to report the full covariances matrices.

## Applying these techniques

See the tour of mrcal for an application of these routines to real-world data