Mateosss/pkf aom

This properly updates marg_data after a prior keyframe is added. It does not implement a way for marginalize() to understand this possible change but it is good enough to add a prior keyframe without needing to remove it and without much divergence.

A way to test it is with the msdmi_config.json and the MIO11 dataset, when the headset starts to move you press "Take Map" to look for new priorkeyframes to add, usually when the print "AFTER covisible_keyframes.size" adds more than 10 keyframes things work, otherwise they crash but that's a problem with the addPriorKeyframes logic.