The below figure shows a ground truth body trajectory (gt, black) and ground truth landmarks (lmk, blue). The goal is to run a SLAM simulation using NLS optimization as the backend and vision as the front end. A dead reckoning signal (dr, red) is generated using an IMU (with noise) and feature points (with noise) are created using landmark positions. Camera extrinsics and intrinsics were used to determine which landmarks were within the fov.
Decision variables for NLS optimization in the SLAM problem are body position, orientation, and landmark position. Noisy landmark positions served as the initial seed for the landmark position decision variable. The dr signal was the initial seed for the body position decision variable. IMU data was used to constrain relative motion, and observation reprojection error was used to constrain landmark and body positions. The NLS optimized body positions signal is also shown below (opt, gold). Noisy landmark position and the corrresponding optimized positions are not shown. Currently only localization results are shown.
The above figure shows trajectories for all signals and landmarks.
The above figure shows body orientation for all signals.
The above figure shows body velocity for all signals.
The above figure shows body position for all signals.
The above figure shows position error between signals and ground truth. The dr signal drifts over time due to noise in the IMU and integration error. The optimized signal shows a decrease in drift due to the inclusion of observations into the cost, constraints of the optimization problem.
TO DO:
-
Ensure accurate/realistic parameters in config file
-
Choose trajectory that best displays performance of BOEM SLAM
-
How to plot mapped landmark points in a clear and illustrative manner?
-
How to determine error metrics for landmarks?