This PR adds an A1 example in order to create a benchmark for future improvements on the factor graph.
The example includes a main.cpp which builds the factor graph, optimizes and writes the results to a .csv file.
The example inclues also a python simulation script which takes the values and simulates the robot motion.
This PR adds an A1 example in order to create a benchmark for future improvements on the factor graph. The example includes a main.cpp which builds the factor graph, optimizes and writes the results to a .csv file. The example inclues also a python simulation script which takes the values and simulates the robot motion.