Handling Constrained Optimization in Factor Graphs for Autonomous Navigation