Function graph_based_slam 

pub fn graph_based_slam(
    x_init: &DMatrix<f64>,
    hz: &[Option<Vec<Observation>>],
) -> DMatrix<f64>

Graph-based SLAM optimization