#+TITLE: Optimization problem formulation
#+OPTIONS: toc:t

* Overview
mrcal contains a solver used to compute the [[file:lensmodels.org][lens models]] and/or geometry. This is
accessible via either

- =mrcal_optimize()= routine in the [[file:c-api.org][C API]]
- [[file:mrcal-python-api-reference.html#-optimize][=mrcal.optimize()=]] routine in the [[file:python-api.org][Python API]]

These are the main call in the [[file:mrcal-calibrate-cameras.html][=mrcal-calibrate-cameras=]] tool (to calibrate
cameras) and [[file:mrcal-convert-lensmodel.html][=mrcal-convert-lensmodel=]] tool (to fit a different lens model into
an existing model). The optimization routines are more general than what these
tools provide, 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 b$, which contains all the
geometry and all the lens models. For any hypothesis $\vec b$, 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 b$ to minimize the cost function $E \equiv \left \Vert \vec x
\left(\vec b\right)\right \Vert ^2$.

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

- =mrcal_optimizer_callback()= routine in the [[file:c-api.org][C API]]
- [[file:mrcal-python-api-reference.html#-optimizer_callback][=mrcal.optimizer_callback()=]] routine in the [[file:python-api.org][Python API]]

* World geometry
:PROPERTIES:
:CUSTOM_ID: world-geometry
:END:
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. Note that this coordinate system doesn't necessarily have
  any physical meaning.
- *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. 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 camera pose
$T_\mathrm{cr}$ camera and 10 separate chessboard poses $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, setting
$T_\mathrm{cr}$ to the identity transformation. 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 the $T_\mathrm{cr}$ is enough.

** 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 it would shift the camera and chessboard
coordinate systems. The [[file:uncertainty.org][projection uncertainty]] documentation talks about this in
depth.

* Calibration object
:PROPERTIES:
:CUSTOM_ID: calibration-object
:END:
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 regular grid of
observable points, possibly with a small amount of [[#board-deformation][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.

Detections of these corners serve as the input features to mrcal. mrcal is a
purely geometrical toolkit, and this vision problem must be [[file:how-to-calibrate.org::#corner-detector][handled by another
library]].

** Board deformation
:PROPERTIES:
:CUSTOM_ID: board-deformation
:END:

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 currently supports a simple 2-parameter deformation model.
This model uses 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.

Clearly, this does not work for especially strong or asymmetric deflections.
There's a richer 5-parameter deformation model in a not-yet-released branch that
appears to work even for asymmetric deflections. This needs more testing, and
has not yet been released. Talk to Dima if you want to play with it.

* Lens behavior
:PROPERTIES:
:CUSTOM_ID: lens-behavior
:END:
The fundamental operation to map a point in the camera coordinate system to a
pixel where that point would be observed by the camera is called /projection/.
mrcal supports [[file:lensmodels.org][multiple methods]] to model the behavior of different lenses. Of
particular note is that at this time, mrcal assumes that all projections are
/central/: all rays of light are assumed to intersect at a single point (the
origin of the camera coordinate system). So $k \vec v$ projects to the same
$\vec q$ for any $k$. This is very convenient, mostly true, and almost every
camera library assumes this. This assumption breaks down when projecting
observations /very/ close to the lens, with "close" being judged relative to the
physical size of the lens. Extreme close-ups exhibit /non-central/ behavior:

[[file:figures/noncentral.svg]]

While [[file:tour-choreography.org::#choreography-distance][extreme close-ups are very good for calibration quality]], the working
distance is usually past the range where non-central effects are observable. So
it is recommended to avoid the non-central regime by keeping the chessboard
far-enough out to make the central model work well. [[File:tour-cross-validation.org][Cross-validation]] can be used
to verify whether noncentral effects are present. That said, support for
/non-central/ lenses is coming in a future release of mrcal, which will allow us
to model the noncentral behavior.

* Optimization details
The mrcal solver is an optimization routine based on sparse nonlinear least
squares. The optimization loop is implemented in [[https://www.github.com/dkogan/libdogleg][=libdogleg=]], which at its core
uses the [[https://people.engr.tamu.edu/davis/suitesparse.html][CHOLMOD solver]] to compute the [[https://en.wikipedia.org/wiki/Cholesky_decomposition][Cholesky factorization]], to then
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
[[https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector][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
[[file:mrcal-calibrate-cameras.html][=mrcal-calibrate-cameras=]] tool achieves this by

1. Computing an initial estimate directly using geometry and some simplifying
   assumptions. These geometric seeding routines are available standalone:

   - [[file:mrcal-python-api-reference.html#-estimate_monocular_calobject_poses_Rt_tocam][=mrcal.estimate_monocular_calobject_poses_Rt_tocam()=]]: Estimate camera-referenced poses of the calibration object from monocular views
   - [[file:mrcal-python-api-reference.html#-estimate_joint_frame_poses][=mrcal.estimate_joint_frame_poses()=]]: Estimate world-referenced poses of the calibration object
   - [[file:mrcal-python-api-reference.html#-seed_stereographic][=mrcal.seed_stereographic()=]]: Compute an optimization seed for a camera calibration

2. 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 [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_problem_selections_t=]] structure
   passed to [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_optimize()=]] in C (or the =do_optimize_...= arguments to
   [[file:mrcal-python-api-reference.html#-optimize][=mrcal.optimize()=]] in Python).

* State vector $\vec b$
:PROPERTIES:
:CUSTOM_ID: state-vector
:END:
The state vector $\vec b$ 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 [[#board-deformation][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 b$ contains intrinsics and extrinsics and frames and the
  calibration-object warp
- Structure-from-motion. We have moving cameras observing a stationary world.
  $\vec b$ contains extrinsics and points.
- An intrinsics-fitting problem such as what [[file:mrcal-convert-lensmodel.html][=mrcal-convert-lensmodel=]] solves.
  $\vec b$ 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 b$. mrcal provides a
number of functions to report the index and size of the block of $\vec b$ that
contains specific data. In C:

- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_state_index_intrinsics()=]]: Return the index in the optimization vector of the intrinsics of camera i
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_state_index_extrinsics()=]]: Return the index in the optimization vector of the extrinsics of camera i
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_state_index_frames()=]]: Return the index in the optimization vector of the pose of frame i
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_state_index_points()=]]: Return the index in the optimization vector of the position of point i
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_state_index_calobject_warp()=]]: Return the index in the optimization vector of the calibration object warp

- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_states_intrinsics()=]]: Get the number of intrinsics parameters in the optimization vector
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_states_extrinsics()=]]: Get the number of extrinsics parameters in the optimization vector
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_states_frames()=]]: Get the number of calibration object pose parameters in the optimization vector
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_states_points()=]]: Get the number of point-position parameters in the optimization vector
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_states_calobject_warp()=]]: Get the number of parameters in the optimization vector for the board warp

- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_states()=]]: Get the full length of the optimization vector

And in Python:

- [[file:mrcal-python-api-reference.html#-state_index_intrinsics][=mrcal.state_index_intrinsics()=]]: Return the index in the optimization vector of the intrinsics of camera i
- [[file:mrcal-python-api-reference.html#-state_index_extrinsics][=mrcal.state_index_extrinsics()=]]: Return the index in the optimization vector of the extrinsics of camera i
- [[file:mrcal-python-api-reference.html#-state_index_frames][=mrcal.state_index_frames()=]]: Return the index in the optimization vector of the pose of frame i
- [[file:mrcal-python-api-reference.html#-state_index_points][=mrcal.state_index_points()=]]: Return the index in the optimization vector of the position of point i
- [[file:mrcal-python-api-reference.html#-state_index_calobject_warp][=mrcal.state_index_calobject_warp()=]]: Return the index in the optimization vector of the calibration object warp

- [[file:mrcal-python-api-reference.html#-num_states_intrinsics][=mrcal.num_states_intrinsics()=]]: Get the number of intrinsics parameters in the optimization vector
- [[file:mrcal-python-api-reference.html#-num_states_extrinsics][=mrcal.num_states_extrinsics()=]]: Get the number of extrinsics parameters in the optimization vector
- [[file:mrcal-python-api-reference.html#-num_states_frames][=mrcal.num_states_frames()=]]: Get the number of calibration object pose parameters in the optimization vector
- [[file:mrcal-python-api-reference.html#-num_states_points][=mrcal.num_states_points()=]]: Get the number of point-position parameters in the optimization vector
- [[file:mrcal-python-api-reference.html#-num_states_calobject_warp][=mrcal.num_states_calobject_warp()=]]: Get the number of parameters in the optimization vector for the board warp

- [[file:mrcal-python-api-reference.html#-num_states][=mrcal.num_states()=]]: Get the full length of the optimization vector

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

- [[file:mrcal-python-api-reference.html#-plotoptions_state_boundaries][=mrcal.plotoptions_state_boundaries()=]]: Return the =set= plot options for gnuplotlib to show the state boundaries

** State vector scaling
:PROPERTIES:
:CUSTOM_ID: state-packing
:END:
The nonlinear least squares-solving library used by mrcal is [[https://www.github.com/dkogan/libdogleg][=libdogleg=]], which
implements [[https://en.wikipedia.org/wiki/Powell's_dog_leg_method][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 b$ 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 b$.

Clearly getting identical behavior from each variable is impossible, but we can
scale the elements of $\vec b$ to keep things more or less even. mrcal applies
this scaling, and the =libdogleg= optimization library never sees the full state
vector $\vec b$, but the scaled vector $\vec b_\mathrm{packed}$. Similarly, it
never sees the full jacobian $J \equiv \frac{\partial \vec x}{\partial \vec b}$,
but rather $J_\mathrm{packed} \equiv \frac{\partial \vec x}{\partial \vec
b_\mathrm{packed}}$. This means that the optimization callback functions report
packed state. These are

- =mrcal_optimizer_callback()= routine in the [[file:c-api.org][C API]]
- [[file:mrcal-python-api-reference.html#-optimizer_callback][=mrcal.optimizer_callback()=]] routine in the [[file:python-api.org][Python API]]

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

- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_pack_solver_state_vector()=]]: Scales a state vector to the packed, unitless form used by the optimizer
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_unpack_solver_state_vector()=]]: Scales a state vector from the packed, unitless form used by the optimizer

And in Python:

- [[file:mrcal-python-api-reference.html#-pack_state][=mrcal.pack_state()=]]: Scales a state vector to the packed, unitless form used by the optimizer
- [[file:mrcal-python-api-reference.html#-unpack_state][=mrcal.unpack_state()=]]: Scales a state vector from the packed, unitless form used by the optimizer

* Measurement vector $\vec x$
:PROPERTIES:
:CUSTOM_ID: measurement-vector
:END:

Given a hypothesis state vector $\vec b$ mrcal computes a vector of errors, or
/measurements/ $\vec x$. The optimization algorithm searches the space of
hypotheses $\vec b$, 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 b$ 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 b \right) - \vec q_\mathrm{ref}$.

From the [[#noise-model][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 b \right) - \vec q_\mathrm{ref} \right) \]

This is the bulk of the measurement vector.

** Regularization
:PROPERTIES:
:CUSTOM_ID: Regularization
:END:

In addition to $\vec x_\mathrm{observations}$, the measurement vector contains
[[https://en.wikipedia.org/wiki/Regularization_(mathematics)][/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 [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.c][=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 [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_problem_selections_t=]] or the
=do_apply_regularization= key in the call to [[file:mrcal-python-api-reference.html#-optimize][=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:

- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_measurement_index_boards()=]]: Return the measurement index of the start of a given board observation
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_measurement_index_points()=]]: Return the measurement index of the start of a given point observation
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_measurement_index_regularization()=]]: Return the index of the start of the regularization measurements
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_measurements_boards()=]]: Return how many measurements we have from calibration object observations
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_measurements_points()=]]: Return how many measurements we have from point observations
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_num_measurements_regularization()=]]: Return how many measurements we have from regularization
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal_measurements()=]]: Return how many measurements we have in the full optimization problem

And in Python:

- [[file:mrcal-python-api-reference.html#-measurement_index_boards][=mrcal.measurement_index_boards()=]]: Return the measurement index of the start of a given board observation
- [[file:mrcal-python-api-reference.html#-measurement_index_points][=mrcal.measurement_index_points()=]]: Return the measurement index of the start of a given point observation
- [[file:mrcal-python-api-reference.html#-measurement_index_regularization][=mrcal.measurement_index_regularization()=]]: Return the index of the start of the regularization measurements
- [[file:mrcal-python-api-reference.html#-num_measurements_boards][=mrcal.num_measurements_boards()=]]: Return how many measurements we have from calibration object observations
- [[file:mrcal-python-api-reference.html#-num_measurements_points][=mrcal.num_measurements_points()=]]: Return how many measurements we have from point observations
- [[file:mrcal-python-api-reference.html#-num_measurements_regularization][=mrcal.num_measurements_regularization()=]]: Return how many measurements we have from regularization
- [[file:mrcal-python-api-reference.html#-num_measurements][=mrcal.num_measurements()=]]: Return how many measurements we have in the full optimization problem

If plotting a whole vector of measurements, it is helpful to annotate the plot
to make it clear which values correspond to each block of measurements. mrcal
provides a helper function to help with this:

- [[file:mrcal-python-api-reference.html#-plotoptions_measurement_boundaries][=mrcal.plotoptions_measurement_boundaries()=]]: Return the =set= plot options for gnuplotlib to show the measurement boundaries

* Noise modeling
:PROPERTIES:
:CUSTOM_ID: noise-model
:END:

The [[file:uncertainty.org][projection uncertainty routine]] is used to gauge the effects of sampling
error on the solve. This is done by modelling the noise in the input pixel
observations, and propagating it through the solve. This assumes that model
errors are insignificant.

If the model errors /were/ significant, then

- the computed projection uncertainty would underestimate the expected errors:
  the non-negligible model errors would be ignored
- the computed calibration would be biased: the residuals $\vec x$ would be
  heteroscedastic, so the computed optimum would /not/ be a maximum-likelihood
  estimate of the true calibration

** Noise on the inputs
:PROPERTIES:
:CUSTOM_ID: noise-model-inputs
:END:

I solve the calibration problem using [[https://en.wikipedia.org/wiki/Ordinary_least_squares][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.
There's no 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. In order to propagate the input noise, we
need to quantify it: for the $i$ -th observed point, what is
$\mathrm{Var}\left(\vec q_{\mathrm{ref}_i}\right)$?

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$. This is one value that
  applies to /all/ the observations
- The scale $s_i$ applied to that baseline. These are different for each
  observed corner

The [[https://github.com/dkogan/mrgingham/][=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_2 \]

and the variance for all the pixel observations is the block-diagonal

\[\mathrm{Var}\left(\vec q_\mathrm{ref}\right) = \mathrm{diag}_i \left(\mathrm{Var}\left( \vec q_{\mathrm{ref}_i} \right)\right) \]

The remaining piece is to compute $\sigma$, but this is hard to measure
directly. There's an [[https://github.com/dkogan/mrgingham/blob/master/mrgingham-observe-pixel-uncertainty][attempt]] in mrgingham, but it doesn't obviously work well.
Thus the current method is to estimate $\sigma$ from the solve residuals.

** Estimating the input noise
:PROPERTIES:
:CUSTOM_ID: estimating-input-noise
:END:

I'm trying to find a relationship between the solve residuals $\vec
x^*$ (which I have from the solve) and the input noise
$\Delta \vec q_\mathrm{ref}$.

I use $\vec x^*$ to signify the measurement vector at the optimum. For this
analysis I make a simplifying assumption that the regularization terms are
insignificant and that the whole measurement vector $\vec x$ contains
observations.

Let's start with a perfect noise-free solve. The observations $\vec
q_\mathrm{ref}$ are perfect and $\vec x = \vec 0$.

Then I add some input noise $\Delta \vec q_\mathrm{ref}$. I now have

\[\vec x = -W \Delta \vec q_\mathrm{ref}\]

And I reoptimize. As noted on the [[file:uncertainty.org::#propagating-to-state-vector][projection uncertainty page]] this moves the
optimization state by

\[
\Delta \vec b = \left( J^T J \right)^{-1} J^T W \Delta \vec q_\mathrm{ref}
\]

I assume that everything is locally linear, so we can quantify the corresponding
shift in the measurement vector:

\begin{aligned}
\vec x^* &= -W \Delta \vec q_\mathrm{ref} + \Delta \vec x \\
&= -W \Delta \vec q_\mathrm{ref} + J \Delta \vec b \\
&= -W \Delta \vec q_\mathrm{ref} + J \left( J^T J \right)^{-1} J^T W \Delta \vec q_\mathrm{ref} \\
&= \left( -I + J \left( J^T J \right)^{-1} J^T \right) W \Delta \vec q_\mathrm{ref}
\end{aligned}

And

\[
\left\Vert \vec x^* \right\Vert^2 =
\Delta \vec q_\mathrm{ref}^T W \left( I - J \left( J^T J \right)^{-1} J^T \right) W \Delta \vec q_\mathrm{ref}
\]

Let's treat $\Delta \vec q_\mathrm{ref}$ as a random variable, and look at the
expectation:

\begin{aligned}
E\left( \left\Vert \vec x^* \right\Vert^2 \right)
&=
\mathrm{tr}
\left[ \left( I - J \left( J^T J \right)^{-1} J^T \right) \mathrm{Var}
\left(
W \Delta \vec q_\mathrm{ref}
\right) \right] + E\left( W \Delta \vec q_\mathrm{ref} \right)^T \mathrm{Var}\left( \cdot \right) E\left( W \Delta \vec q_\mathrm{ref} \right)
\end{aligned}

$\Delta \vec q_\mathrm{ref}$ is 0-mean, so its expectation is 0. And as noted
[[#noise-in-measurement-vector][below]], $\mathrm{Var}\left(W \Delta \vec q_\mathrm{ref}\right) = \sigma^2 I$. So

\begin{aligned}
E\left( \left\Vert \vec x^* \right\Vert^2 \right)
&=
\mathrm{tr}
\left( I - J \left( J^T J \right)^{-1} J^T \right)
\sigma^2 \\
&=
\left(N_\mathrm{measurements} - N_\mathrm{state}\right) \sigma^2
\end{aligned}


So the expected RMS reprojection error in a solve is

\[
\sqrt{ \frac{E\left( \left\Vert \vec x^* \right\Vert^2 \right)}{N_\mathrm{measurements}}} =
\sqrt{ \frac{\left(N_\mathrm{measurements} - N_\mathrm{state}\right) \sigma^2}{N_\mathrm{measurements}}} =
\sigma \sqrt{1 - \frac{N_\mathrm{state}}{N_\mathrm{measurements}}}
\]

If we have information-full data, such as chessboard observations, then
$N_\mathrm{measurements} >> N_\mathrm{state}$, and we can simply assume that the
standard deviation of the input observation noise $\sigma$ is the RMS
reprojection error in the solve. It's possible that with lower-quality data the
extra correction factor of $\sqrt{1 -
\frac{N_\mathrm{state}}{N_\mathrm{measurements}}}$ cannot be ignored.

Today, mrcal /does/ ignore this extra correction factor: the
=_observed_pixel_uncertainty_from_inputs()= implementation in
[[https://www.github.com/dkogan/mrcal/blob/master/mrcal/model_analysis.py][=model_analysis.py=]] uses $\sigma$ only because it's not clear if regularization
terms should count as "measurements" here. I /have/ seen this factor be
required, but only when running very funky experiments. The
[[https://www.github.com/dkogan/mrcal/blob/master/analyses/validate-input-noise.py][=analyses/validate-input-noise.py=]] tool reports whether the noise assumptions
are valid for any given solve. For the [[file:tour-initial-calibration.org::#splined-stereographic-fit][splined-model solve in the tour of mrcal]]:

#+begin_example
$ analyses/validate-input-noise.py ../doc/data/figueroa-overpass-looking-S/splined.cameramodel

Noise ratios measured/actual. Should be ~ 1.0
  observed, by looking at the distribution of residulas: 0.981
  predicted, by correcting the above by sqrt(1-Nstates/Nmeasurements_observed): 1.012
#+end_example

** Noise in the measurement vector $\vec x$
:PROPERTIES:
:CUSTOM_ID: noise-in-measurement-vector
:END:
We know where each point was observed in reality, and we know where the state
vector $\vec b$ 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 b \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 b \right) - \vec q_\mathrm{ref} \right) \]

If we assume that the model fits the data and that we have enough data to not
overfit (both reasonable assumptions), then 

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

where $\sigma$ is the input noise we're propagating. Furthermore, $\vec
x_\mathrm{observations}$ is homoscedastic: each element as the same variance. I
make two more (reasonable) assumptions:

- The rest of the measurement vector $\vec x$ ([[#Regularization][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
:PROPERTIES:
:CUSTOM_ID: outlier-rejection
:END:

Some of the input 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][board deformation model]] for instance). Either of these would violate the [[#noise-model][noise
model]], which could bias the resulting estimate. Finding and detecting such
points would eliminate such a bias.

Currently mrcal employs a very simple outlier-rejection scheme. More or less,
all measurements that have $x_i$ beyond some $k$ standard deviations above 0 are
thrown out as outliers. See [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.c][=markOutliers()=]] for details.

This scheme 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.