This PR refactors the initialization of the factor graphs.
All functions which were in initialize_solution_utils.cpp are now part of Initializer class which is given as input to the trajectory.
Should make life easier when different types of initializations are needed.
This PR refactors the initialization of the factor graphs. All functions which were in initialize_solution_utils.cpp are now part of Initializer class which is given as input to the trajectory. Should make life easier when different types of initializations are needed.