Educational 2D SLAM implementation based on ICP and Pose Graph
Use keyboard arrow keys to navigate robot.
Press 'r' to view the raw map built from the odometry and sensor measurements.
Press 'i' to view map built from sensor measurements aligned with the ICP algorithm.
To update map and the robot path with the Pose Graph optimization, you have to navigate robot around the map and return it to the starting position in the center, the robot have to look in same direction as at the beginning. Then press the 's' or 'g' key. The 's' key will start naive basic SLAM implementation and the 'g' key will launch solution based on GTSAM library.
To change the world map you can edit the 'map.png' file in the assets folder.
Please take a look in to the doc folder to find the reading list.
Also, you can find there the jupyter notebook with derivation of all formulas for Jacobians and Hessian for SLAM implementation with corresponding Python examples.
You can set a different noise level for the odometry and sensors, or even disable noise at all.
This configuration can be done by editing the simulation.py file, where you can change corresponding values for mu and sigma of a random gaussian noise.
odometry = Odometry(mu=0, sigma=3) # noised measurements
sensor = Sensor(dist_range=350, fov=90, mu=0, sigma=1) # noised measurements
...
slam_back_end = playground.slam.backend.BackEnd(edge_sigma=0.5, angle_sigma=0.1)