Marginalization speed #338
Description
Implementing visual odometry with fuse is showing be rather tricky.I have noticed that in the optimization loop, marginalization is the main bottleneck. Notable within marginalization the most expensive step seems to be the qr decomposition:
https://github.com/locusrobotics/fuse/blob/devel/fuse_constraints/src/marginalize_variables.cpp#L483-L498
This isn't an issue when marginalization only needs to marginalize position/orientation variables since there aren't too many, but for visual odometry, marginalization may need to compute this step for ~100 landmark variables and it can take up to ~1s. I was wondering if there is any way I can implement this with another QR implementation to speed it up? I noticed the SPQR module (https://eigen.tuxfamily.org/dox/group__SPQRSupport__Module.html) from eigen, is it possible to use this if its available? or has it been investigated?