jcbb.hpp

Go to the documentation of this file.

#pragma once
#include <Eigen/Dense>
#include <cmath>
#include <limits>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <vector>
 
#include "common_lib/maths/transformations.hpp"
#include "perception_sensor_lib/data_association/base_data_association.hpp"
 
class JCBB : public DataAssociationModel {
  void search_branch_and_bound(int current_obs_idx, int num_observations, int num_landmarks,
                               const Eigen::MatrixXd& distances,
                               Eigen::VectorXi& current_hypothesis,
                               Eigen::VectorXi& best_hypothesis, double& best_score,
                               double current_score) const;
  bool is_landmark_already_assigned(const Eigen::VectorXi& hypothesis, int landmark_idx) const;
 
public:
  JCBB(const DataAssociationParameters& params) : DataAssociationModel(params) {}
 
  ~JCBB() = default;
  Eigen::VectorXi associate(const Eigen::VectorXd& landmarks, const Eigen::VectorXd& observations,
                            const Eigen::MatrixXd& covariance,
                            const Eigen::VectorXd& observation_confidences) const override;
};