graph_slam_solver.hpp

Go to the documentation of this file.

#pragma once
 
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
 
#include <queue>
 
#include "motion_lib/v2p_models/odometry_model.hpp"
#include "perception_sensor_lib/loop_closure/lap_counter.hpp"
#include "slam_solver/graph_slam_solver/factor_data_structures.hpp"
#include "slam_solver/graph_slam_solver/graph_slam_instance.hpp"
#include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp"
#include "slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp"
#include "slam_solver/slam_solver.hpp"
#include "slam_solver/solver_traits/imu_integrator_trait.hpp"
#include "slam_solver/solver_traits/node_controller_trait.hpp"
#include "slam_solver/solver_traits/odometry_integrator_trait.hpp"
#include "slam_solver/solver_traits/trajectory_calculator.hpp"
#include "slam_solver/solver_traits/velocities_integrator_trait.hpp"
 
class GraphSLAMSolver : public SLAMSolver,
                        public VelocitiesIntegratorTrait,
                        public OdometryIntegratorTrait,
                        public ImuIntegratorTrait,
                        public NodeControllerTrait,
                        public TrajectoryCalculator {
  std::shared_ptr<GraphSLAMInstance> _graph_slam_instance_;  //< Instance of the graph SLAM solver
  std::shared_ptr<PoseUpdater> _pose_updater_;  //< Pose updater for the graph SLAM solver
  std::shared_ptr<OdometryModel>
      _odometry_model;  //< Motion model for odometry integration TODO: improve, I dont like it
  std::queue<MotionData>
      _motion_data_queue_;  //< Queue of velocities received while optimization ran
  std::queue<ObservationData>
      _observation_data_queue_;  //< Queue of observations received while optimization ran
  rclcpp::TimerBase::SharedPtr _optimization_timer_;  //< Timer for asynchronous optimization
  mutable std::shared_mutex
      _mutex_;  //< Mutex for the graph SLAM solver, locks access to the graph and other vars
  bool _optimization_under_way_ = false;  //< Flag to check if the optimization is under way
  Eigen::VectorXi _associations_;         //< Associations of the cones in the map
  Eigen::VectorXd _observations_global_;  //< Global observations of the cones
  Eigen::VectorXd _map_coordinates_;      //< Coordinates of the landmarks in the map
 
  common_lib::sensor_data::ImuData _last_imu_data_;  //< Last IMU data received
 
  rclcpp::CallbackGroup::SharedPtr
      _reentrant_group_;  //< Reentrant callback group for the timer callback
 
  void _asynchronous_optimization_routine();
 
  bool _add_motion_data_to_graph(const std::shared_ptr<PoseUpdater> pose_updater,
                                 const std::shared_ptr<GraphSLAMInstance> graph_slam_instance,
                                 bool force_update = false);
 
  friend class GraphSlamSolverTest_MotionAndObservation_Test;
 
public:
  GraphSLAMSolver(const SLAMParameters& params,
                  std::shared_ptr<DataAssociationModel> data_association,
                  std::shared_ptr<V2PMotionModel> motion_model,
                  std::shared_ptr<LandmarkFilter> landmark_filter,
                  std::shared_ptr<std::vector<double>> execution_times,
                  std::shared_ptr<LoopClosure> loop_closure);
 
  ~GraphSLAMSolver() = default;
 
  void init(std::weak_ptr<rclcpp::Node> node) override;
 
  void add_velocities(const common_lib::structures::Velocities& velocities) override;
 
  void add_odometry(const common_lib::structures::Pose& pose_difference) override;
 
  void add_imu_data(const common_lib::sensor_data::ImuData& imu_data) override;
 
  void add_observations(const std::vector<common_lib::structures::Cone>& cones,
                        rclcpp::Time cones_timestamp) override;
 
  void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) override;
 
  std::vector<common_lib::structures::Cone> get_map_estimate() override;
 
  common_lib::structures::Pose get_pose_estimate() override;
 
  std::vector<common_lib::structures::Pose> get_trajectory_estimate() override;
 
  Eigen::MatrixXd get_covariance() override;
 
  int get_lap_counter() override { return lap_counter_; }
 
  Eigen::VectorXi get_associations() const override;
 
  Eigen::VectorXd get_observations_global() const override;
 
  Eigen::VectorXd get_map_coordinates() const override;
 
};