A Lagrange-Newton Approach to Smoothing-and-Mapping