Graph optimisation

To complete the mapping exercise, you will need to estimate a more accurate robot trajectory.

You should by this point have, in your possession,:

• the odometry calculated from the wheel encoders, and
• the visual odometry calculated between consecutive images.

To optimise a more accurate trajectory from these two sources, you will apply a graph-based slam solution.

Graph-based slam (gtsam) requires a set of latent variables (the robot poses along the trajectory) and an initial guess for each (in global coordinates), as well as the factors between these variables (relative odometry from both sources). The slam system requires these relative and global trajectories in the form [x, y, theta].

In order to convert from [d, theta] to this form, set the x component to equal d, and the y component to zero. You will be required to compound the relative robot transforms (distance and bearing) into global coordinates (x,y,theta).

This can be done by using the following:

given: d,theta : x_prev, y_prev, theta_prev

x_cur = x_prev + d*cos(theta_prev) # in radians

y_cur = y_prev + d*sin(theta_prev) # in radians

theta_cur = theta_prev + theta 

Once you have the relative and global odometry estimated from the wheel encoders and visually (in the form [x,y,theta]) you can optimise a solution via:

s = Slam(odom_global, odom_relative, visual_global, visual_relative)

estimated_global_trajectory = s.estimate_global()