From the pre-optimization (blue line) to the post-optimization (orange line), we can see that the optimization method enables the loop closure effect.
The key improvement lies in the better computer vision techniques by discarding possibly erroneous keypoints. Also, the Jacobians in Lie group should be carefully handle.
The same challenges in the previous part arise here. The Jacobians in Lie group have also to be implemented correctly here.
However, the EM result is not complete yet. Only part of the entire KF and RTS smoother is implemented.