|
Formula Student Autonomous Systems
The code for the main driverless system
|
#include "slam_solver/graph_slam_solver/graph_slam_solver.hpp"#include <gtsam/base/Vector.h>#include <gtsam/geometry/Point2.h>#include <gtsam/geometry/Pose2.h>#include <gtsam/geometry/Rot2.h>#include <gtsam/inference/Symbol.h>#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>#include <gtsam/nonlinear/Marginals.h>#include <gtsam/nonlinear/Values.h>#include <gtsam/sam/BearingFactor.h>#include <gtsam/sam/BearingRangeFactor.h>#include <gtsam/sam/RangeFactor.h>#include <gtsam/slam/BetweenFactor.h>#include <gtsam/slam/PriorFactor.h>#include <gtsam/slam/ProjectionFactor.h>#include <iostream>#include <rclcpp/rclcpp.hpp>#include "common_lib/maths/transformations.hpp"#include "slam_solver/graph_slam_solver/optimizer/map.hpp"#include "slam_solver/graph_slam_solver/pose_updater/map.hpp"#include "slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp"#include "slam_solver/graph_slam_solver/utils.hpp"