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} = \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 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 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 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.

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.

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