Projection uncertainty: cross-reprojection

In 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} \rightarrow \vec q^+_\mathrm{ref} = \vec q_\mathrm{ref} + \Delta \vec q_\mathrm{ref}\) then we have a different optimization, with 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 above, 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) contant, 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 (\(\color{red} \vec p^+_\mathrm{board}\), \(\color{red} \vec p^+_\mathrm{point}\), \(\color{red} 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 different formulation, described below.

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

\[ \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 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 operating point \(\vec {\mathrm{rt}_\mathrm{rr^+}} = 0\). The baseline \(\vec x_\mathrm{cross_0}\) comes from the above expression at \(\vec {\mathrm{rt}_\mathrm{rr^+}} = 0\). 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

\[ \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 } \]

The operating point is at \(\vec{\mathrm{rt}_\mathrm{rr^+}} = 0\) so

\begin{aligned} \vec{\mathrm{rt}_\mathrm{rr^+}} &= 0 + \Delta \vec{\mathrm{rt}_\mathrm{rr^+}} \\ &= -\left(J_\mathrm{cross}^T J_\mathrm{cross}\right)^{-1} J_\mathrm{cross}^T \vec x_\mathrm{cross_0} \end{aligned}

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}\) at \(\vec{\mathrm{rt}_\mathrm{rr^+}} = 0\). This is exactly the \(\vec x^*\) from the original optimization, except I perturb \(\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:

• 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 referene coord system) 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}

Each observation can use a different \(J_\mathrm{cross}\) form, as appropriate.

There's one more simplification available. The original optimization problem was solved, 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.