ekf_slam_solver.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Dense>
#include <chrono>
 
#include "common_lib/conversions/cones.hpp"
#include "common_lib/maths/transformations.hpp"
#include "perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp"
#include "slam_solver/slam_solver.hpp"
#include "solver_traits/velocities_integrator_trait.hpp"
 
class EKFSLAMSolver : public SLAMSolver, public VelocitiesIntegratorTrait {
  SLAMParameters slam_parameters_;
  std::shared_ptr<SLAMObservationModel> observation_model_;
  Eigen::VectorXd state_ = Eigen::VectorXd::Zero(3);
  Eigen::MatrixXd covariance_;
  Eigen::MatrixXd process_noise_matrix_;
  std::shared_mutex mutex_;
  Eigen::Vector3d pose_difference_ = Eigen::Vector3d::Zero();
  bool correction_step_ongoing_ = false;
 
  rclcpp::Time last_update_;
 
  bool velocities_received_ = false;
  bool cones_receieved_ = false;
 
  Eigen::MatrixXd get_observation_noise_matrix(int num_landmarks) const;
 
  void predict(Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
               const Eigen::MatrixXd& process_noise_matrix, const rclcpp::Time last_update,
               const common_lib::structures::Velocities& velocities);
 
  void correct(Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
               const std::vector<int>& observed_landmarks_indices,
               const Eigen::VectorXd& matched_observations);
 
  void state_augmentation(Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
                          const Eigen::VectorXd& new_landmarks);
 
  std::vector<common_lib::structures::Cone> get_map_estimate() override;
  common_lib::structures::Pose get_pose_estimate() override;
 
  friend class EKFSLAMSolverTest_stateAugmentation_Test;
  friend class EKFSLAMSolverTest_stateAugmentation2_Test;
 
public:
  EKFSLAMSolver(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);
 
  void add_velocities(const common_lib::structures::Velocities& velocities) override;
 
  void add_observations(const std::vector<common_lib::structures::Cone>& positions,
                        rclcpp::Time cones_timestamp) override;
 
  void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) override;
 
  Eigen::MatrixXd get_covariance() override { return covariance_; }
 
  int get_lap_counter() override { return lap_counter_; }
 
  Eigen::VectorXi get_associations() const override { return Eigen::VectorXi::Zero(0); }
  Eigen::VectorXd get_observations_global() const override { return Eigen::VectorXd::Zero(0); }
  Eigen::VectorXd get_map_coordinates() const override { return Eigen::VectorXd::Zero(0); }
};