Pose Graph Optimization
In the bundle adjustment problem, we want to optimize the camera poses and the 3D points by minimizing the reprojection error, which is denoted as,
\begin{equation}
\begin{aligned}
& \arg \min_{\mathbf{T}_{CWi}, \mathbf{X}_{Wj}} \left( \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\| \mathbf{x}_j - \mathbf{K} \mathbf{T}_{CWi} \mathbf{X}_{Wj} \right\|_2^2 \right) \\
&= \arg \min_{\mathbf{T}_{CWi}, \mathbf{X}_{Wj}} \left( \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\| \mathbf{x}_j - \pi (\mathbf{T}_{CWi}, \mathbf{X}_{Wj}) \right\|_2^2 \right) \\
&= \arg \min_{\mathbf{T}_{CWi}, \mathbf{X}_{Wj}} f = \arg \min_{\mathbf{T}_{CWi}, \mathbf{X}_{Wj}} \left( \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n} \mathbf{e}_{ij}^2 \right) \\
\label{eq:reproj_error}
\end{aligned}
\end{equation}
The variables we want to optimize are the camera poses and the 3D points, which are denoted as:
\begin{equation}
\mathbf{\mathcal{X}} = [\mathbf{T}_1, \ldots, \mathbf{T}_m, \mathbf{X}_1, \ldots, \mathbf{X}_n]^\top
\end{equation}
recall the Gaussian-Newton method for solving the non-linear least squares problem,
\begin{equation}
f(\mathbf{x} + \Delta\mathbf{x})
\approx f(\mathbf{x}) + \mathbf{J}(\mathbf{x})^T \Delta\mathbf{x}
\end{equation}
\begin{equation}
\Delta\mathbf{x}^* =
\arg\min_{\Delta\mathbf{x}}
\tfrac{1}{2}\|f(\mathbf{x}) + \mathbf{J}(\mathbf{x})^T\Delta\mathbf{x}\|^2
\end{equation}
\begin{equation}
\mathbf{J}(\mathbf{x})^T\mathbf{J}(\mathbf{x})\Delta\mathbf{x}
= -\mathbf{J}(\mathbf{x})^T f(\mathbf{x})
\end{equation}
\begin{equation}
\mathbf{H}\Delta\mathbf{x} = \mathbf{g}
\end{equation}
where $\mathbf{H}$ is the second-order approximation of the Hessian.
Appy to this BA problem,
\begin{equation}
\begin{aligned}
\frac{1}{2}\| f(\mathbf{\mathcal{X}} + \Delta\mathbf{\mathcal{X}})\|^2
&\approx \frac{1}{2}\sum_{i=1}^m \sum_{j=1}^n
\|\mathbf{F}_{ij}\Delta\boldsymbol{\xi}_i + \mathbf{E}_{ij}\Delta\mathbf{X}_{j}\|^2 \\
&= \frac{1}{2}\|\mathbf{e} + \mathbf{F}\Delta \mathbf{\mathcal{X}_c} + \mathbf{E}\Delta\mathbf{\mathcal{X}_p}\|^2
\end{aligned}
\end{equation}
\begin{equation}
\mathbf{H}\Delta\mathbf{\mathcal{X}} = \mathbf{g}
\end{equation}
\begin{equation}
\mathbf{J} = [\mathbf{F}~\mathbf{E}]
\end{equation}
\begin{equation}
\mathbf{H} = \mathbf{J}^T\mathbf{J} =
\begin{bmatrix}
\mathbf{F}^T\mathbf{F} & \mathbf{F}^T\mathbf{E} \\
\mathbf{E}^T\mathbf{F} & \mathbf{E}^T\mathbf{E}
\end{bmatrix}
\end{equation}
\begin{equation}
\mathbf{J}_{ij}(\mathbf{x}) =
( \mathbf{0}_{2\times6}, \ldots,
\tfrac{\partial \mathbf{e}_{ij}}{\partial \mathbf{T}_i},
\mathbf{0}_{2\times6}, \ldots,
\tfrac{\partial \mathbf{e}_{ij}}{\partial \mathbf{p}_j},
\mathbf{0}_{2\times3}, \ldots )
\end{equation}
\begin{equation}
\mathbf{H} = \sum_{i,j} \mathbf{J}_{ij}^T \mathbf{J}_{ij}
\end{equation}
\begin{equation}
\mathbf{H} =
\begin{bmatrix}
\mathbf{H}_{11} & \mathbf{H}_{12} \\
\mathbf{H}_{21} & \mathbf{H}_{22}
\end{bmatrix}
\end{equation}
\begin{equation}
\frac{1}{2}\sum \|\mathbf{e}_{ij}\|^2
= \tfrac{1}{2}(\|\mathbf{e}_{11}\|^2+\|\mathbf{e}_{12}\|^2+\|\mathbf{e}_{13}\|^2+
\|\mathbf{e}_{14}\|^2+\|\mathbf{e}_{23}\|^2+\|\mathbf{e}_{25}\|^2+\|\mathbf{e}_{26}\|^2)
\end{equation}
\begin{equation}
\mathbf{J}_{11} =
\frac{\partial \mathbf{e}_{11}}{\partial \mathbf{x}} =
( \tfrac{\partial \mathbf{e}_{11}}{\partial \boldsymbol{\xi}_1},
\mathbf{0}_{2\times6},
\tfrac{\partial \mathbf{e}_{11}}{\partial \mathbf{p}_1},
\mathbf{0}_{2\times3}, \ldots )
\end{equation}
\begin{equation}
\begin{bmatrix}
\mathbf{B} & \mathbf{E} \\
\mathbf{E}^T & \mathbf{C}
\end{bmatrix}
\begin{bmatrix}
\Delta\mathbf{x}_c \\ \Delta\mathbf{x}_p
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{v} \\ \mathbf{w}
\end{bmatrix}
\end{equation}
\begin{equation}
\begin{bmatrix}
\mathbf{I} & -\mathbf{E}\mathbf{C}^{-1} \\
\mathbf{0} & \mathbf{I}
\end{bmatrix}
\begin{bmatrix}
\mathbf{B} & \mathbf{E} \\
\mathbf{E}^T & \mathbf{C}
\end{bmatrix}
\begin{bmatrix}
\Delta\mathbf{x}_c \\ \Delta\mathbf{x}_p
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{I} & -\mathbf{E}\mathbf{C}^{-1} \\
\mathbf{0} & \mathbf{I}
\end{bmatrix}
\begin{bmatrix}
\mathbf{v} \\ \mathbf{w}
\end{bmatrix}
\end{equation}
\begin{equation}
\begin{bmatrix}
\mathbf{B} - \mathbf{E}\mathbf{C}^{-1}\mathbf{E}^T & \mathbf{0} \\
\mathbf{E}^T & \mathbf{C}
\end{bmatrix}
\begin{bmatrix}
\Delta\mathbf{x}_c \\ \Delta\mathbf{x}_p
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{v} - \mathbf{E}\mathbf{C}^{-1}\mathbf{w} \\ \mathbf{w}
\end{bmatrix}
\end{equation}
\begin{equation}
[\mathbf{B} - \mathbf{E}\mathbf{C}^{-1}\mathbf{E}^T]\Delta\mathbf{x}_c
= \mathbf{v} - \mathbf{E}\mathbf{C}^{-1}\mathbf{w}
\end{equation}