Why do we need a marginalization in Bundle Adjustment?

A common practice in Bundle Adjustment is to reduce the state dimension by marginalizing structure or pose states to improve the optimization speed.

In case 3d points(structure) $\textbf{p}_i$ are marginalized out as follows, $\textbf{p}_i$ are triangulated to calculate residual $\textbf{e}$.

$\textbf{e} = \textbf{z}_{ij} - \pi(\textbf{T}_j\textbf{p}_i)$

where $\textbf{T}_j\in SE(3), \textbf{p}_i\in R^3$ are the states we want to estimate and $\textbf{z}_{ij}$ is the observed feature in $R^2$.

And just optimize the pose related terms only.

$\begin{bmatrix} \textbf{H}_{cc}& \textbf{H}_{cs} \\ \textbf{H}_{sc} & \textbf{H}_{ss} \end{bmatrix} \begin{bmatrix} \mathbf{\xi}_c \\ \textbf{p}_s \end{bmatrix}= \begin{bmatrix} \textbf{g}_{c} \\ \textbf{g}_{s} \end{bmatrix}$

$\bar{\textbf{H}}_{cc}=\textbf{H}_{cc}-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{H}_{sc}$

$\bar{\textbf{g}}_{c}=\textbf{g}_{c}-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{g}_{s}$

$\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$

Here my question arises. If we can calculate 3d points $\textbf{p}_i$ by the triangulation, only $\textbf{T}_j$ are the state variable to be estimated.

Then, why are we bothered to calculate marginalization related terms $-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{H}_{sc}$ and $-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{g}_{s}$ instead of optimizing only poses by ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ (note that H and g are without bar).

I guess ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ is enough to find the optimal poses $\textbf{T}_j$.

So, my question is why do we use $\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$ instead of ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$?

• Sorry can you clarify what your question actually is? Specifically reword just this portion "Here my question arise. If we can calculate 3d points, why are we bothered to calculate marginalization related terms instead of getting an optimization on poses only? " – edwinem Apr 3 '18 at 0:26
• Hi, I tried to make it clear. Let me know if you need more explanation:) – C.O Park Apr 3 '18 at 2:18

I am still not entirely sure what you are asking, but I believe this may answer your question. Also note I myself don't fully understand all the details behind the marginalization trick, but I will attempt to explain why it is used.

Lets first setup how the optimization problem looks for your two different possibilities with just 2 cameras. So we are only solving for 1 pose.

Here is the actual bundle adjustment reprojection error.

$\displaystyle \min_{T,p} \sum_{i=1}^{p} (P_i-T(\pi(p_i))^2$ $\quad Eq1$

You solve for $T$(pose/transform) and $p$(points) in one optimization problem.

And here is the 2nd version to only find the poses.

$\displaystyle \min_{T} \sum_{i=1}^{p} (P_i-T(\pi(p_i))^2 \quad Eq2$

After solving for the pose($T$) with $Eq2$ you would then apply some sort of triangulation method to solve for the 3d points.

Lets agree on the fact that $Eq1$ will be more accurate then applying the 2nd method when it comes to finding the position of the poses. This is because it allows for error in the 3d points. The 2nd method assumes that the positions of the 3d points is correct. However, $Eq1$ is also a lot slower than $Eq2$ cause you are also optimizing for the points($p$).

Now for why we use the marginalization trick. We would apply the marginalization to Eq1. You do all the math you have written above, and are able to solve for the $T$ without also optimizing the points($p$). Remember how I said $Eq1$ was more accurate, but slower. Well with the marginalization we are able to reduce the problem into only solving for the pose($T$), but still keep most of the accuracy. So with marginalization we can almost do the exact same as $Eq2$, but with the accuracy of $Eq1$.

I typically know this trick as the Schur complement, however it also appears in the EKF formulations as some sort of null space trick.

References:

GTSAM Smart Factors

MSCEKF

• Thanks for the explanation! I understood that Eq1 is accurate but slower than Eq2. My point is a bit different from that problem. – C.O Park Apr 3 '18 at 21:31
• Eq2 can be solved by either ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ or $\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$. Then, what is the diffrence between ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ and $\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$? I did a similar optimization to above but in my experience ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ was enough to estimate pose parameters. So, I was wondering the difference. – C.O Park Apr 3 '18 at 21:32
• So I believe you can not solve Eq2 with $\bar{H}_{cc}\xi$ because $H_{cs}$ and $H_{sc}$ and $H_{ss}$ do not appear in Eq2. All the $s$ terms are related to the 3d points and in Eq2 we are not optimizing for those. – edwinem Apr 4 '18 at 14:15
• According to Eq(20) in your last reference, $\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$ should be used instead of ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$. All the structure related terms are not optimized but its information is merged in $\bar{\textbf{H}}_{cc}, \bar{\textbf{g}}_{c}$. That is what I understood as the definition of marginalization or Schur complement. Please, correct me if I am wrong. – C.O Park Apr 4 '18 at 18:54
• Your understanding is correct. – edwinem Apr 5 '18 at 0:13

Let's have a look at the Hessian H of simulated BA.

Yello represents non zero element and upper 60 by 60 matrix represent ${\textbf{H}}_{cc}$. When a structure is observed over multiple poses, Hessian make correlation terms.

Obiously, ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ ignores the correlation terms. Therefore, each poses loose information of linkage, whereas in $\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$ off diagonal terms are added to Hessian which represents the rumped relationship miginalized from $-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{H}_{sc}$.

The benefit are faster convergence and low final error in optimization.