Parallelize optimize_slam