Investigate marginalization speed up techniques

[jakemclaughlin6]

1. Nullspace constraints for landmarks (QR factorization at constraint level)
2. Zero out off diagonals of hessian
3. Replace optimized landmarks with fixed
4. Remove poorly constrained landmarks fully
5. use FEJ during marginalization
6. Use maringalization boundary -> only do marginalization on 1-3 keyframes before newest anchor, throw out the rest
7. Partial marginalization -> remove constraints between marginalized pose and rest of window, then marginalize it, keep reproj constraints only seen by that pose (https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/90dabb5ec79946ae42fd2e1e91d4e69aabe1e25d/vins_estimator/src/factor/marginalization_factor.cpp#L110)
8. Local schur complement on each landmark

Fix the fixed lag smoother to include landmarks connected to poses to marginalize in the marginalization function

Create helper functions for converting constraints

Add logs for the actual marginilization constraints being added and the runtime of them

Remove keyframes that are redundant in the middle

[jakemclaughlin6]

marginalization_context.md

[jakemclaughlin6]

ba_marginalization_notes.md