Fixed bugs in non-robust version of pose graph optimization.