This is an educational implementation of the pose graph algorithm. We offer two scripts for running pose graph examples.
The first one is main_simple_pose.m responsible for running the following 2D and 3D examples:
while main_dataset.m is able to run true datasets, such as the sphere_small_noise and
sphere_big_noise shown below:
The problem that we intend to solve is the following. Given a set of noisy measurements, divided in relative-pose and relative-pose-landmark measurements, find the best configuration of poses and landmarks that satisfies, in a least-square sense, the given set of measurements. Mathematically, this corresponds in minimizing the following function illustrated for the 2D case:
A typical solution for this minimization problem, and implemented in this project, is the Gauss-Newton method. To implement it, we assumed that the initial configuration is close to the true one, and at each iteration, we linearize the errors, and approximate the final function to its quadratic form. The minimum of a quadratic form has an exact solution that can be computed, and the pose-landmarks are updated with this minimum. Finally, we finish the algorithm when we reach convergence. The equations are shown below.
