Pose Graph Optimization in 3D environment (including pose-to-pose pose-to-landmark)

This repository includes Nonlinear-Least-Square solver for 3-D pose graph optimization, by running main.m, you can get the simulated dataset including 3 camera poses and 1 landmark, then run Main_LS.m to get the optimized result.