graph_slam_instance.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 "common_lib/structures/circular_buffer.hpp"
#include "slam_config/general_config.hpp"
#include "slam_solver/graph_slam_solver/factor_data_structures.hpp"
#include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp"
 
class GraphSLAMInstance {
protected:
  struct TimedPose {
    unsigned int pose_id;
    rclcpp::Time timestamp;
  };
 
  gtsam::NonlinearFactorGraph
      _factor_graph_;  //< Factor graph for the graph SLAM solver (only factors, no estimates)
  gtsam::Values
      _graph_values_;  //< Estimate for the graph SLAM solver: l_ are landmarks, x_ are poses
  unsigned int _pose_counter_ = 0;      //< Counter for the pose symbols
  unsigned int _landmark_counter_ = 0;  //< Counter for the landmark symbols
  bool _new_pose_node_ =
      false;  //< Flag to check if the pose was updated before adding observations
  bool _new_observation_factors_ =
      false;  //< Flag to check if new factors were added to the graph for optimization
  bool _locked_landmarks_ = false;  //< Flag to check if landmarks are locked in the graph
 
  SLAMParameters _params_;                     //< Parameters for the SLAM solver
  std::shared_ptr<BaseOptimizer> _optimizer_;  //< Optimizer for the graph SLAM solver
 
  CircularBuffer<TimedPose>
      _pose_timestamps_;  //< Buffer to hold the timestamps of the poses added to the graph
 
  unsigned int get_landmark_id_at_time(const rclcpp::Time& timestamp) const;
 
public:
  GraphSLAMInstance() = default;
 
  GraphSLAMInstance(const SLAMParameters& params, std::shared_ptr<BaseOptimizer> optimizer);
 
  GraphSLAMInstance(const GraphSLAMInstance& other);
 
  GraphSLAMInstance& operator=(const GraphSLAMInstance& other);
 
  ~GraphSLAMInstance() = default;
 
  std::shared_ptr<GraphSLAMInstance> clone() const {
    return std::make_shared<GraphSLAMInstance>(*this);
  }
 
  bool new_pose_factors() const;
 
  bool new_observation_factors() const;
 
  unsigned int get_landmark_counter() const;
 
  unsigned int get_pose_counter() const;
 
  const gtsam::Pose2 get_pose() const;
 
  const gtsam::Values& get_graph_values_reference() const;
 
  Eigen::VectorXd get_state_vector() const;
 
  Eigen::MatrixXd get_covariance_matrix() const;
 
  void process_observations(const ObservationData& observation_data);
 
  void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose,
                          double preloaded_map_noise);
  void process_new_pose(const Eigen::Vector3d& pose_difference, const Eigen::Vector3d& noise_vector,
                        const Eigen::Vector3d& new_pose, const rclcpp::Time& pose_timestamp);
 
  void process_pose_difference(const Eigen::Vector3d& pose_difference,
                               const Eigen::Vector3d& noise_vector, unsigned int before_pose_id,
                               unsigned int after_pose_id);
 
  void process_pose_difference(const Eigen::Vector3d& pose_difference,
                               const Eigen::Vector3d& noise_vector, unsigned int before_pose_id);
 
  void process_pose_difference(const Eigen::Vector3d& pose_difference,
                               const Eigen::Vector3d& noise_vector);
 
  void optimize();
 
  void lock_landmarks();
};