Function optimize_pose_graph 

pub fn optimize_pose_graph(
    initial_poses: &[Pose2DNode],
    edges: &[Edge2D],
    config: &PoseGraphConfig,
) -> PoseGraphResult

Optimizes a 2D pose graph using Gauss-Newton iteration.