Consider a simple example of Bundle Adjustment where I have robot and landmark poses $x = \left[ x_p \text{ } x_m\right]^T$ and measurements given by $z$, such that a simple factor graph can be generated with the nodes containing poses and edges containing the measurements. I'll have to solve for a non-linear least squares problem of the form $C(x) = \frac{1}{2}|| r(x) ||^2$ where $r(x)$ denotes the residuals.

I can implement and use any non-linear least squares optimization algorithm such as Gauss-Newton or use a popular library like ceres-solver.

My question is: Now suppose out of the state variables $x = \left[ a \text{ } b\right]^T$, I need to marginalize some variables $b$, while keeping the rest $a$. How do I apply this in terms of Gauss-Newton Algorithm and ceres-solver?

I understand the Gauss-Newton Algorithm and Schur Complement. If the original covariance of the system is \begin{equation} K = \begin{bmatrix} A & C^T \\ C & D \end{bmatrix} \end{equation}

Original Information \begin{equation} K^{-1} = \begin{bmatrix} \Lambda_{aa} & \Lambda_{ab} \\ \Lambda_{ba} & \Lambda_{bb} \end{bmatrix} \end{equation} Marginalized covariance $K_m = [A]$ and marginalized information $K_m^{-1} = A^{-1}$ where is $A$ is computed by the schur complement $A^{-1} = \Lambda_{aa} - \Lambda_{ab}\Lambda_{bb}^{-1}\Lambda_{ba}$.

Now, what do I do when new poses and measurements are added to the system and optimization is done?



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.