2
$\begingroup$

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?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.