#+TITLE: Projection uncertainty: cross-reprojection
#+OPTIONS: toc:t


A key part of the [[file:uncertainty.org][uncertainty-in-intrinsics method]] is to compute a function
$\vec q^+\left(\vec b\right)$ to represent the change in projected pixel $\vec
q$ as the optimization vector $\vec b$ moves around.

The *cross-reprojection* method of computing this function is described here.
This is still experimental and not yet complete in mrcal 2.5. It is the future,
and will be the recommended method in mrcal 3.0. This method is accessible by
calling [[file:mrcal-python-api-reference.html#-projection_uncertainty][=mrcal.projection_uncertainty(method = "cross-reprojection-rrp-Jfp")=]].

The logic described here is validated thoroughly in
[[https://www.github.com/dkogan/mrcal/blob/master/test/test-projection-uncertainty.py][=test/test-projection-uncertainty.py=]].

* Cross-reprojection uncertainty
** Cross-reprojection uncertainty via $T_\mathrm{rr^+}$
:PROPERTIES:
:CUSTOM_ID: cross-reprojection-rt-rrp
:END:

In [[file:formulation.org::#measurement-vector][the usual least squares solve using chessboards]], each point produces two
elements (horizontal, vertical error) of the measurements vector:

\[
\vec x_i = W \left( \mathrm{project}\left(T_\mathrm{cr} \, T_\mathrm{rf} \, \vec p_{\mathrm{board}_i}, \vec b_\mathrm{intrinsics} \right) -
\vec q_{\mathrm{ref}_i} \right)
\]

The data flow is:

\[
\xymatrix{
\vec p_\mathrm{board}   \ar[d]^{T_\mathrm{rf}} \\
\vec p_\mathrm{ref}     \ar[d]^{T_\mathrm{cr}} \\
\vec p_\mathrm{cam}     \ar[d]^{\vec b_\mathrm{intrinsics} } \\
\vec q
}
\]

The optimization vector $\vec b$ contains the calibration object warp, which
affects $\vec p_\mathrm{board}$, and it contains $T_\mathrm{cr}$,
$T_\mathrm{rf}$ and $\vec b_\mathrm{intrinsics}$. When optimizing discrete
points the flow is slightly different:

\[
\xymatrix{
\vec p_\mathrm{point}   \ar[d]^{T_\mathrm{cr}} \\
\vec p_\mathrm{cam}     \ar[d]^{\vec b_\mathrm{intrinsics} } \\
\vec q
}
\]

Each point is defined in the reference coordinate system, and each point
coordinate is stored in $\vec b$. The analysis is very similar for the two cases
(boards, points). In this writeup I mostly focus on boards, but the
implementation supports both formulations.

If we perturb $\vec q^+_\mathrm{ref} = \vec
q_\mathrm{ref} + \Delta \vec q_\mathrm{ref}$ and reoptimize, then we get
different quantities in this same data flow:

\[
\xymatrix{
\vec p^+_\mathrm{board}  \ar[d]^{T_\mathrm{r^+f^+}} \\
\vec p^+_\mathrm{ref}    \ar[d]^{T_\mathrm{c^+r^+}} \\
\vec p^+_\mathrm{cam}    \ar[d]^{\vec b^+_\mathrm{intrinsics} } \\
\vec q^+
}
\]

All perturbed quantities are marked with a $+$ superscript.

As noted [[file:uncertainty.org::#propagating-through-projection][previously]], after we optimize a perturbed problem, all the coordinate
systems have shifted, so the two optimizations aren't directly comparable. In
order to gauge the effect of $\Delta \vec q_\mathrm{ref}$ I optimize the
"cross-reprojection error": I treat the two optimized worlds (unperturbed,
perturbed) constant, and I compute an optimal transformation $T_\mathrm{rr^+}$
to relate the two sets of coordinate systems. I do this by solving the original
optimization problem, but using half of the data from each of the unperturbed
and perturbed optimizations. The data flow in this cross-reprojection
optimization appears in red:

\[
\xymatrix{
  \vec p  _\mathrm{board}  \ar[d]^{T_\mathrm{rf}}
& {\color{red} \vec p^+_\mathrm{board}}  \ar@[red][d]^{\color{red} T_\mathrm{r^+f^+}} \\
  {\color{red} \vec p  _\mathrm{ref}}    \ar@[red][d]^{\color{red} T_\mathrm{cr}}
& {\color{red} \vec p^+_\mathrm{ref}}    \ar[d]^{T_\mathrm{c^+r^+}}
  \ar@[red][l]^{\color{red} T_\mathrm{rr^+}} \\
  {\color{red} \vec p  _\mathrm{cam}}    \ar@[red][d]^{\color{red} \vec b_\mathrm{intrinsics} }
& \vec p^+_\mathrm{cam}    \ar[d]^{\vec b^+_\mathrm{intrinsics} } \\
  {\color{red} \vec q}
& \vec q^+
}
\]

All the quantities *except* $T_\mathrm{rr^+}$ are available in the perturbed and
unperturbed optimizations. From those quantities we can compute the optimal
$T_\mathrm{rr^+}$.

In this red data flow I look at the /perturbed/ chessboard,points,frames ($\vec
p^+_\mathrm{board}$, $\vec p^+_\mathrm{point}$, $T_\mathrm{r^+f^+}$) and the
/unperturbed/ camera extrinsics,intrinsics ($T_\mathrm{cr}$, $\vec
b_\mathrm{intrinsics}$). Note that I could have traversed this data flow diagram
from the top-left to the bottom-right instead. This produces a [[#cross-reprojection-rt-rpr][different
formulation, described below]].

So let's compute $T_\mathrm{rr^+}$. For a given perturbation $\Delta \vec
q_\mathrm{ref}$ I solve an optimization problem

\[
\vec{ \mathrm{rt}_\mathrm{rr^+}} = \mathrm{argmin}\Vert \vec x_\mathrm{cross} \Vert^2
\]

where

\begin{aligned}
\vec x_\mathrm{cross} \equiv \,
& W_\mathrm{board} \left( \mathrm{project}\left(
                  T_\mathrm{cr} T_\mathrm{rr^+} T_\mathrm{r^+f^+} \vec p^+_\mathrm{board}, \vec b_\mathrm{intrinsics}\right)
  - \vec q_\mathrm{refboard} \right) + \\
& W_\mathrm{point} \left( \mathrm{project}\left(
                  T_\mathrm{cr} T_\mathrm{rr^+} \vec p^+_\mathrm{point}, \vec b_\mathrm{intrinsics} \right)
  -  \vec q_\mathrm{refpoint} \right)
\end{aligned}

$\vec{ \mathrm{rt}_\mathrm{rr^+}}$ is a single transform (a vector with 6
elements, regardless of how many cameras or observations we have). This is an
[[file:conventions.org::#pose-representation][=rt= pose]] that defines the $T_\mathrm{rr^+}$ transform.

I optimize $\Vert\vec x_\mathrm{cross}\Vert^2$ by making a linearity assumption,
and taking a Newton step from the baseline operating point $\vec
x_\mathrm{cross_0} \equiv \vec x_\mathrm{cross} \left( \vec
{\mathrm{rt}_\mathrm{rr^+}} = 0 \right)$. This is valid because we're assuming
/small/ perturbations $\Delta \vec q_\mathrm{ref}$, and at $\Delta \vec
q_\mathrm{ref} = 0$ the optimum /is/ at this baseline operating point.
Furthermore I have

\[
J_\mathrm{cross} \equiv
\frac{\partial \vec x_\mathrm{cross}}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}}
\]

I assume everything is locally linear, so $\Delta \vec x_\mathrm{cross} \approx
J_\mathrm{cross} \Delta \vec{\mathrm{rt}_\mathrm{rr^+}}$. I minimize $E \equiv
\Vert \vec x_\mathrm{cross_0} + \Delta \vec x_\mathrm{cross}\Vert^2$ by setting
the derivative to $\vec 0$:

\[
0 = \frac{\partial E}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}} \propto (\vec x_\mathrm{cross_0} + \Delta \vec x_\mathrm{cross})^T J_\mathrm{cross}
\]

So

\begin{aligned}
J_\mathrm{cross}^T \vec x_\mathrm{cross_0} &= -J_\mathrm{cross}^T \Delta \vec x_\mathrm{cross} \\
& \approx -J_\mathrm{cross}^T J_\mathrm{cross} \Delta \vec{\mathrm{rt}_\mathrm{rr^+}}
\end{aligned}

and

\[
\vec{\mathrm{rt}_\mathrm{rr^+}} = \vec 0 + \Delta \vec{\mathrm{rt}_\mathrm{rr^+}} \approx -\left(J_\mathrm{cross}^T J_\mathrm{cross}\right)^{-1} J_\mathrm{cross}^T \vec x_\mathrm{cross_0 }
\]

This is good, but requires that $\vec x_\mathrm{cross}$ and $J_\mathrm{cross}$
be computed directly. We can do better.

Since everything I'm looking at is near the original solution to the main
optimization problem, I can look at /everything/ in the linear space defined by
the optimal measurements $\vec x^*$ and their gradient $J$:

\[
\vec x \approx \vec x_0 + J \Delta \vec b
\]

Once again, we have this data flow:

\[
\xymatrix{
  \vec p  _\mathrm{board}  \ar[d]^{T_\mathrm{rf}}
& {\color{red} \vec p^+_\mathrm{board}}  \ar@[red][d]^{\color{red} T_\mathrm{r^+f^+}} \\
  {\color{red} \vec p  _\mathrm{ref}}    \ar@[red][d]^{\color{red} T_\mathrm{cr}}
& {\color{red} \vec p^+_\mathrm{ref}}    \ar[d]^{T_\mathrm{c^+r^+}}
  \ar@[red][l]^{\color{red} T_\mathrm{rr^+}} \\
  {\color{red} \vec p  _\mathrm{cam}}    \ar@[red][d]^{\color{red} \vec b_\mathrm{intrinsics} }
& \vec p^+_\mathrm{cam}    \ar[d]^{\vec b^+_\mathrm{intrinsics} } \\
  {\color{red} \vec q}
& \vec q^+
}
\]

implying this cost vector:

\begin{aligned}
\vec x_\mathrm{cross} \equiv \,
& W_\mathrm{board} \left( \mathrm{project}\left(
                  T_\mathrm{cr} T_\mathrm{rr^+} T_\mathrm{r^+f^+} \vec p^+_\mathrm{board}, \vec b_\mathrm{intrinsics}\right)
  - \vec q_\mathrm{refboard} \right) + \\
& W_\mathrm{point} \left( \mathrm{project}\left(
                  T_\mathrm{cr} T_\mathrm{rr^+} \vec p^+_\mathrm{point}, \vec b_\mathrm{intrinsics}\right)
  -  \vec q_\mathrm{refpoint} \right)
\end{aligned}

I evaluate $\vec x_\mathrm{cross_0} \equiv \vec x_\mathrm{cross} \left( \vec
{\mathrm{rt}_\mathrm{rr^+}} = 0 \right)$. This is $\vec x^*$ from
the original optimization, with perturbed $\vec b_\mathrm{frames}$ and $\vec
b_\mathrm{points}$ and $\vec b_\mathrm{calobjectwarp}$:

\[
\vec x_\mathrm{cross_0} = \vec x^* +
J_\mathrm{frames,points,calobjectwarp} \Delta \vec b_\mathrm{frames,points,calobjectwarp}
\]

To evaluate $J_\mathrm{cross} \equiv \frac{\partial \vec
x_\mathrm{cross}}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}}$ I need to consider
how $\vec x_\mathrm{cross}$ changes in response to
$\vec{\mathrm{rt}_\mathrm{rr^+}}$. I want to look at deviations from the
/original/, unperturbed optimization problem. This can be done in two different
ways, shifting the quantity directly preceding or directly following
$T_\mathrm{rr^+}$ in the above $\vec x_\mathrm{cross}$ expression; for each
measurement I can pick either of these methods:

- We can see $\vec{\mathrm{rt}_\mathrm{rr^+}}$ as shifting $\vec
  {\mathrm{rt}_\mathrm{cr}} \rightarrow \vec {\mathrm{rt}_\mathrm{cr^+}} = \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{cr}},\vec{\mathrm{rt}_\mathrm{rr^+}}
  \right)$:
  \[
  J_{\mathrm{cross}_\mathrm{e}} =
  J_\mathrm{extrinsics} \frac{\partial \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{cr}},\vec{\mathrm{rt}_\mathrm{rr^+}} \right)}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}}
  \]

  For observations that have no extrinsics (the camera is defined to sit at the
  reference coord system or the extrinsics aren't being optimized) this
  formulation is not possible: there is no $J_\mathrm{extrinsics}$

- Or we can see it as a shift $\vec {\mathrm{rt}_\mathrm{rf}} \rightarrow
  \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{rr^+}},\vec{\mathrm{rt}_\mathrm{r^+f^+}}\right)$
  and/or a point shift $\vec p_\mathrm{point} \rightarrow T_\mathrm{rr^+} \vec p^+_\mathrm{point}$

  Since $\vec{\mathrm{rt}_\mathrm{r^+f^+}}$ is a tiny shift off
  $\vec{\mathrm{rt}_\mathrm{rf}}$ /and/ I'm assuming that everything is locally
  linear, I use $\vec{\mathrm{rt}_\mathrm{rf}}$ to compute the gradient instead
  of $\vec{\mathrm{rt}_\mathrm{r^+f^+}}$. Similarly for $p^+$ and $p$:

  \begin{aligned}
  J_{\mathrm{cross}_\mathrm{f}}
            & = J_\mathrm{frame}  \frac{\partial \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{rr^+}},\mathrm{rt}_\mathrm{r^+f^+}\right)}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}} \\
            & \approx J_\mathrm{frame}  \frac{\partial \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{rr^+}},\mathrm{rt}_\mathrm{rf}\right)}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}}
 \\
  J_{\mathrm{cross}_\mathrm{p}}
            & =       J_\mathrm{points} \frac{\partial T_\mathrm{rr^+} p^+}{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}} \\
            & \approx J_\mathrm{points} \frac{\partial T_\mathrm{rr^+} p  }{\partial \vec{\mathrm{rt}_\mathrm{rr^+}}} \\
  \end{aligned}

  For observations that have no frames or points (the chessboard or points are
  sitting at the reference coordinate system origin /or/ these aren't being
  optimized) this formulation is not possible: there is no
  $J_\mathrm{frame}$ or $J_\mathrm{points}$.

There's one more simplification available. We were at an optimum, so we have $\frac{\partial E}{\partial \vec b} =
\frac{\partial}{\partial \vec b} \Vert \vec x \Vert^2 = 0$, and thus $J^T \vec
x^* = 0$.

We can combine the expressions we just computed to simplify:
\begin{aligned}
\vec{\mathrm{rt}_\mathrm{rr^+}} &= -\left(J_\mathrm{cross}^T J_\mathrm{cross}\right)^{-1} J_\mathrm{cross}^T \vec x_\mathrm{cross_0} \\
&= \cdots J_\mathrm{some\_state\_subset}^T \vec x_\mathrm{cross_0} \\
&= \cdots J_\mathrm{some\_state\_subset}^T \left(\vec x^* + \Delta \vec x\right) \\
&= \cdots J_\mathrm{some\_state\_subset}^T \Delta \vec x \\
&= -\left(J_\mathrm{cross}^T J_\mathrm{cross}\right)^{-1} J_\mathrm{cross}^T \Delta \vec x_\mathrm{cross_0}
\end{aligned}

So instead of $\vec x_\mathrm{cross_0}$ we can use

\[
\Delta \vec x_\mathrm{cross_0} = J_\mathrm{frames,points,calobjectwarp} \Delta \vec b_\mathrm{frames,points,calobjectwarp}
\]

So we have $\vec{\mathrm{rt}_\mathrm{rr^+}} = K \Delta \vec b$ for some $K$ that
depends on the various $J$ matrices that are constant for each solve.

** Cross-reprojection uncertainty via $T_\mathrm{r^+r}$
:PROPERTIES:
:CUSTOM_ID: cross-reprojection-rt-rpr
:END:

I can also go the other way: traversing the data flow diagram above from the
top-left to bottom-right:

\[
\xymatrix{
  {\color{red}\vec p  _\mathrm{board}} \ar@[red][d]^{\color{red} T_\mathrm{rf}}
& {           \vec p^+_\mathrm{board}} \ar      [d]^{            T_\mathrm{r^+f^+}} \\
  {\color{red} \vec p _\mathrm{ref}}   \ar      [d]^{            T_\mathrm{cr}}
  \ar@[red][r]^{\color{red} T_\mathrm{r^+r}}
& {\color{red} \vec p^+_\mathrm{ref}}  \ar@[red][d]^{\color{red} T_\mathrm{c^+r^+}} \\
  {            \vec p  _\mathrm{cam}}  \ar      [d]^{            \vec b_\mathrm{intrinsics} }
& {\color{red} \vec p^+_\mathrm{cam}}  \ar@[red][d]^{\color{red} \vec b^+_\mathrm{intrinsics} } \\
  {            \vec q}
& {\color{red} \vec q^+}
}
\]

The derivation differs only slightly. We have

\begin{aligned}
\vec x_\mathrm{cross} \equiv \,
& W_\mathrm{board} \left( \mathrm{project}\left(
                  T_\mathrm{c^+r^+} T_\mathrm{r^+r} T_\mathrm{rf} \vec p_\mathrm{board}, \vec b^+_\mathrm{intrinsics} \right)
  - \vec q^+_\mathrm{refboard} \right) + \\
& W_\mathrm{point} \left( \mathrm{project}\left(
                  T_\mathrm{c^+r^+} T_\mathrm{r^+r} \vec p_\mathrm{point}, \vec b^+_\mathrm{intrinsics} \right)
  -  \vec q^+_\mathrm{refpoint} \right)
\end{aligned}

And the optimum is similarly at

\[
\vec{\mathrm{rt}_\mathrm{r^+r}} = -\left(J_\mathrm{cross}^T J_\mathrm{cross}\right)^{-1} J_\mathrm{cross}^T \Delta \vec x_\mathrm{cross_0}
\]

where

\[
\Delta \vec x_\mathrm{cross_0} = J_\mathrm{intrinsics,extrinsics} \Delta \vec b_\mathrm{intrinsics,extrinsics} - W \Delta \vec q_\mathrm{ref}
\]

When evaluating $J_\mathrm{cross} = \frac{\partial \vec
x_\mathrm{cross}}{\partial \vec{\mathrm{rt}_\mathrm{r^+r}}}$ I can once again
look at it in two ways (for each measurement I can pick either of these
methods):

- a shift $\vec{\mathrm{rt}_\mathrm{cr}} \rightarrow \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{c^+r^+}},\vec{\mathrm{rt}_\mathrm{r^+r}}\right)$.

  Since $\vec{\mathrm{rt}_\mathrm{c^+r^+}}$ is a tiny shift off
  $\vec{\mathrm{rt}_\mathrm{cr}}$ /and/ I'm assuming that everything is locally
  linear, I use
  $\vec{\mathrm{rt}_\mathrm{cr}}$ to compute the gradient instead of $\vec{\mathrm{rt}_\mathrm{c^+r^+}}$

  \begin{aligned}
  J_{\mathrm{cross}_\mathrm{e}}
            & = J_\mathrm{extrinsics} \frac{\partial \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{c^+r^+}},\vec{\mathrm{rt}_\mathrm{r^+r}}\right)}{\partial \vec{\mathrm{rt}_\mathrm{r^+r}}} \\
            & \approx J_\mathrm{extrinsics} \frac{\partial \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{cr}},  \vec{\mathrm{rt}_\mathrm{r^+r}}\right)}{\partial \vec{\mathrm{rt}_\mathrm{r^+r}}}
  \end{aligned}

  As before, for observations that have no extrinsics (the camera is defined to
  sit at the reference coord system or the extrinsics aren't being optimized)
  there is no $J_\mathrm{extrinsics}$, so this formulation is not possible. Use
  $J_{\mathrm{cross}_\mathrm{f}}$ and/or $J_{\mathrm{cross}_\mathrm{p}}$

- a shift $\vec {\mathrm{rt}_\mathrm{rf}} \rightarrow \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{r^+r}}, \vec {\mathrm{rt}_\mathrm{rf}}\right)$ and/or a point
  shift $\vec p_\mathrm{point} \rightarrow T_\mathrm{r^+r} \vec p_\mathrm{point}$:

  \begin{aligned}
  J_{\mathrm{cross}_\mathrm{f}} & = J_\mathrm{frame} \frac{\partial \mathrm{compose\_rt}\left(\vec{\mathrm{rt}_\mathrm{r^+r}},\vec {\mathrm{rt}_\mathrm{rf}}\right)}{\vec{\mathrm{rt}_\mathrm{r^+r}}} \\
  J_{\mathrm{cross}_\mathrm{p}} & = J_\mathrm{points} \frac{T_\mathrm{r^+r} \vec p}{\partial \vec{\mathrm{rt}_\mathrm{r^+r}}}
  \end{aligned}

  As before, if there is no $J_\mathrm{frame}$ or $J_\mathrm{points}$, this
  isn't possible: use $J_{\mathrm{cross}_\mathrm{e}}$.

So we have $\vec{\mathrm{rt}_\mathrm{r^+r}} = K \Delta \vec b - W \Delta \vec
q_\mathrm{ref}$ for some $K$ that depends on the various $J$ matrices that are
constant for each solve.

** Putting it all together
Now that I have $\vec{\mathrm{rt}_\mathrm{rr^+}}$ or
$\vec{\mathrm{rt}_\mathrm{r^+r}}$, I can use it to compute $\vec q^+$. This can
accept arbitrary $\vec q$, not just those in the solve, so I actually need to
compute projections, rather than looking at a linearized space defined by $J$. I
traverse the data flow diagram in a different direction to compute $\vec q^+$:

\[
\xymatrix{
  {\vec p  _\mathrm{ref}} \ar[r]^{T_\mathrm{r^+r}}
& {\vec p^+_\mathrm{ref}}    \ar[d]^{T_\mathrm{c^+r^+}} \\
  {\vec p  _\mathrm{cam}} \ar[u]_{T_\mathrm{rc}}
& {\vec p^+_\mathrm{cam}}    \ar[d]^{\vec b^+_\mathrm{intrinsics} } \\
  {\vec q} \ar[u]_{\vec b_\mathrm{intrinsics} }
& {\vec q^+}
}
\]

So
\begin{aligned}
\vec p_\mathrm{ref}   & = T_\mathrm{rc} \mathrm{unproject}\left(\vec q, \vec b_\mathrm{intrinsics} \right) \\
\vec p^+_\mathrm{ref} & = T_\mathrm{r^+r} \vec p_\mathrm{ref} \\
\vec p^+_\mathrm{cam} & = T_\mathrm{c^+r^+} \vec p^+_\mathrm{ref} \\
\vec q^+              & = \mathrm{project}\left(\vec p^+_\mathrm{cam}, \vec b^+_\mathrm{intrinsics} \right)
\end{aligned}

With these expressions I can compute the gradient of $\vec q^+$ in respect to
all the variables, and I can propagate those gradients to get

\[ \mathrm{Var}\left( \vec q^+ \right) = \frac{\partial \vec q^+}{\partial \vec b} \mathrm{Var}\left( \vec b \right) \frac{\partial \vec q^+}{\partial \vec b}^T \]

* init                                                             :noexport:
Need to do this to render the latex snippets with C-c C-x C-l

(add-to-list 'org-latex-packages-alist '("all,cmtip,color,matrix,arrow" "xy" t))
(add-to-list 'org-latex-packages-alist '("" "color" t))
