# Optimization problem formulation

## Table of Contents

## Overview

mrcal contains a solver used to compute the lens models and/or geometry. This is accessible via either

`mrcal_optimize()`

routine in the C API`mrcal.optimize()`

routine in the Python API

These are the main call in the `mrcal-calibrate-cameras`

tool (to calibrate
cameras) and `mrcal-convert-lensmodel`

tool (to fit a different lens model into
an existing model). The optimization routines themselves are more general than
this, and can solve other problems, such as structure-from-motion. Note that the
APIs for handling discrete points are still unstable, so the SFM functionality
remains lightly-documented for now.

The solver moves around the *state* vector \(\vec p\), which contains all the
geometry and all the lens models. For any hypothesis \(\vec p\), the solver can
predict the pixel coordinates where the hypothetical cameras would observe their
hypothetical world. The differences between these predicted pixel observations
and the actual pixel observations we gathered from looking at chessboards are
stored in a *measurement* vector \(\vec x\). The solver then tries to find the set
of geometry and lens parameters to best explain the observed pixel coordinates,
so it seeks the \(\vec p\) to minimize the cost function \(E \equiv \left \Vert \vec x
\left(\vec p\right)\right \Vert ^2\).

The optimization library interfaces with mrcal by invoking a callback function for each hypothesis \(\vec p\) to test. This callback function computes \(\vec x\) and the local gradients \(J \equiv \frac{\partial \vec x}{\partial \vec p}\) (large and sparse). For analysis, this callback function is available by itself via

`mrcal_optimizer_callback()`

routine in the C API`mrcal.optimizer_callback()`

routine in the Python API

## World geometry

There are 3 different coordinate systems in the optimization:

**frame**coordinate system: the local coordinate system of the chessboard. The chessboard is assumed mostly flat, with the grid of points lying in the \(xy\) plane. The origin is at one of the corners.**reference**coordinate system: the "world" coordinate system in the optimization problem. This coordinate system is the common system that ties everything together. Each chessboard pose is represented as a transformation between the local chessboard frame and the reference frame. And each camera pose is represented as the transformation between the local camera frame and the reference frame.**camera**coordinate system: the local coordinate system of each camera. The \(x\) and \(y\) axes are aligned with pixel coordinates in an image: \(x\) is to the right and \(y\) is down. \(z\) is then forward to complete the right-handed system of coordinates.

So the data flow to project a particular chessboard corner which sits at \(\vec p_\mathrm{frame}\) in the local chessboard coordinate system is:

\[ \vec q \xleftarrow{\mathrm{intrinsics}} \vec p_\mathrm{camera} \xleftarrow{T_\mathrm{cr}} \vec p_\mathrm{reference} \xleftarrow{T_\mathrm{rf}} \vec p_\mathrm{frame} \]

where the intrinsics and the transformations \(T_\mathrm{cr}\) and \(T_\mathrm{rf}\) are all elements of the state vector.

### Geometric free variables

If too many transformations are left as free variables for the optimizer to find, the system will be under-determined, and the optimization routine will fail: complaining about a "not positive definite" (singular in this case) Hessian.

Example: we have 1 stationary camera observing 10 chessboards poses. We want to be able to uniquely represent the transformation between each chessboard and the camera, for a total of 10 transformations. If we optimize a separate \(T_\mathrm{cr}\) for the camera and 10 separate \(T_\mathrm{rf}\) for each chessboard, we will have 11 transformations in the optimization vector. Since 11 > 10, we have more variables in the optimization vector than are needed to uniquely describe the world. So the system is under-determined, and the optimization will fail.

In a vanilla calibration problem such as this, we would address this by fixing the reference coordinate system to one of the camera or chessboard frames. The mrcal convention is to fix the reference coordinate system to camera 0. In the above example, this would reduce the number of transformations being optimized from 11 to 10, which resolves the issue.

Any other method of making the optimization variables unique is valid also. For
instance, the chessboard poses might be known. In that case we don't need to
optimize any \(T_\mathrm{rf}\), and solving for *all* the \(T_\mathrm{cr}\) is
valid.

### The physical meaning of the *reference* coordinate system

The reference coordinate system is a single coordinate system common to the
whole optimization problem that all the objects in the world can use to localize
themselves. It does *not* have *any* physical meaning beyond that. In
particular, the reference coordinate system is *not* attached to any fixed
object in the world. Thus noise in the chessboard observations would shift the
reference coordinate system, just as would shift the camera and chessboard
coordinate systems. The projection uncertainty documentation talks about this in
depth.

## Calibration object

This is called a "chessboard" or just "board" in some parts of the code. The optimization code refers to the chessboard pose array as "frames".

When running a camera calibration, we use observations of a known-geometry object. At this time mrcal expects this object to be a planar grid of observable points, possibly with a small amount of deformation. Usually this object is a chessboard-like grid of black and white squares, where the observed points are at the corner of each square.

### Chessboard corner detector

Detections of these corners serve as the input features to mrcal. mrcal is a
purely geometrical toolkit, and this vision problem must be handled by another
library. A number of tools are available to detect chessboard corners. These did
not work well for my use cases, so I recommend `mrgingham`

for all corner
detections. mrgingham is fast and is able to find chessboard corners subject to
very un-pinhole-like projections. At this time it has two limitations that will
be lifted eventually:

- It more or less assumes a grid of 10x10 corners (i.e. 11x11 squares)
- It requires
*all*the corners to be observed in order to report the detections from an image. Incomplete chessboard observations aren't supported

If these are unacceptable, any other detector may be used instead.

### Choice of calibration object

When given an image of a *chessboard*, the detector is directly observing the
feature we actually care about: the corner. Another common calibration board
style is a grid of circles, where the feature of interest is the center of each
circle. When given an image of such a grid of circles, the detector either

- detects the contour at the edge of each circle
- finds the pixel blob comprising each circle observation

and from either of these, the detector infers the circle center. This can work
when looking at head-on images, but when given tilted images subjected to
non-pinhole lens behaviors, getting accurate circle centers from outer contours
or blobs is *hard*. The resulting inaccuracies in the detections of circle
centers will introduce biases into the solve that aren't modeled by the projection uncertainty routine, so chessboards are *strongly* recommended in
favor of circle grids.

mrcal assumes independent noise on each point observation, so correlated sources of point observations (such as corners of an apriltag) are also not appropriate sources of data. Apriltag centers would work, however.

### Board deformation

The calibration object is assumed to be nominally planar. However, large calibration boards used for calibration of wide lenses are never flat: temperature and humidity effects deform the board strongly-enough to affect the calibration. mrcal models this deformation with two axis-aligned parabolic factors. Let the chessboard grid span \([-1,1]\) along the \(x\) and \(y\) axes, then I define the non-planar deformation as \(z \equiv k_x (1 - x^2) + k_y (1 - y^2)\) with \(k_x\) and \(k_y\) being the two deformation factors being optimized by the solver. If the board were flat, \(k_x\) and \(k_y\) would be 0, and thus we would have \(z=0\) everywhere. The deflection at the edges is 0, and is strongest at the center.

Empirically, this appears to work well: I get better-fitting solves, and less systematic error. And the optimal deformation factors \(k_x\), \(k_y\) are consistent between different calibrations. A richer deformation model could work even better, and will eventually be the studied.

## Optimization details

The mrcal solver is an optimization routine based on sparse nonlinear least
squares. The optimization loop is implemented in `libdogleg`

, which at its core
uses the CHOLMOD solver to compute the Cholesky factorization. With a Cholesky
factorization we can efficiently solve the linear system \(J^T J \vec a = \vec b\)
where the jacobian matrix \(J\) is large and sparse.

The optimization problem is posed without constraints. This is achieved by using Rodrigues vectors to represent rotations. A different rotation representation, such as one using unit quaternions or rotation matrices would require constraints: not all sets of 4 numbers are a unit quaternion, and not all sets of 9 numbers are a valid rotation matrix.

The optimization algorithm is iterative, so it isn't guaranteed to converge to
the global optimum. Thus it is imperative to pass a good **seed** (an initial
estimate of the solution) to the optimization routines. The
`mrcal-calibrate-cameras`

tool achieves this by

- Computing an initial estimate directly using geometry and some simplifying
assumptions. These geometric seeding routines are available standalone:
`mrcal.estimate_monocular_calobject_poses_Rt_tocam()`

: Estimate camera-referenced poses of the calibration object from monocular views`mrcal.estimate_joint_frame_poses()`

: Estimate world-referenced poses of the calibration object`mrcal.seed_pinhole()`

: Compute an optimization seed for a camera calibration

- Refining that estimate with a sequences of optimization problems that allow
more and more of the parameters to vary. The final problem is the
*full*problem where all the variables are free to move. The set of variables we're optimizing can be selected with the`mrcal_problem_selections_t`

structure passed to`mrcal_optimize()`

in C (or the`do_optimize_...`

arguments to`mrcal.optimize()`

in Python).

## State vector \(\vec p\)

The state vector \(\vec p\) is controlled by the optimization algorithm as it searches for the optimal solution. This vector may contain

**intrinsics**: the lens parameters of all the cameras in the optimization problem**extrinsics**: the poses of all the cameras in the optimization problem. These are specified as unconstrained`rt`

transformations from some arbitrary "reference". coordinate system, to the camera coordinate system. These are represented by \(T_\mathrm{cr}\) in the flow diagram above**frames**: the poses of all the chessboards in the optimization problem. These are specified as unconstrained`rt`

transformations from the local chessboard coordinate system to some arbitrary "reference" coordinate system. These are represented by \(T_\mathrm{rf}\) in the flow diagram above**points**: the location in the reference coordinate system of any discrete points being observed. A vanilla "calibration" problem wouldn't have any of these, but an SFM problem would have many**calibration-object warp**: the deformation of the calibration object

An optimization problem could contain *all* those things, but it usually only
contains a subset, depending on the specific problem being solved. Common
problems are:

- A vanilla calibration problem. We have stationary cameras observing a moving chessboard. \(\vec p\) contains intrinsics and extrinsics and frames and the calibration-object warp
- Structure-from-motion. We have moving cameras observing a stationary world. \(\vec p\) contains extrinsics and points.
- An intrinsics-fitting problem such as what
`mrcal-convert-lensmodel`

solves. \(\vec p\) contains intrinsics and points

Any other combination is possible.

### State vector layout

When analyzing the behavior of the optimizer it is often useful to pick out particular elements of the full optimization vector \(\vec p\). mrcal provides a number of functions to report the index and size of the block of \(\vec p\) that contains specific data. In C:

`mrcal_state_index_intrinsics()`

: Return the index in the optimization vector of the intrinsics of camera i`mrcal_state_index_extrinsics()`

: Return the index in the optimization vector of the extrinsics of camera i`mrcal_state_index_frames()`

: Return the index in the optimization vector of the pose of frame i`mrcal_state_index_points()`

: Return the index in the optimization vector of the position of point i`mrcal_state_index_calobject_warp()`

: Return the index in the optimization vector of the calibration object warp`mrcal_num_states_intrinsics()`

: Get the number of intrinsics parameters in the optimization vector`mrcal_num_states_extrinsics()`

: Get the number of extrinsics parameters in the optimization vector`mrcal_num_states_frames()`

: Get the number of calibration object pose parameters in the optimization vector`mrcal_num_states_points()`

: Get the number of point-position parameters in the optimization vector`mrcal_num_states_calobject_warp()`

: Get the number of parameters in the optimization vector for the board warp`mrcal_num_states()`

: Get the full length of the optimization vector

And in Python:

`mrcal.state_index_intrinsics()`

: Return the index in the optimization vector of the intrinsics of camera i`mrcal.state_index_extrinsics()`

: Return the index in the optimization vector of the extrinsics of camera i`mrcal.state_index_frames()`

: Return the index in the optimization vector of the pose of frame i`mrcal.state_index_points()`

: Return the index in the optimization vector of the position of point i`mrcal.state_index_calobject_warp()`

: Return the index in the optimization vector of the calibration object warp`mrcal.num_states_intrinsics()`

: Get the number of intrinsics parameters in the optimization vector`mrcal.num_states_extrinsics()`

: Get the number of extrinsics parameters in the optimization vector`mrcal.num_states_frames()`

: Get the number of calibration object pose parameters in the optimization vector`mrcal.num_states_points()`

: Get the number of point-position parameters in the optimization vector`mrcal.num_states_calobject_warp()`

: Get the number of parameters in the optimization vector for the board warp

If plotting a whole vector of state (or a vector of measurements), it is really helpful to annotate the plot to make it clear which variables correspond to each block of state (or measurements). mrcal provides helper functions to help with this:

`mrcal.plotoptions_state_boundaries()`

: Return the 'set' plot options for gnuplotlib to show the state boundaries`mrcal.plotoptions_measurement_boundaries()`

: Return the 'set' plot options for gnuplotlib to show the measurement boundaries

### State vector scaling

The nonlinear least squares-solving library used by mrcal is `libdogleg`

, which
implements Powell's dogleg method. This is a trust-region algorithm that
represents the trust region as a ball in state space. I.e. the radius of this
trust region is the same in every direction. And *that* means that the
optimization will work best when each state variable in \(\vec p\) affects the
cost function \(E\) evenly. Example of what we don't want: camera positions
measured in km, while the chessboard positions are measured in mm, with both
sets of these very different numbers stored in \(\vec p\).

Clearly getting identical behavior from each variable is impossible, but we can
scale the elements of \(\vec p\) to keep things more or less even. mrcal applies
this scaling, and the `libdogleg`

optimization library never sees the full state
vector \(\vec p\), but the scaled vector \(\vec p_\mathrm{packed}\). Similarly, it
never sees the full jacobian \(J \equiv \frac{\partial \vec x}{\partial \vec p}\),
but rather \(J_\mathrm{packed} \equiv \frac{\partial \vec x}{\partial \vec
p_\mathrm{packed}}\). This means that the optimization callback functions report
packed state. These are

`mrcal_optimizer_callback()`

routine in the C API`mrcal.optimizer_callback()`

routine in the Python API

To pack or unpack an array of state, mrcal provides some routines. In C:

`mrcal_pack_solver_state_vector()`

: Scales a state vector to the packed, unitless form used by the optimizer`mrcal_unpack_solver_state_vector()`

: Scales a state vector from the packed, unitless form used by the optimizer

And in Python:

`mrcal.pack_state()`

: Scales a state vector to the packed, unitless form used by the optimizer`mrcal.unpack_state()`

: Scales a state vector from the packed, unitless form used by the optimizer

## Measurement vector \(\vec x\)

Given a hypothesis state vector \(\vec p\) mrcal computes a vector of errors, or
*measurements* \(\vec x\). The optimization algorithm searches the space of
hypotheses \(\vec p\), trying to minimize \(E \equiv \left \Vert \vec x \right \Vert^2\).

We know where each point was observed in reality, and we know where the state vector \(\vec p\) predicts each one would have been observed. So we can construct a vector of errors \(\vec q_\mathrm{err} \equiv \vec q_\mathrm{predicted}\left( \vec p \right) - \vec q_\mathrm{ref}\).

From the noise analysis we derive a matrix of weights \(W\) to construct

\[ \vec x_\mathrm{observations} \equiv W q_\mathrm{err} = W \left( \vec q_\mathrm{predicted}\left( \vec p \right) - \vec q_\mathrm{ref} \right) \]

This is the bulk of the measurement vector.

### Regularization

In addition to \(\vec x_\mathrm{observations}\), the measurement vector contains
*regularization* terms. These are mostly-insignificant terms that are meant to
improve the convergence of the solver. They are also aphysical, and cause a bias
in the solution, so mrcal is careful to keep these small-enough to not break
anything noticeably. The behavior of these terms is likely to change in the
future, so I don't document these in detail; please consult the sources.
Currently the logic is at the end of the `optimizer_callback()`

function in
`mrcal.c`

.

It is possible to control whether a solve does/does not include regularization
terms with the `do_apply_regularization`

bit in `mrcal_problem_selections_t`

or the
`do_apply_regularization`

key in the call to `mrcal.optimize()`

.

### Measurement vector layout

When analyzing the behavior of the optimizer it is often useful to pick out particular elements of the full measurement vector \(\vec x\). mrcal provides a number of functions to report the index and size of the block of \(\vec x\) that contains specific data. In C:

`mrcal_measurement_index_boards()`

: Return the measurement index of the start of a given board observation`mrcal_measurement_index_points()`

: Return the measurement index of the start of a given point observation`mrcal_measurement_index_regularization()`

: Return the index of the start of the regularization measurements`mrcal_num_measurements_boards()`

: Return how many measurements we have from calibration object observations`mrcal_num_measurements_points()`

: Return how many measurements we have from point observations`mrcal_num_measurements_regularization()`

: Return how many measurements we have from regularization`mrcal_measurements()`

: Return how many measurements we have in the full optimization problem

And in Python:

`mrcal.measurement_index_boards()`

: Return the measurement index of the start of a given board observation`mrcal.measurement_index_points()`

: Return the measurement index of the start of a given point observation`mrcal.measurement_index_regularization()`

: Return the index of the start of the regularization measurements`mrcal.num_measurements_boards()`

: Return how many measurements we have from calibration object observations`mrcal.num_measurements_points()`

: Return how many measurements we have from point observations`mrcal.num_measurements_regularization()`

: Return how many measurements we have from regularization`mrcal.num_measurements()`

: Return how many measurements we have in the full optimization problem

## Noise modeling

### Noise on the inputs

I solve the calibration problem using Ordinary Least Squares, minimizing the discrepancies between pixel observations and their predictions. The pixel observations \(\vec q_\mathrm{ref}\) are noisy, and I assume that they are zero-mean, independent and normally-distributed. In particular, I treat the 2 values in each observation (\(x\) and \(y\)) as two independent measurements. I have no prior proof that the noise truly meets all those criteria, but empirical evidence suggests that these are all reasonable assumptions. And they simplify lots of analyses that we want to do. For any \(i\) -th observed point, what is \(\mathrm{Var}\left(\vec q_{\mathrm{ref}_i}\right)\)?

The chessboard corner detection routine tells us how confident it was in each
observation, and we use that confidence to estimate the noise level. Chessboard
corner detectors often make it easy to infer the *relative* accuracy levels
between the different corners, as opposed to an *absolute* noise level
for each one. Thus the implementation splits the observed noise into two parts:

- The baseline standard deviation of the noise \(\sigma\) (referred to as the
`observed_pixel_uncertainty`

in the implementation). This is one value that applies to*all*the observations - The scale \(s_i\) applied to that baseline. These are different for each corner

The `mrgingham`

corner detector, in particular, reports the resolution used in
detecting each corner as a decimation level: level-0 is "full-resolution",
level-1 is "half-resolution" and so on. From that decimation level we get the
relative scale

\[ s_i \equiv 2^{\mathrm{level}} \]

and we can define the 2x2 variance for each observed corner

\[ \mathrm{Var}\left( \vec q_{\mathrm{ref}_i} \right) = s_i^2 \sigma^2 I \]

and the variance for all the pixel observations

\[\mathrm{Var}\left(\vec q_\mathrm{ref}\right) = \mathrm{diag}\left(s_i^2\right) \sigma^2 \]

How do we get \(\sigma\)? The noise \(\sigma\) is hard to measure (there's an attempt in mrgingham), but easy to loosely estimate. The current best practice is to get a conservative eyeball estimate to produce conservative estimates of projection uncertainty.

### Noise in the measurement vector \(\vec x\)

We know where each point was observed in reality, and we know where the state vector \(\vec p\) predicts each one would have been observed. So we can construct a vector of errors \(\vec q_\mathrm{err} \equiv \vec q_\mathrm{predicted}\left( \vec p \right) - \vec q_\mathrm{ref}\).

For the purposes of optimization we want to weight the errors of uncertain observations less than confident ones, and to do that we can use the same \(s_i\) scale factor we computed earlier. For point \(i\) I define the weight

\[w_i \equiv \frac{1}{s_i} \]

Let's construct a diagonal matrix of all these weights: \(W \equiv \mathrm{diag}\left( \vec w \right)\). Then the measurement vector is

\[ \vec x_\mathrm{observations} \equiv W q_\mathrm{err} = W \left( \vec q_\mathrm{predicted}\left( \vec p \right) - \vec q_\mathrm{ref} \right) \]

and

\[\mathrm{Var}\left( \vec x_\mathrm{observations} \right) = W \mathrm{Var}\left( \vec q_\mathrm{ref} \right) W^T = \sigma^2 I \]

Thus \(\vec x_\mathrm{observations}\) is homoscedastic: each element as the same variance. I make two more assumptions:

- The rest of the measurement vector \(\vec x\) (regularization) is insignificant
- I consider the linear problem at the local linearization of my nonlinear system

And then I can make a larger statement: the optimal parameter vector we compute from the least-squares optimization is the maximum-likelihood estimate of the true solution.

## Outlier rejection

Some of the input data may not fit the model due to errors in the input data (chessboard corner mis-detections or motion blur for instance) or due to the model not being able to represent reality (insufficiently-flexible lens model or board deformation model for instance). Either of these would violate the noise model, which could bias the resulting estimate.

Currently mrcal employs a very simple outlier-rejection scheme. Pseudo-code:

- do
- optimize
- if any \(x_i > 3 \, \mathrm{max}\left(\sigma, \mathrm{RMS}\left( \vec x \right)\right)\)
- throw out those measurements \(x_i\) as outliers. These are excluded from future reoptimization

- until no new outliers were found

This is effective in handling small numbers of obvious outliers. Any subtle outliers will get through, and will poison the solve. So it is imperative that the input data is as clean as possible. More sophisticated methods are under development.