FSF-8582 Perception filter

config/slam/pacsim.yaml

@@ -1,5 +1,6 @@
 slam:
   motion_model_name: "constant_velocity" # "constant_velocity" or "constant_velocity_turnrate" or ... -> name of the motion model
+  landmark_filter_name: "consecutive_count" # "consecutive_count" or other names of filters of perception data
   data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model
   data_association_limit_distance: 70 # maximum distance to consider a cone for data association
   data_association_gate: 1.23 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark
@@ -10,6 +11,8 @@ slam:
   velocity_y_noise: 0.1 # sigma of the noise for the velocity in y
   angular_velocity_noise: 0.1 # sigma of the noise for the angular velocity
   slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver
+  minimum_observation_count: 3 # Minimum number of times a new landmark must be observed to be added to the map
+  minimum_frequency_of_detections: 5 # Minimum frequency of detections to consider a landmark as valid
   slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph
   slam_optimization_mode: "sync" # "sync" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step
   slam_optimization_type: "normal_levenberg" # "normal_levenberg" or "isam2" or ... -> name of the optimization type

config/slam/vehicle.yaml

@@ -1,5 +1,6 @@
 slam:
   motion_model_name: "constant_velocity" # "constant_velocity" or "constant_velocity_turnrate" or ... -> name of the motion model
+  landmark_filter_name: "consecutive_count" # "consecutive_count" or other names of filters of perception data
   data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model
   data_association_limit_distance: 70 # maximum distance to consider a cone for data association
   data_association_gate: 1.23 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark
@@ -10,6 +11,8 @@ slam:
   velocity_y_noise: 0.1 # sigma of the noise for the velocity in y
   angular_velocity_noise: 0.1 # sigma of the noise for the angular velocity
   slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver
+  minimum_observation_count: 3 # Minimum number of times a new landmark must be observed to be added to the map
+  minimum_frequency_of_detections: 5 # Minimum frequency of the detections of a landmark to add it to the map
   slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph
   slam_optimization_mode: "sync" # "sync" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step
   slam_optimization_type: "normal_levenberg" # "normal_levenberg" or "isam2" or ... -> name of the optimization type

src/perception_sensor_lib/CMakeLists.txt

@@ -35,6 +35,7 @@ set(IMPLEMENTATION_FILES
   src/data_association/nearest_neighbour.cpp
   src/data_association/nearest_neighbor.cpp
   src/observation_model/base_observation_model.cpp
+  src/landmark_filter/consecutive_counter_filter.cpp
 )
 
 add_library(${PROJECT_NAME} SHARED ${IMPLEMENTATION_FILES})
@@ -70,7 +71,9 @@ if(BUILD_TESTING)
     test/observation_model/base_observation_model_test.cpp
     test/data_association/maximum_likelihood_nll_test.cpp
     test/data_association/nearest_neighbor_test.cpp
+    test/data_association/nearest_neighbour_test.cpp
     test/data_association/maximum_likelihood_md_test.cpp
+    test/landmark_filter/consecutive_counter_filter_test.cpp
   )
 
   include_directories(test)  # Include the 'test' directory for your test targets

src/perception_sensor_lib/include/perception_sensor_lib/data_association/base_data_association.hpp

@@ -19,18 +19,18 @@ class DataAssociationModel {
   /**
    * @brief This function associates the landmarks with the observations
    *
-   * @param state Vector of car's position and landmarks in the form of [car_x, car_y,
-   * car_orientation, x1, y1, x2, y2, ...] all in the global frame
-   * @param covariance Covariance matrix of the state vector
-   * @param observations Observations in the form of [x1, y1, x2, y2, ...] in the car's frame
+   * @param landmarks Landmarks in the form of [x1, y1, x2, y2, ...] in the global frame
+   * @param observations Observations in the form of [x1, y1, x2, y2, ...] in the global frame
+   * @param covariance Covariance matrix of the landmark vector
    * @param observation_confidences Confidence in the observations in the same order as the
    * observations
    * @return Eigen::VectorXi Each entry corresponds to an observation and contains the index of the
-   * landmark that the observation is associated with in the state vector (x coordinate). If the
+   * landmark that the observation is associated with in the landmark vector (x coordinate). If the
    * observation is considered new, the entry is -1. If the observation is considered an outlier,
    * the entry is -2.
    */
-  virtual Eigen::VectorXi associate(const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance,
+  virtual Eigen::VectorXi associate(const Eigen::VectorXd& landmarks,
                                     const Eigen::VectorXd& observations,
+                                    const Eigen::MatrixXd& covariance,
                                     const Eigen::VectorXd& observation_confidences) const = 0;
 };

src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_md.hpp

@@ -22,7 +22,8 @@ class MaximumLikelihoodMD : public DataAssociationModel {
 
   ~MaximumLikelihoodMD() = default;
 
-  Eigen::VectorXi associate(const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance,
+  Eigen::VectorXi associate(const Eigen::VectorXd& landmarks,
                             const Eigen::VectorXd& observations,
+                            const Eigen::MatrixXd& covariance,
                             const Eigen::VectorXd& observation_confidences) const override;
 };

src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_nll.hpp

@@ -22,7 +22,8 @@ class MaximumLikelihoodNLL : public DataAssociationModel {
 
   ~MaximumLikelihoodNLL() = default;
 
-  Eigen::VectorXi associate(const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance,
+  Eigen::VectorXi associate(const Eigen::VectorXd& landmarks, 
                             const Eigen::VectorXd& observations,
+                            const Eigen::MatrixXd& covariance,
                             const Eigen::VectorXd& observation_confidences) const override;
 };

src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbor.hpp

@@ -16,7 +16,8 @@ class NearestNeighbor : public DataAssociationModel {
 
   ~NearestNeighbor() = default;
 
-  Eigen::VectorXi associate(const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance,
+  Eigen::VectorXi associate(const Eigen::VectorXd& landmarks, 
                             const Eigen::VectorXd& observations,
+                            const Eigen::MatrixXd& covariance,                            
                             const Eigen::VectorXd& observation_confidences) const override;
 };

src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour.hpp

@@ -17,7 +17,8 @@ class NearestNeighbour : public DataAssociationModel {
 
   ~NearestNeighbour() = default;
 
-  Eigen::VectorXi associate(const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance,
+  Eigen::VectorXi associate(const Eigen::VectorXd& landmarks,
                             const Eigen::VectorXd& observations,
+                            const Eigen::MatrixXd& covariance,
                             const Eigen::VectorXd& observation_confidences) const override;
 };

src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp

@@ -0,0 +1,45 @@
+#pragma once
+#include <Eigen/Dense>
+#include <memory>
+#include <vector>
+
+#include "perception_sensor_lib/data_association/base_data_association.hpp"
+#include "perception_sensor_lib/landmark_filter/parameters.hpp"
+
+/**
+ * @brief This class is meant to filter observations from perception to try to reduce the presence
+ * of outliers
+ *
+ */
+class LandmarkFilter {
+protected:
+  LandmarkFilterParameters _params_;
+  std::shared_ptr<DataAssociationModel> _data_association_;
+
+public:
+  LandmarkFilter() = default;
+  LandmarkFilter(LandmarkFilterParameters params,
+                 std::shared_ptr<DataAssociationModel> data_association)
+      : _params_(params), _data_association_(data_association) {}
+  virtual ~LandmarkFilter() = default;
+  /**
+   * @brief This function receives a new set of observations and returns a filtered set of landmarks
+   * in global coordinates that are ready to be added to be added to the map
+   *
+   * @param observations Observations in the form of [x1, y1, x2, y2, ...] in the global frame
+   * @param observation_confidences Confidence in the observations in the same order as the
+   * observations
+   * @return Eigen::VectorXd the filtered observations in the form of [x1, y1, x2, y2, ...] in the
+   * global frame
+   */
+  virtual Eigen::VectorXd filter(const Eigen::VectorXd& new_observations,
+                                 const Eigen::VectorXd& new_observation_confidences) = 0;
+  /**
+   * @brief Used by SLAM to signal to the filter that the landmarks are already in SLAM's map, and
+   * they should no longer be returned by the filter as new.
+   *
+   * @param some_landmarks landmarks to be deleted in the form of [x1, y1, x2, y2, ...] in the
+   * global frame
+   */
+  virtual void delete_landmarks(const Eigen::VectorXd& some_landmarks) = 0;
+};

src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp

@@ -0,0 +1,21 @@
+#include <set>
+
+#include "perception_sensor_lib/landmark_filter/base_landmark_filter.hpp"
+#define EQUALITY_TOLERANCE 1e-3
+
+class ConsecutiveCounterFilter : public LandmarkFilter {
+private:
+  Eigen::VectorXd map;
+  Eigen::VectorXi counter;
+
+public:
+  ConsecutiveCounterFilter(LandmarkFilterParameters params,
+                           std::shared_ptr<DataAssociationModel> data_association)
+      : LandmarkFilter(params, data_association), map(Eigen::VectorXd::Zero(0)) {}
+
+  Eigen::VectorXd filter(const Eigen::VectorXd& new_observations,
+                         const Eigen::VectorXd& new_observation_confidences) override;
+
+  void delete_landmarks(const Eigen::VectorXd& some_landmarks) override;
+  friend class ConsecutiveCounterFilter_TestCase1_Test;
+};

src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/map.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+
+#include "perception_sensor_lib/data_association/base_data_association.hpp"
+#include "perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp"
+
+/**
+ * @brief Map of landmark filters, with the key being the name of the landmark filter
+ * and the value being a lambda function that returns a shared pointer to the corresponding
+ * LandmarkFilter object.
+ */
+const std::map<std::string,
+               std::function<std::shared_ptr<LandmarkFilter>(
+                   const LandmarkFilterParameters&, std::shared_ptr<DataAssociationModel>)>,
+               std::less<>>
+    landmark_filters_map = {
+        {"consecutive_count",
+         [](const LandmarkFilterParameters& params,
+            std::shared_ptr<DataAssociationModel> data_association)
+             -> std::shared_ptr<LandmarkFilter> {
+           return std::make_shared<ConsecutiveCounterFilter>(params, data_association);
+         }},
+};

src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/parameters.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+struct LandmarkFilterParameters {
+  int minimum_observation_count_ = 3;
+  double minimum_frequency_of_detections_ = 5;
+
+  LandmarkFilterParameters() = default;
+
+  LandmarkFilterParameters(const int minimum_observation_count,
+                           const double minimum_frequency_of_detections)
+      : minimum_observation_count_(minimum_observation_count),
+        minimum_frequency_of_detections_(minimum_frequency_of_detections) {}
+};

src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp

@@ -4,51 +4,35 @@
 #include <limits>
 #include <vector>
 
+#define INFINITESIMAL 1e-9
+
 Eigen::VectorXi MaximumLikelihoodMD::associate(
-    const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance,
-    const Eigen::VectorXd& observations, const Eigen::VectorXd& observation_confidences) const {
-  int num_observations = observations.size() / 2;
+    const Eigen::VectorXd& landmarks, const Eigen::VectorXd& observations,
+    const Eigen::MatrixXd& covariance, const Eigen::VectorXd& observation_confidences) const {
+  const int num_observations = observations.size() / 2;
+  const int num_landmarks = landmarks.size() / 2;
   Eigen::VectorXi associations =
       Eigen::VectorXi::Constant(num_observations, -2);  // Default: not associated (-2)
 
-  double car_x = state(0);
-  double car_y = state(1);
-  double car_theta = state(2);
-  double cos_theta = std::cos(car_theta);
-  double sin_theta = std::sin(car_theta);
-
-  // Get coordinates of all landmarks in local frame
-  Eigen::VectorXd landmarks_local_coordinates = common_lib::maths::global_to_local_coordinates(
-      state.segment(0, 3), state.segment(3, state.size() - 3));
-
   for (int i = 0; i < num_observations; ++i) {
     Eigen::Vector2d obs;
     obs << observations(2 * i), observations(2 * i + 1);
 
     double best_cost = std::numeric_limits<double>::max();
     int best_landmark_index = -1;
 
-    for (int j = 3; j < state.size(); j += 2) {
-      // Check if the landmark is within a valid distance from the car
-      double distance =
-          std::hypot(landmarks_local_coordinates(j - 3), landmarks_local_coordinates(j - 2));
-      if (distance > this->_params_.max_landmark_distance) continue;
-
-      // Extract landmark global coordinates
-      double lx = state(j);
-      double ly = state(j + 1);
-      double dx = lx - car_x;
-      double dy = ly - car_y;
-
+    for (int j = 0; j < num_landmarks; j++) {
       // Innovation: difference between actual and predicted observation
-      Eigen::Vector2d innovation = obs - landmarks_local_coordinates.segment(j - 3, 2);
+      Eigen::Vector2d innovation = obs - landmarks.segment(2 * j, 2);
 
-      Eigen::Matrix2d innovation_covariance = covariance.block(j, j, 2, 2);
+      Eigen::Matrix2d innovation_covariance =
+          covariance.block(2 * j, 2 * j, 2, 2) + this->observation_noise_covariance_matrix_;
       // Guard against non-invertible innovation_covariance
       double det = innovation_covariance.determinant();
-      if (det == 0) {
+      if (std::abs(det) <= INFINITESIMAL) {
         RCLCPP_WARN(rclcpp::get_logger("data_association"),
-                    "Non-invertible innovation covariance matrix");
+                    "Non-invertible innovation covariance matrix for landmark at : [%f, %f]",
+                    landmarks(2 * j), landmarks(2 * j + 1));
         continue;
       }
       double normalized_innovation_squared =
@@ -57,7 +41,7 @@ Eigen::VectorXi MaximumLikelihoodMD::associate(
       // Check if this landmark is a good candidate for association
       if (normalized_innovation_squared < best_cost) {
         best_cost = normalized_innovation_squared;
-        best_landmark_index = j;
+        best_landmark_index = 2 * j;
       }
     }