|
Formula Student Autonomous Systems
The code for the main driverless system
|
#include "slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp"#include <gtsam/geometry/Point2.h>#include <gtsam/geometry/Pose2.h>#include <gtsam/nonlinear/ISAM2.h>#include <gtsam/nonlinear/ISAM2Params.h>#include <gtsam/slam/BearingRangeFactor.h>#include <gtsam/slam/BetweenFactor.h>#include <algorithm>#include <cmath>#include <memory>#include <rclcpp/rclcpp.hpp>#include <sstream>#include <string>#include <unordered_set>