GTSAM Toy Problem
Authors: Nitin J. Sanket and Chahat Deep Singh.
This is a Toy Example to perform SLAM (Simultaneous Localization and Mapping) on a 2D Map. The robot moves around the scene, gets a noisy estimate (distance and direction) of the observed landmarks (you cannot see all landmarks at once) from a simulated LIDAR/Camera sensor.
This code simulates the environment, robot movement, robot observations and then formulates the SLAM problem as a Factor Graph using the famous GTSAM graph optimization framework.
- The accompanying Video lecture for this Toy Problem is can be found here.
- The accompanying slides can be found here.
- Install GTSAM 3.2.1 from here. Try to avoid the latest version of GTSAM (GTSAM 4.0) as it has some bugs.
- Be sure to be able to run any of the example from the toolbox.
- To run this code, run the
Test1.mscript.
The assumptions made are:
- The world extends from -WorldLim to WorldLim in both X and Y directions. Refer to Test1.m script.
- You have a noisy odometry estimate with noise covariance given by [0.3 0.3 0.1] ([MovementX MovementY MovementTheta]).
- The landmark noise is given by [0.1 0.1] in m.
- Landmarks are non blocking (you can see a landmark behind another landmark).