To complete the mapping exercise, you will need to estimate a more accurate robot trajectory.
You should by this point have, in your possession,:
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_prevx_cur = x_prev + d*cos(theta_prev) # in radiansy_cur = y_prev + d*sin(theta_prev) # in radianstheta_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()