data_association.hpp

Go to the documentation of this file.

#pragma once
 
#include <gtest/gtest_prod.h>
 
#include <Eigen/Dense>
#include <functional>
#include <map>
#include <memory>
 
#include "common_lib/structures/cone.hpp"
#include "kalman_filter/observation_models.hpp"
 
class DataAssociationModel {
  // virtual bool valid_match(const float delta, const float distance_to_vehicle) const = 0;
 
  float max_landmark_distance_;
 
protected:
  float get_max_landmark_distance() const;
 
public:
  virtual int associate_n_filter(const std::vector<common_lib::structures::Cone> &perception_map,
                                 Eigen::VectorXf &_x_vector_, Eigen::MatrixXf &_p_matrix_,
                                 std::vector<int> &matched_ids,
                                 std::vector<Eigen::Vector2f> &matched_cone_positions,
                                 std::vector<Eigen::Vector2f> &new_features,
                                 ObservationModel *observation_model) const = 0;
  explicit DataAssociationModel(float max_landmark_distance);
 
  virtual ~DataAssociationModel() = default;
};
 
class MaxLikelihood : public DataAssociationModel {
  // bool valid_match(const float delta, const float distance_to_vehicle) const override;
 
public:
  static float association_gate_;   
  static float new_landmark_gate_;  
 
  int associate_n_filter(const std::vector<common_lib::structures::Cone> &perception_map,
                         Eigen::VectorXf &_x_vector_, Eigen::MatrixXf &_p_matrix_,
                         std::vector<int> &matched_ids,
                         std::vector<Eigen::Vector2f> &matched_cone_positions,
                         std::vector<Eigen::Vector2f> &new_features,
                         ObservationModel *observation_model) const override;
 
  explicit MaxLikelihood(float max_landmark_distance);
 
  // FRIEND_TEST(DATA_ASSOCIATION_MODEL, VALID_MATCH_FUNC_PERFECT_MATCH);
  // FRIEND_TEST(DATA_ASSOCIATION_MODEL, VALID_MATCH_FUNC_NEAR_MATCH);
  // FRIEND_TEST(DATA_ASSOCIATION_MODEL, VALID_MATCH_FUNC_FAILED_MATCH);
};
 
const std::map<std::string,
               std::function<std::shared_ptr<DataAssociationModel>(float max_landmark_distance)>,
               std::less<>>
    data_association_model_constructors = {
        {"max_likelihood",
         [](float max_landmark_distance) -> std::shared_ptr<DataAssociationModel> {
           return std::make_shared<MaxLikelihood>(max_landmark_distance);
         }}};