diff --git a/config/slam/pacsim.yaml b/config/slam/pacsim.yaml index f8d50bec6..66fc6ca08 100644 --- a/config/slam/pacsim.yaml +++ b/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 diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index f8d50bec6..92a30ffbc 100644 --- a/config/slam/vehicle.yaml +++ b/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 diff --git a/src/perception_sensor_lib/CMakeLists.txt b/src/perception_sensor_lib/CMakeLists.txt index 9e31f0b7f..0acf6d074 100644 --- a/src/perception_sensor_lib/CMakeLists.txt +++ b/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 diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/base_data_association.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/base_data_association.hpp index 5272e704d..156150b9d 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/base_data_association.hpp +++ b/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; }; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_md.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_md.hpp index 5004554e6..ffcc8a3ea 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_md.hpp +++ b/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; }; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_nll.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_nll.hpp index a1c983367..05e9f8284 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/maximum_likelihood_nll.hpp +++ b/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; }; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbor.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbor.hpp index f5570c019..0d9bf620b 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbor.hpp +++ b/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; }; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour.hpp index 275505812..a2bc9006f 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour.hpp +++ b/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; }; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp new file mode 100644 index 000000000..cab2108f5 --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp @@ -0,0 +1,45 @@ +#pragma once +#include +#include +#include + +#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 _data_association_; + +public: + LandmarkFilter() = default; + LandmarkFilter(LandmarkFilterParameters params, + std::shared_ptr 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; +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp new file mode 100644 index 000000000..e5553778f --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp @@ -0,0 +1,21 @@ +#include + +#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 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; +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/map.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/map.hpp new file mode 100644 index 000000000..801d01d4f --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/map.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include + +#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( + const LandmarkFilterParameters&, std::shared_ptr)>, + std::less<>> + landmark_filters_map = { + {"consecutive_count", + [](const LandmarkFilterParameters& params, + std::shared_ptr data_association) + -> std::shared_ptr { + return std::make_shared(params, data_association); + }}, +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/parameters.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/parameters.hpp new file mode 100644 index 000000000..68566e356 --- /dev/null +++ b/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) {} +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp b/src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp index 98157c473..4236d0fc5 100644 --- a/src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp +++ b/src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp @@ -4,23 +4,16 @@ #include #include +#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); @@ -28,27 +21,18 @@ Eigen::VectorXi MaximumLikelihoodMD::associate( double best_cost = std::numeric_limits::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; } } diff --git a/src/perception_sensor_lib/src/data_association/maximum_likelihood_nll.cpp b/src/perception_sensor_lib/src/data_association/maximum_likelihood_nll.cpp index e95644de1..1db5567ea 100644 --- a/src/perception_sensor_lib/src/data_association/maximum_likelihood_nll.cpp +++ b/src/perception_sensor_lib/src/data_association/maximum_likelihood_nll.cpp @@ -5,90 +5,45 @@ #include Eigen::VectorXi MaximumLikelihoodNLL::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)); - - // Set fields of the jacobian that are common to all landmarks - Eigen::MatrixXd jacobian_local_coordinates = Eigen::MatrixXd::Zero(2, state.size()); - jacobian_local_coordinates(0, 0) = -cos_theta; - jacobian_local_coordinates(0, 1) = -sin_theta; - jacobian_local_coordinates(1, 0) = sin_theta; - jacobian_local_coordinates(1, 1) = -cos_theta; - - for (int i = 0; i < num_observations; ++i) { + 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::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 observations and landmarks + Eigen::Vector2d innovation = obs - landmarks.segment(2 * j, 2); - // Set fields of the jacobian that are not common to all landmarks. - jacobian_local_coordinates(0, 2) = -sin_theta * dx + cos_theta * dy; - jacobian_local_coordinates(1, 2) = -(cos_theta * dx + sin_theta * dy); - jacobian_local_coordinates(0, j) = cos_theta; - jacobian_local_coordinates(0, j + 1) = sin_theta; - jacobian_local_coordinates(1, j) = -sin_theta; - jacobian_local_coordinates(1, j + 1) = cos_theta; - - // Innovation: difference between actual and predicted observation - Eigen::Vector2d innovation = obs - landmarks_local_coordinates.segment(j - 3, 2); - - // Innovation covariance: S = H * covariance * H^T + Q Eigen::Matrix2d innovation_covariance = - jacobian_local_coordinates * covariance * jacobian_local_coordinates.transpose() + - this->observation_noise_covariance_matrix_; + 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) { 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; } - if (det < 0.001) det = 0.001; + if (det < 1e-9) det = 1e-9; double normalized_innovation_squared = innovation.transpose() * innovation_covariance.inverse() * innovation; double cost = normalized_innovation_squared + std::log(det); - // Check if this landmark is a good candidate for association if (normalized_innovation_squared < this->_params_.association_gate && cost < best_cost) { best_cost = cost; - best_landmark_index = j; + best_landmark_index = 2 * j; } - - // Reset fields of the jacobian that are not common to all landmarks - jacobian_local_coordinates(0, 2) = 0; - jacobian_local_coordinates(1, 2) = 0; - jacobian_local_coordinates(0, j) = 0; - jacobian_local_coordinates(0, j + 1) = 0; - jacobian_local_coordinates(1, j) = 0; - jacobian_local_coordinates(1, j + 1) = 0; } if (best_landmark_index != -1) { diff --git a/src/perception_sensor_lib/src/data_association/nearest_neighbor.cpp b/src/perception_sensor_lib/src/data_association/nearest_neighbor.cpp index b1eceab11..cebd7bdfa 100644 --- a/src/perception_sensor_lib/src/data_association/nearest_neighbor.cpp +++ b/src/perception_sensor_lib/src/data_association/nearest_neighbor.cpp @@ -3,22 +3,16 @@ NearestNeighbor::NearestNeighbor(const DataAssociationParameters& params) : DataAssociationModel(params) {} -Eigen::VectorXi NearestNeighbor::associate(const Eigen::VectorXd& state, - const Eigen::MatrixXd& covariance, +Eigen::VectorXi NearestNeighbor::associate(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) - // 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)); - Eigen::VectorXd landmarks_global_coordinates = state.segment(3, state.size() - 3); - Eigen::VectorXd observation_global_coordinates = - common_lib::maths::local_to_global_coordinates(state.segment(0, 3), observations); - for (int i = 0; i < num_observations; ++i) { Eigen::Vector2d obs; obs << observations(2 * i), observations(2 * i + 1); @@ -26,21 +20,14 @@ Eigen::VectorXi NearestNeighbor::associate(const Eigen::VectorXd& state, double best_cost = std::numeric_limits::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 - if (const double distance = - std::hypot(landmarks_local_coordinates(j - 3), landmarks_local_coordinates(j - 2)); - distance > this->_params_.max_landmark_distance) { - continue; - } - - const double euclidian_difference = std::hypot(obs(0) - landmarks_local_coordinates(j - 3), - obs(1) - landmarks_local_coordinates(j - 2)); + for (int j = 0; j < num_landmarks; j++) { + const double euclidean_distance = + std::hypot(obs(0) - landmarks(2 * j), obs(1) - landmarks(2 * j + 1)); // Check if this landmark is a good candidate for association - if (euclidian_difference < best_cost) { - best_cost = euclidian_difference; - best_landmark_index = j; + if (euclidean_distance < best_cost) { + best_cost = euclidean_distance; + best_landmark_index = 2 * j; } } diff --git a/src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp b/src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp index e8bdc1336..40e4f0e9e 100644 --- a/src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp +++ b/src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp @@ -1,47 +1,36 @@ #include "perception_sensor_lib/data_association/nearest_neighbour.hpp" -Eigen::VectorXi NearestNeighbour::associate(const Eigen::VectorXd& state, - const Eigen::MatrixXd& covariance, +Eigen::VectorXi NearestNeighbour::associate(const Eigen::VectorXd& landmarks, const Eigen::VectorXd& observations, + const Eigen::MatrixXd& covariance, const Eigen::VectorXd& observation_confidences) const { - // Transform observations to global coordinates - Eigen::VectorXd global_observations = - common_lib::maths::local_to_global_coordinates(state.segment(0, 3), observations); - - double squared_distance_threshold = - this->_params_.association_gate * this->_params_.association_gate; - double new_landmark_squared_distance_gate = 1.5; + double new_landmark_distance_gate = 1.2; + const int num_observations = observations.size() / 2; + const int num_landmarks = landmarks.size() / 2; Eigen::VectorXi associations = Eigen::VectorXi::Constant(observations.size() / 2, -2); - std::unordered_set used_indices((state.size() - 3) / 2); - - for (int i = 0; i < observations.size(); i += 2) { - if (observations(i) * observations(i) + observations(i + 1) * observations(i + 1) > - this->_params_.max_landmark_distance * this->_params_.max_landmark_distance) { - continue; - } + std::unordered_set used_indices(num_landmarks); + for (int i = 0; i < num_observations; i++) { double min_distance = std::numeric_limits::max(); int min_index = -1; - for (int j = 3; j < state.size(); j += 2) { - double squared_distance = - (state(j) - global_observations(i)) * (state(j) - global_observations(i)) + - (state(j + 1) - global_observations(i + 1)) * (state(j + 1) - global_observations(i + 1)); - - if (squared_distance < min_distance) { - min_distance = squared_distance; - min_index = j; + for (int j = 0; j < num_landmarks; j++) { + const double euclidean_distance = std::hypot(observations(2 * i) - landmarks(2 * j), + observations(2 * i + 1) - landmarks(2 * j + 1)); + if (euclidean_distance < min_distance) { + min_distance = euclidean_distance; + min_index = 2 * j; } } - if (observation_confidences(i / 2) >= this->_params_.new_landmark_confidence_gate || + if (observation_confidences(i) >= this->_params_.new_landmark_confidence_gate && used_indices.find(min_index) == used_indices.end()) { - if (min_distance < squared_distance_threshold) { - associations(i / 2) = min_index; + if (min_distance < this->_params_.association_gate) { + associations(i) = min_index; used_indices.insert(min_index); - } else if (min_distance > new_landmark_squared_distance_gate) { - associations(i / 2) = -1; + } else if (min_distance > new_landmark_distance_gate) { + associations(i) = -1; } } } diff --git a/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp b/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp new file mode 100644 index 000000000..44b0f3ba4 --- /dev/null +++ b/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp @@ -0,0 +1,87 @@ +#include "perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp" + +#include + +#include "common_lib/maths/transformations.hpp" +Eigen::VectorXd ConsecutiveCounterFilter::filter( + const Eigen::VectorXd& new_observations, const Eigen::VectorXd& new_observation_confidences) { + // Associate the observations that are considered new with the map stored in this filter + Eigen::VectorXi new_associations = this->_data_association_->associate( + this->map, new_observations, {}, new_observation_confidences); + const int num_landmarks = this->map.size() / 2; + const int num_new_observations = new_observations.size() / 2; + + // Calculate which observations should be added to this filter's map and which landmarks in the + // map were observed + std::vector was_observed(num_landmarks, false); + Eigen::VectorXd to_be_added_to_map; + for (int i = 0; i < num_new_observations; i++) { + if (new_associations(i) != -1 && new_associations(i) != -2) { + was_observed[new_associations(i) / 2] = true; + } else if (new_associations(i) == -1) { + to_be_added_to_map.conservativeResize(to_be_added_to_map.size() + 2); + to_be_added_to_map.segment(to_be_added_to_map.size() - 2, 2) = + new_observations.segment(i * 2, 2); + } + } + // Update counter + Eigen::VectorXd new_map; + Eigen::VectorXi new_counter; + Eigen::VectorXd filtered_observations; + for (int i = 0; i < num_landmarks; i++) { + if (!was_observed[i]) { + continue; + } + if (this->counter(i) >= this->_params_.minimum_observation_count_ - 1) { + filtered_observations.conservativeResize(filtered_observations.size() + 2); + for (int j = 0; j < num_new_observations; j++) { + if (new_associations[j] == i * 2) { + filtered_observations.segment(filtered_observations.size() - 2, 2) = + new_observations.segment(j * 2, 2); + break; + } + } + filtered_observations.segment(filtered_observations.size() - 2, 2) = + this->map.segment(i * 2, 2); + } + new_map.conservativeResize(new_map.size() + 2); + new_map.segment(new_map.size() - 2, 2) = this->map.segment(i * 2, 2); + new_counter.conservativeResize(new_counter.size() + 1); + new_counter(new_counter.size() - 1) = this->counter(i) + 1; + } + new_map.conservativeResize(new_map.size() + to_be_added_to_map.size()); + new_map.tail(to_be_added_to_map.size()) = to_be_added_to_map; + new_counter.conservativeResize(new_counter.size() + to_be_added_to_map.size() / 2); + new_counter.tail(to_be_added_to_map.size() / 2).setOnes(); + this->map = new_map; + this->counter = new_counter; + + return filtered_observations; +} + +void ConsecutiveCounterFilter::delete_landmarks(const Eigen::VectorXd& some_landmarks) { + int map_size = this->map.size() / 2; + int num_landmarks = some_landmarks.size() / 2; + std::vector to_be_deleted(map_size, false); + Eigen::VectorXd new_map; + Eigen::VectorXi new_counter; + for (int i = 0; i < num_landmarks; i++) { + for (int j = 0; j < map_size; j++) { + if (std::hypot(this->map(j * 2) - some_landmarks(i * 2), + this->map(j * 2 + 1) - some_landmarks(i * 2 + 1)) < EQUALITY_TOLERANCE) { + to_be_deleted[j] = true; + } + } + } + for (int i = 0; i < map_size; i++) { + if (to_be_deleted[i]) { + continue; + } + new_map.conservativeResize(new_map.size() + 2); + new_map.segment(new_map.size() - 2, 2) = this->map.segment(i * 2, 2); + new_counter.conservativeResize(new_counter.size() + 1); + new_counter(new_counter.size() - 1) = this->counter(i); + } + this->map = new_map; + this->counter = new_counter; +} \ No newline at end of file diff --git a/src/perception_sensor_lib/test/data_association/maximum_likelihood_md_test.cpp b/src/perception_sensor_lib/test/data_association/maximum_likelihood_md_test.cpp index 42a8947b8..ac9089d24 100644 --- a/src/perception_sensor_lib/test/data_association/maximum_likelihood_md_test.cpp +++ b/src/perception_sensor_lib/test/data_association/maximum_likelihood_md_test.cpp @@ -8,20 +8,22 @@ */ TEST(MaximumLikelihoodMD, TestCase1) { // Arrange - Eigen::VectorXd state(13); - state << 0, 0, 0, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << 4.5, 13.2, 34.2, -7.2, 3.1, 5.65, 6.86, 9.2, 1.7, 0.5; + observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975, + 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675, + 1.699999998811323, 0.49999999845647614; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -34,21 +36,22 @@ TEST(MaximumLikelihoodMD, TestCase1) { */ TEST(MaximumLikelihoodMD, TestCase2) { // Arrange - Eigen::VectorXd state(13); - state << 0, 0, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -5.0618836, 12.994896, 30.79597, 16.52538, -1.26881915231104, 6.31843318859, - -0.67998532, 11.455881457, 0.97812287485, 1.47759116; + observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975, + 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675, + 1.699999998811323, 0.49999999845647614; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -61,21 +64,22 @@ TEST(MaximumLikelihoodMD, TestCase2) { */ TEST(MaximumLikelihoodMD, TestCase3) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.2; - DataAssociationParameters params; - std::vector expected_associations = {-1, 3, 7, 9, 11}; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); + std::vector expected_associations = {-1, 0, 4, 6, 8}; MaximumLikelihoodMD ml(params); // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -88,21 +92,21 @@ TEST(MaximumLikelihoodMD, TestCase3) { */ TEST(MaximumLikelihoodMD, TestCase4) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(8); - observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, - 12.7823521, 0.449790270; + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); std::vector expected_associations = {-1, -1, -1, -1}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -115,21 +119,21 @@ TEST(MaximumLikelihoodMD, TestCase4) { */ TEST(MaximumLikelihoodMD, TestCase5) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(8); - observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, - 12.7823521, 0.449790270; + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); std::vector expected_associations = {-2, -2, -2, -2}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -142,21 +146,18 @@ TEST(MaximumLikelihoodMD, TestCase5) { */ TEST(MaximumLikelihoodMD, TestCase6) { // Arrange - Eigen::VectorXd state(3); - state << 4, -10, 2; + Eigen::VectorXd landmarks(0); Eigen::VectorXd observations(8); observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, 12.7823521, 0.449790270; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(3, 3); - covariance.setIdentity(); - covariance *= 0.6; - DataAssociationParameters params; + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); std::vector expected_associations = {-1, -1, -1, -1}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -169,18 +170,15 @@ TEST(MaximumLikelihoodMD, TestCase6) { */ TEST(MaximumLikelihoodMD, TestCase7) { // Arrange - Eigen::VectorXd state(3); - state << 4, -10, 2; + Eigen::VectorXd state(0); Eigen::VectorXd observations(0); - Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(3, 3); - covariance.setIdentity(); - covariance *= 0.6; - DataAssociationParameters params; + Eigen::VectorXd observation_confidences(0); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert EXPECT_EQ(associations.size(), 0); } @@ -191,23 +189,24 @@ TEST(MaximumLikelihoodMD, TestCase7) { */ TEST(MaximumLikelihoodMD, TestCase8) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.2; - covariance(5, 5) = 1000; // Very high uncertainty, should match even though wasn't observed - covariance(6, 6) = 1000; - DataAssociationParameters params; + covariance(2, 2) = 1000; // Very high uncertainty, should match even though wasn't observed + covariance(3, 3) = 1000; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); - std::vector expected_associations = {5, 3, 7, 9, 11}; + std::vector expected_associations = {2, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -220,23 +219,24 @@ TEST(MaximumLikelihoodMD, TestCase8) { */ TEST(MaximumLikelihoodMD, TestCase9) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.01; - covariance(3, 3) = 0; // Very low uncertainty, should not match even though was observed - covariance(4, 4) = 0; - DataAssociationParameters params; + covariance(0, 0) = 0; // Very low uncertainty, should not match even though was observed + covariance(1, 1) = 0; + DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1); MaximumLikelihoodMD ml(params); - std::vector expected_associations = {-1, -1, 7, 9, 11}; + std::vector expected_associations = {-1, -1, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); diff --git a/src/perception_sensor_lib/test/data_association/maximum_likelihood_nll_test.cpp b/src/perception_sensor_lib/test/data_association/maximum_likelihood_nll_test.cpp index 5675ddf60..85293f16a 100644 --- a/src/perception_sensor_lib/test/data_association/maximum_likelihood_nll_test.cpp +++ b/src/perception_sensor_lib/test/data_association/maximum_likelihood_nll_test.cpp @@ -8,20 +8,20 @@ */ TEST(MaximumLikelihoodNLL, TestCase1) { // Arrange - Eigen::VectorXd state(13); - state << 0, 0, 0, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); observations << 4.5, 13.2, 34.2, -7.2, 3.1, 5.65, 6.86, 9.2, 1.7, 0.5; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -34,21 +34,22 @@ TEST(MaximumLikelihoodNLL, TestCase1) { */ TEST(MaximumLikelihoodNLL, TestCase2) { // Arrange - Eigen::VectorXd state(13); - state << 0, 0, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -5.0618836, 12.994896, 30.79597, 16.52538, -1.26881915231104, 6.31843318859, - -0.67998532, 11.455881457, 0.97812287485, 1.47759116; + observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975, + 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675, + 1.699999998811323, 0.49999999845647614; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -61,21 +62,22 @@ TEST(MaximumLikelihoodNLL, TestCase2) { */ TEST(MaximumLikelihoodNLL, TestCase3) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.2; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -88,21 +90,21 @@ TEST(MaximumLikelihoodNLL, TestCase3) { */ TEST(MaximumLikelihoodNLL, TestCase4) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(8); - observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, - 12.7823521, 0.449790270; + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); std::vector expected_associations = {-1, -1, -1, -1}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -115,21 +117,21 @@ TEST(MaximumLikelihoodNLL, TestCase4) { */ TEST(MaximumLikelihoodNLL, TestCase5) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(8); - observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, - 12.7823521, 0.449790270; + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.1; - DataAssociationParameters params; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); std::vector expected_associations = {-2, -2, -2, -2}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -142,21 +144,18 @@ TEST(MaximumLikelihoodNLL, TestCase5) { */ TEST(MaximumLikelihoodNLL, TestCase6) { // Arrange - Eigen::VectorXd state(3); - state << 4, -10, 2; + Eigen::VectorXd state(0); Eigen::VectorXd observations(8); observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, 12.7823521, 0.449790270; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(3, 3); - covariance.setIdentity(); - covariance *= 0.6; - DataAssociationParameters params; + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); std::vector expected_associations = {-1, -1, -1, -1}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -169,18 +168,15 @@ TEST(MaximumLikelihoodNLL, TestCase6) { */ TEST(MaximumLikelihoodNLL, TestCase7) { // Arrange - Eigen::VectorXd state(3); - state << 4, -10, 2; + Eigen::VectorXd state(0); Eigen::VectorXd observations(0); - Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(3, 3); - covariance.setIdentity(); - covariance *= 0.6; - DataAssociationParameters params; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(0); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert EXPECT_EQ(associations.size(), 0); } @@ -191,23 +187,24 @@ TEST(MaximumLikelihoodNLL, TestCase7) { */ TEST(MaximumLikelihoodNLL, TestCase8) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.2; - covariance(5, 5) = 1000; // Very high uncertainty, should match even though wasn't observed - covariance(6, 6) = 1000; - DataAssociationParameters params; + covariance(2, 2) = 1000; // Very high uncertainty, should match even though wasn't observed + covariance(3, 3) = 1000; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); - std::vector expected_associations = {5, 3, 7, 9, 11}; + std::vector expected_associations = {2, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -220,23 +217,24 @@ TEST(MaximumLikelihoodNLL, TestCase8) { */ TEST(MaximumLikelihoodNLL, TestCase9) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd state(10); + state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); + Eigen::MatrixXd covariance(10, 10); covariance.setIdentity(); covariance *= 0.01; - covariance(3, 3) = 0; // Very low uncertainty, should not match even though was observed - covariance(4, 4) = 0; - DataAssociationParameters params; + covariance(0, 0) = 0; // Very low uncertainty, should not match even though was observed + covariance(1, 1) = 0; + DataAssociationParameters params(50.0, 0.8, 0.8, 0.1, 0.1); MaximumLikelihoodNLL ml(params); - std::vector expected_associations = {-1, -1, 7, 9, 11}; + std::vector expected_associations = {-1, -1, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(state, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); diff --git a/src/perception_sensor_lib/test/data_association/nearest_neighbor_test.cpp b/src/perception_sensor_lib/test/data_association/nearest_neighbor_test.cpp index c9d32ece7..fee767b16 100644 --- a/src/perception_sensor_lib/test/data_association/nearest_neighbor_test.cpp +++ b/src/perception_sensor_lib/test/data_association/nearest_neighbor_test.cpp @@ -8,18 +8,18 @@ */ TEST(NearestNeighbor, TestCase1) { // Arrange - Eigen::VectorXd state(13); - state << 0, 0, 0, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); observations << 4.5, 13.2, 34.2, -7.2, 3.1, 5.65, 6.86, 9.2, 1.7, 0.5; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); - DataAssociationParameters params; + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); NearestNeighbor ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -32,19 +32,20 @@ TEST(NearestNeighbor, TestCase1) { */ TEST(NearestNeighbor, TestCase2) { // Arrange - Eigen::VectorXd state(13); - state << 0, 0, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -5.0618836, 12.994896, 30.79597, 16.52538, -1.26881915231104, 6.31843318859, - -0.67998532, 11.455881457, 0.97812287485, 1.47759116; + observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975, + 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675, + 1.699999998811323, 0.49999999845647614; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); - DataAssociationParameters params; + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); NearestNeighbor ml(params); - std::vector expected_associations = {-1, 3, 7, 9, 11}; + std::vector expected_associations = {-1, 0, 4, 6, 8}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -57,19 +58,20 @@ TEST(NearestNeighbor, TestCase2) { */ TEST(NearestNeighbor, TestCase3) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(10); - observations << -1.59954619, 11.988805, 34.25830, 15.5192899677, 2.193518284, 5.3123420012, - 2.7823521, 10.449790270, 4.440460311, 0.47149997; + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); - Eigen::MatrixXd covariance(13, 13); - DataAssociationParameters params; - std::vector expected_associations = {-1, 3, 7, 9, 11}; + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + std::vector expected_associations = {-1, 0, 4, 6, 8}; NearestNeighbor ml(params); // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -82,19 +84,19 @@ TEST(NearestNeighbor, TestCase3) { */ TEST(NearestNeighbor, TestCase4) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(8); - observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, - 12.7823521, 0.449790270; + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(13, 13); - DataAssociationParameters params; + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); NearestNeighbor ml(params); std::vector expected_associations = {-1, -1, -1, -1}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -107,19 +109,19 @@ TEST(NearestNeighbor, TestCase4) { */ TEST(NearestNeighbor, TestCase5) { // Arrange - Eigen::VectorXd state(13); - state << -2, 3, -0.7, 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; Eigen::VectorXd observations(8); - observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, - 12.7823521, 0.449790270; + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4); - Eigen::MatrixXd covariance(13, 13); - DataAssociationParameters params; + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); NearestNeighbor ml(params); std::vector expected_associations = {-2, -2, -2, -2}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -127,24 +129,23 @@ TEST(NearestNeighbor, TestCase5) { } /** - * @brief Only new landmarks with high confidence, high covariance and empty state + * @brief Only new landmarks with high confidence, high covariance and empty landmarks * */ TEST(NearestNeighbor, TestCase6) { // Arrange - Eigen::VectorXd state(3); - state << 4, -10, 2; + Eigen::VectorXd landmarks(0); Eigen::VectorXd observations(8); observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, 12.7823521, 0.449790270; Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(3, 3); - DataAssociationParameters params; + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); NearestNeighbor ml(params); std::vector expected_associations = {-1, -1, -1, -1}; // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert for (int i = 0; i < static_cast(associations.size()); ++i) { EXPECT_EQ(associations[i], expected_associations[i]); @@ -152,21 +153,20 @@ TEST(NearestNeighbor, TestCase6) { } /** - * @brief Empty state and observations + * @brief Empty landmarks and observations * */ TEST(NearestNeighbor, TestCase7) { // Arrange - Eigen::VectorXd state(3); - state << 4, -10, 2; + Eigen::VectorXd landmarks(0); Eigen::VectorXd observations(0); - Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); - Eigen::MatrixXd covariance(3, 3); - DataAssociationParameters params; + Eigen::VectorXd observation_confidences(0); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); NearestNeighbor ml(params); // Act Eigen::VectorXi associations = - ml.associate(state, covariance, observations, observation_confidences); + ml.associate(landmarks, observations, covariance, observation_confidences); // Assert EXPECT_EQ(associations.size(), 0); } diff --git a/src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp b/src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp new file mode 100644 index 000000000..35dafede0 --- /dev/null +++ b/src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp @@ -0,0 +1,172 @@ +#include "perception_sensor_lib/data_association/nearest_neighbour.hpp" + +#include + +/** + * @brief Test with mostly easy matches and a new landmark. + * + */ +TEST(NearestNeighbour, TestCase1) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(10); + observations << 4.5, 13.2, 34.2, -7.2, 3.1, 5.65, 6.86, 9.2, 1.7, 0.5; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbour ml(params); + std::vector expected_associations = {-1, 0, 4, 6, 8}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Same case as TestCase1 but with rotation. + * + */ +TEST(NearestNeighbour, TestCase2) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(10); + observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975, + 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675, + 1.699999998811323, 0.49999999845647614; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbour ml(params); + std::vector expected_associations = {-1, 0, 4, 6, 8}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Same case as TestCase2 but with translation. + * + */ +TEST(NearestNeighbour, TestCase3) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(10); + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + std::vector expected_associations = {-1, 0, 4, 6, 8}; + NearestNeighbour ml(params); + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Only new landmarks with high confidence + * + */ +TEST(NearestNeighbour, TestCase4) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(8); + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbour ml(params); + std::vector expected_associations = {-1, -1, -1, -1}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Only new landmarks with low confidence + * + */ +TEST(NearestNeighbour, TestCase5) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(8); + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbour ml(params); + std::vector expected_associations = {-2, -2, -2, -2}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Only new landmarks with high confidence, high covariance and empty landmarks + * + */ +TEST(NearestNeighbour, TestCase6) { + // Arrange + Eigen::VectorXd landmarks(0); + Eigen::VectorXd observations(8); + observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, + 12.7823521, 0.449790270; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbour ml(params); + std::vector expected_associations = {-1, -1, -1, -1}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Empty landmarks and observations + * + */ +TEST(NearestNeighbour, TestCase7) { + // Arrange + Eigen::VectorXd landmarks(0); + Eigen::VectorXd observations(0); + Eigen::VectorXd observation_confidences(0); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbour ml(params); + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + EXPECT_EQ(associations.size(), 0); +} diff --git a/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp b/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp new file mode 100644 index 000000000..4ccf4605a --- /dev/null +++ b/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp @@ -0,0 +1,117 @@ +#include "perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp" + +#include + +#include "perception_sensor_lib/data_association/nearest_neighbor.hpp" + +/** + * @brief Test with same observations 3 times and car still. + * + */ +TEST(ConsecutiveCounterFilter, TestCase1) { + Eigen::VectorXd observations(4); + observations << 0.1, 0.2, 0.7, 0.8; + Eigen::VectorXd confidences(2); + confidences << 0.99, 0.99; + LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); + std::shared_ptr data_association = + std::make_shared(data_association_params); + ConsecutiveCounterFilter filter(params, data_association); + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + filtered_observations = filter.filter(observations, confidences); + ASSERT_EQ(filtered_observations.size(), 4); + EXPECT_EQ(filtered_observations(0), 0.1); + EXPECT_EQ(filtered_observations(1), 0.2); + EXPECT_EQ(filtered_observations(2), 0.7); + EXPECT_EQ(filtered_observations(3), 0.8); +} + +/** + * @brief Same test as above but with different order of observations. + * + */ +TEST(ConsecutiveCounterFilter, TestCase2) { + Eigen::VectorXd observations(4); + Eigen::VectorXd confidences(2); + LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); + std::shared_ptr data_association = + std::make_shared(data_association_params); + ConsecutiveCounterFilter filter(params, data_association); + observations << 0.1, 0.2, 0.7, 0.8; + confidences << 0.99, 0.99; + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + observations << 0.1, 0.2, 0.7, 0.8; + confidences << 0.99, 0.99; + filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + observations << 0.7, 0.8, 0.1, 0.2; + confidences << 0.99, 0.99; + filtered_observations = filter.filter(observations, confidences); + ASSERT_EQ(filtered_observations.size(), 4); + EXPECT_EQ(filtered_observations(0), 0.1); + EXPECT_EQ(filtered_observations(1), 0.2); + EXPECT_EQ(filtered_observations(2), 0.7); + EXPECT_EQ(filtered_observations(3), 0.8); +} + +/** + * @brief Same test as above but one of the landmarks is not observed 3 times. + * + */ +TEST(ConsecutiveCounterFilter, TestCase3) { + Eigen::VectorXd observations(4); + Eigen::VectorXd confidences(2); + LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); + std::shared_ptr data_association = + std::make_shared(data_association_params); + ConsecutiveCounterFilter filter(params, data_association); + observations << 0.1, 0.2, 0.7, 0.8; + confidences << 0.99, 0.99; + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + observations << 0.1, 0.2, 0.7, 0.8; + confidences << 0.99, 0.99; + filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + observations << 0.7, 0.8, 2, 2.1; + confidences << 0.99, 0.99; + filtered_observations = filter.filter(observations, confidences); + ASSERT_EQ(filtered_observations.size(), 2); + EXPECT_EQ(filtered_observations(0), 0.7); + EXPECT_EQ(filtered_observations(1), 0.8); +} + +/** + * @brief Same test as above but the car moves and observations change accordingly. + * + */ +TEST(ConsecutiveCounterFilter, TestCase4) { + Eigen::VectorXd observations(4); + Eigen::VectorXd confidences(2); + LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); + std::shared_ptr data_association = + std::make_shared(data_association_params); + ConsecutiveCounterFilter filter(params, data_association); + observations << 0.1, 0.2, 0.7, 0.8; + confidences << 0.99, 0.99; + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + observations << 0.1, 0.2, 0.7, 0.8; + confidences << 0.99, 0.99; + filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(filtered_observations.size(), 0); + observations << 0.6980866604370328, 0.7972940119689251, 2.000155460474267, 2.0952219144232935; + confidences << 0.99, 0.99; + filtered_observations = filter.filter(observations, confidences); + ASSERT_EQ(filtered_observations.size(), 2); + EXPECT_EQ(filtered_observations(0), 0.7); + EXPECT_EQ(filtered_observations(1), 0.8); +} \ No newline at end of file diff --git a/src/slam/include/slam_config/general_config.hpp b/src/slam/include/slam_config/general_config.hpp index a6155c194..f12bf59e7 100644 --- a/src/slam/include/slam_config/general_config.hpp +++ b/src/slam/include/slam_config/general_config.hpp @@ -15,6 +15,7 @@ struct SLAMParameters { std::string motion_model_name_ = "constant_velocity"; std::string data_association_model_name_ = "nearest_neighbor"; std::string slam_solver_name_ = "graph_slam"; + std::string landmark_filter_name_ = "consecutive_count"; std::string frame_id_ = "map"; float data_association_limit_distance_ = 70; float observation_x_noise_ = 0.01; @@ -28,6 +29,10 @@ struct SLAMParameters { 0.3; //< Minimum pose difference to add a new pose to the graph double slam_optimization_period_ = 0.0; //< Period for running optimization of the graph (s), // 0.0 means optimization on observations receival + int minimum_observation_count_ = + 3; // Minimum number of times a landmark must be observed to be added to the map + double minimum_frequency_of_detections_ = + 5; // Minimum frequency of the detections of a landmark to add it to the map std::string slam_optimization_type_ = "normal_levenberg"; std::string slam_optimization_mode_ = "sync"; diff --git a/src/slam/include/slam_solver/ekf_slam_solver.hpp b/src/slam/include/slam_solver/ekf_slam_solver.hpp index 37128c226..31745122f 100644 --- a/src/slam/include/slam_solver/ekf_slam_solver.hpp +++ b/src/slam/include/slam_solver/ekf_slam_solver.hpp @@ -82,11 +82,9 @@ class EKFSLAMSolver : public SLAMSolver { * @param covariance covariance matrix of the state vector * @param new_landmarks new landmarks in the form {x_cone_1, y_cone_1, x_cone_2, y_cone_2, ...} in * the car's frame - * @param new_landmarks_confidence confidence of the new landmarks */ void state_augmentation(Eigen::VectorXd& state, Eigen::MatrixXd& covariance, - const Eigen::VectorXd& new_landmarks, - const Eigen::VectorXd& new_landmarks_confidence); + const Eigen::VectorXd& new_landmarks); std::vector get_map_estimate() override; common_lib::structures::Pose get_pose_estimate() override; @@ -103,6 +101,7 @@ class EKFSLAMSolver : public SLAMSolver { EKFSLAMSolver(const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times); /** diff --git a/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp b/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp index 56e6e9375..b46bae888 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp @@ -25,14 +25,17 @@ struct ObservationData { std::shared_ptr observations_; std::shared_ptr associations_; std::shared_ptr observations_global_; + std::shared_ptr observations_confidences_; rclcpp::Time timestamp_; ObservationData() = default; ObservationData(std::shared_ptr observations, std::shared_ptr associations, - std::shared_ptr observations_global, rclcpp::Time timestamp) + std::shared_ptr observations_global, + std::shared_ptr observations_confidences, rclcpp::Time timestamp) : observations_(observations), associations_(associations), observations_global_(observations_global), + observations_confidences_(observations_confidences), timestamp_(timestamp) {} }; \ No newline at end of file diff --git a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp index 9ffa954cb..f8e799783 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp @@ -57,6 +57,7 @@ class GraphSLAMSolver : public SLAMSolver { GraphSLAMSolver(const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times); ~GraphSLAMSolver() = default; diff --git a/src/slam/include/slam_solver/map.hpp b/src/slam/include/slam_solver/map.hpp index 25a104727..094ea9abb 100644 --- a/src/slam/include/slam_solver/map.hpp +++ b/src/slam/include/slam_solver/map.hpp @@ -14,21 +14,24 @@ const std::map( const SLAMParameters&, std::shared_ptr, - std::shared_ptr, std::shared_ptr>)>, + std::shared_ptr, std::shared_ptr, + std::shared_ptr>)>, std::less<>> slam_solver_constructors_map = { {"graph_slam", [](const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times) -> std::shared_ptr { return std::make_shared(params, data_association, motion_model, - execution_times); + landmark_filter, execution_times); }}, {"ekf_slam", [](const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times) -> std::shared_ptr { return std::make_shared(params, data_association, motion_model, - execution_times); + landmark_filter, execution_times); }}, }; diff --git a/src/slam/include/slam_solver/slam_solver.hpp b/src/slam/include/slam_solver/slam_solver.hpp index b10f7c446..d12b94f98 100644 --- a/src/slam/include/slam_solver/slam_solver.hpp +++ b/src/slam/include/slam_solver/slam_solver.hpp @@ -7,6 +7,7 @@ #include "common_lib/structures/velocities.hpp" #include "motion_lib/v2p_models/base_v2p_motion_model.hpp" #include "perception_sensor_lib/data_association/base_data_association.hpp" +#include "perception_sensor_lib/landmark_filter/base_landmark_filter.hpp" #include "rclcpp/rclcpp.hpp" #include "slam_config/general_config.hpp" @@ -20,6 +21,7 @@ class SLAMSolver { SLAMParameters _params_; std::shared_ptr _data_association_; std::shared_ptr _motion_model_; + std::shared_ptr _landmark_filter_; std::shared_ptr> _execution_times_; //< Execution times: 0 -> total motion; 1 -> total // observation; the rest are solver specific @@ -41,6 +43,7 @@ class SLAMSolver { */ SLAMSolver(const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times); virtual ~SLAMSolver() = default; diff --git a/src/slam/launch/slam.launch.py b/src/slam/launch/slam.launch.py index af98620ba..606494795 100644 --- a/src/slam/launch/slam.launch.py +++ b/src/slam/launch/slam.launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): package="slam", executable="slam", name="slam", + output="screen", arguments=["--ros-args", "--log-level", "slam:=info"], ), ] diff --git a/src/slam/src/ros_node/slam_node.cpp b/src/slam/src/ros_node/slam_node.cpp index 677b70afb..b4dec6283 100644 --- a/src/slam/src/ros_node/slam_node.cpp +++ b/src/slam/src/ros_node/slam_node.cpp @@ -12,6 +12,7 @@ #include "common_lib/structures/position.hpp" #include "motion_lib/v2p_models/map.hpp" #include "perception_sensor_lib/data_association/map.hpp" +#include "perception_sensor_lib/landmark_filter/map.hpp" #include "slam_solver/map.hpp" /*---------------------- Constructor --------------------*/ @@ -26,12 +27,17 @@ SLAMNode::SLAMNode(const SLAMParameters ¶ms) : Node("slam") { params.data_association_limit_distance_, params.data_association_gate_, params.new_landmark_confidence_gate_, params.observation_x_noise_, params.observation_y_noise_)); + std::shared_ptr landmark_filter = + landmark_filters_map.at(params.landmark_filter_name_)( + LandmarkFilterParameters(params.minimum_observation_count_, + params.minimum_frequency_of_detections_), + data_association); this->_execution_times_ = std::make_shared>(20, 0.0); // Initialize SLAM solver object this->_slam_solver_ = slam_solver_constructors_map.at(params.slam_solver_name_)( - params, data_association, motion_model, this->_execution_times_); + params, data_association, motion_model, landmark_filter, this->_execution_times_); _perception_map_ = std::vector(); _vehicle_state_velocities_ = common_lib::structures::Velocities(); @@ -82,6 +88,7 @@ void SLAMNode::_perception_subscription_callback(const custom_interfaces::msg::C RCLCPP_DEBUG(this->get_logger(), "SUB - Perception: %ld cones", cone_array.size()); if (!this->_go_) { + RCLCPP_INFO(this->get_logger(), "ATTR - Mission not started yet"); return; } @@ -213,4 +220,4 @@ void SLAMNode::_publish_covariance() { } } this->_covariance_publisher_->publish(covariance_msg); -} \ No newline at end of file +} diff --git a/src/slam/src/slam_config/general_config.cpp b/src/slam/src/slam_config/general_config.cpp index 161eb1a79..a25fbacd3 100644 --- a/src/slam/src/slam_config/general_config.cpp +++ b/src/slam/src/slam_config/general_config.cpp @@ -18,6 +18,9 @@ SLAMParameters::SLAMParameters(const SLAMParameters ¶ms) { slam_solver_name_ = params.slam_solver_name_; slam_min_pose_difference_ = params.slam_min_pose_difference_; slam_optimization_period_ = params.slam_optimization_period_; + landmark_filter_name_ = params.landmark_filter_name_; + minimum_frequency_of_detections_ = params.minimum_frequency_of_detections_; + minimum_observation_count_ = params.minimum_observation_count_; frame_id_ = params.frame_id_; slam_optimization_type_ = params.slam_optimization_type_; slam_optimization_mode_ = params.slam_optimization_mode_; @@ -58,6 +61,10 @@ std::string SLAMParameters::load_config() { this->slam_solver_name_ = slam_config["slam"]["slam_solver_name"].as(); this->slam_min_pose_difference_ = slam_config["slam"]["slam_min_pose_difference"].as(); this->slam_optimization_period_ = slam_config["slam"]["slam_optimization_period"].as(); + this->landmark_filter_name_ = slam_config["slam"]["landmark_filter_name"].as(); + this->minimum_observation_count_ = slam_config["slam"]["minimum_observation_count"].as(); + this->minimum_frequency_of_detections_ = + slam_config["slam"]["minimum_frequency_of_detections"].as(); this->slam_optimization_type_ = slam_config["slam"]["slam_optimization_type"].as(); this->slam_optimization_mode_ = slam_config["slam"]["slam_optimization_mode"].as(); this->slam_isam2_relinearize_threshold_ = diff --git a/src/slam/src/slam_solver/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp index eb6719f8f..70427de8a 100644 --- a/src/slam/src/slam_solver/ekf_slam_solver.cpp +++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp @@ -3,8 +3,9 @@ EKFSLAMSolver::EKFSLAMSolver(const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times) - : SLAMSolver(params, data_association, motion_model, execution_times), + : SLAMSolver(params, data_association, motion_model, landmark_filter, execution_times), slam_parameters_(params) { this->covariance_ = Eigen::MatrixXd::Identity(3, 3) * 0.4; // TODO: initialize with the right values @@ -41,31 +42,37 @@ void EKFSLAMSolver::add_observations(const std::vector matched_landmarks_indices; Eigen::VectorXd matched_observations(0); Eigen::VectorXd new_landmarks(0); + Eigen::VectorXd new_confidences(0); Eigen::VectorXd observations(2 * num_observations); Eigen::VectorXd observation_confidences(num_observations); - Eigen::VectorXd new_landmarks_confidences(0); common_lib::conversions::cone_vector_to_eigen(cones, observations, observation_confidences); + Eigen::VectorXd global_observations = + common_lib::maths::local_to_global_coordinates(this->state_.segment(0, 3), observations); Eigen::VectorXi associations = this->_data_association_->associate( - this->state_, this->covariance_, observations, observation_confidences); + this->state_.segment(3, this->state_.size() - 3), global_observations, + this->covariance_.block(3, 3, this->state_.size() - 3, this->state_.size() - 3), + observation_confidences); for (int i = 0; i < num_observations; ++i) { if (associations(i) == -2) { continue; } else if (associations(i) == -1) { new_landmarks.conservativeResize(new_landmarks.size() + 2); - new_landmarks(new_landmarks.size() - 2) = observations(2 * i); - new_landmarks(new_landmarks.size() - 1) = observations(2 * i + 1); - new_landmarks_confidences.conservativeResize(new_landmarks_confidences.size() + 1); - new_landmarks_confidences(new_landmarks_confidences.size() - 1) = observation_confidences(i); + new_landmarks(new_landmarks.size() - 2) = global_observations(2 * i); + new_landmarks(new_landmarks.size() - 1) = global_observations(2 * i + 1); + new_confidences.conservativeResize(new_confidences.size() + 1); + new_confidences(new_confidences.size() - 1) = observation_confidences(i); } else { - matched_landmarks_indices.push_back(associations(i)); + matched_landmarks_indices.push_back(associations(i) + 3); matched_observations.conservativeResize(matched_observations.size() + 2); matched_observations(matched_observations.size() - 2) = observations(2 * i); matched_observations(matched_observations.size() - 1) = observations(2 * i + 1); } } + Eigen::VectorXd filtered_new_landmarks = + this->_landmark_filter_->filter(new_landmarks, new_confidences); + this->_landmark_filter_->delete_landmarks(filtered_new_landmarks); this->correct(this->state_, this->covariance_, matched_landmarks_indices, matched_observations); - this->state_augmentation(this->state_, this->covariance_, new_landmarks, - new_landmarks_confidences); + this->state_augmentation(this->state_, this->covariance_, filtered_new_landmarks); this->update_process_noise_matrix(); } @@ -111,24 +118,23 @@ void EKFSLAMSolver::correct(Eigen::VectorXd& state, Eigen::MatrixXd& covariance, } void EKFSLAMSolver::state_augmentation(Eigen::VectorXd& state, Eigen::MatrixXd& covariance, - const Eigen::VectorXd& new_landmarks_coordinates, - const Eigen::VectorXd& new_landmarks_confidences) { + const Eigen::VectorXd& new_landmarks_coordinates) { // Resize covariance matrix int num_new_entries = static_cast(new_landmarks_coordinates.size()); int covariance_size = static_cast(covariance.rows()); covariance.conservativeResizeLike( Eigen::MatrixXd::Zero(covariance_size + num_new_entries, covariance_size + num_new_entries)); - for (int i = 0; i < new_landmarks_confidences.size(); i++) { - covariance(2 * i + covariance_size, 2 * i + covariance_size) = new_landmarks_confidences(i); + for (int i = 0; i < num_new_entries / 2; i++) { + covariance(2 * i + covariance_size, 2 * i + covariance_size) = + this->_params_.observation_x_noise_; covariance(2 * i + 1 + covariance_size, 2 * i + 1 + covariance_size) = - new_landmarks_confidences(i); + _params_.observation_y_noise_; } // Resize state vector int original_state_size = static_cast(state.size()); state.conservativeResizeLike(Eigen::VectorXd::Zero(original_state_size + num_new_entries)); - state.segment(original_state_size, num_new_entries) = - this->observation_model_->inverse_observation_model(state, new_landmarks_coordinates); + state.segment(original_state_size, num_new_entries) = new_landmarks_coordinates; } std::vector EKFSLAMSolver::get_map_estimate() { diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp index de3f37673..c64ae15be 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp @@ -70,7 +70,7 @@ Eigen::MatrixXd GraphSLAMInstance::get_covariance_matrix() const { // this->_covariance_up_to_date_ = true; // return _covariance_; - return Eigen::MatrixXd::Zero(this->_landmark_counter_, this->_landmark_counter_); + return Eigen::MatrixXd::Zero(2 * this->_landmark_counter_, 2 * this->_landmark_counter_); } GraphSLAMInstance::GraphSLAMInstance(const SLAMParameters& params, @@ -168,7 +168,7 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ gtsam::Point2 landmark; gtsam::Symbol landmark_symbol; bool landmark_lost_in_optimization = - static_cast(this->_landmark_counter_) < (associations(i) - 3) / 2 + 1; + static_cast(this->_landmark_counter_) < (associations(i)) / 2; if (associations(i) == -2) { // No association continue; @@ -179,8 +179,7 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ this->_graph_values_.insert(landmark_symbol, landmark); } else { // Association to previous landmark - landmark_symbol = - gtsam::Symbol('l', (associations(i) - 3) / 2 + 1); // Convert to landmark id + landmark_symbol = gtsam::Symbol('l', (associations(i)) / 2 + 1); // Convert to landmark id } const Eigen::Vector2d observation_cartesian(observations[i * 2], observations[i * 2 + 1]); Eigen::Vector2d observation_cylindrical = common_lib::maths::cartesian_to_cylindrical( diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp index 56e1e5eaa..a91b90f98 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include "common_lib/maths/transformations.hpp" @@ -31,8 +32,9 @@ Eigen::Vector3d gtsam_pose_to_eigen(const gtsam::Pose2& pose) { GraphSLAMSolver::GraphSLAMSolver(const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times) - : SLAMSolver(params, data_association, motion_model, execution_times), + : SLAMSolver(params, data_association, motion_model, landmark_filter, execution_times), _graph_slam_instance_(params, graph_slam_optimizer_constructors_map.at( params.slam_optimization_type_)(params)) { // TODO: transform into range and bearing noises @@ -91,6 +93,9 @@ void GraphSLAMSolver::add_motion_prior(const common_lib::structures::Velocities& } void GraphSLAMSolver::add_observations(const std::vector& cones) { + if (cones.empty()) { + return; + } rclcpp::Time start_time, initialization_time, covariance_time, association_time, factor_graph_time, optimization_time; start_time = rclcpp::Clock().now(); @@ -109,10 +114,6 @@ void GraphSLAMSolver::add_observations(const std::vector_mutex_); - // Only proceed if the vehicle has moved - if (!this->_graph_slam_instance_.new_pose_factors()) { - return; - } state = this->_graph_slam_instance_.get_state_vector(); observations_global = common_lib::maths::local_to_global_coordinates(state.head<3>(), observations); @@ -124,10 +125,46 @@ void GraphSLAMSolver::add_observations(const std::vector_data_association_->associate( - state, covariance, observations, + state.segment(3, state.size() - 3), observations_global, covariance, observations_confidences); // TODO: implement different mahalanobis distance association_time = rclcpp::Clock().now(); RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Associations calculated"); + Eigen::VectorXd unfiltered_new_observations; + Eigen::VectorXd unfiltered_new_observations_confidences; + for (int i = 0; i < associations.size(); i++) { + if (associations(i) == -1) { + unfiltered_new_observations.conservativeResize(unfiltered_new_observations.size() + 2); + unfiltered_new_observations(unfiltered_new_observations.size() - 2) = + observations_global(i * 2); + unfiltered_new_observations(unfiltered_new_observations.size() - 1) = + observations_global(i * 2 + 1); + unfiltered_new_observations_confidences.conservativeResize( + unfiltered_new_observations_confidences.size() + 1); + unfiltered_new_observations_confidences(unfiltered_new_observations_confidences.size() - 1) = + observations_confidences(i); + associations(i) = -2; // Mark as not ready to be added to the graph + } + } + Eigen::VectorXd filtered_new_observations = this->_landmark_filter_->filter( + unfiltered_new_observations, unfiltered_new_observations_confidences); + + if (!this->_graph_slam_instance_.new_pose_factors()) { + return; + } + + this->_landmark_filter_->delete_landmarks(filtered_new_observations); + + // Set the associations to -1 for the filtered observations + for (int i = 0; i < filtered_new_observations.size() / 2; i++) { + for (int j = 0; j < associations.size(); j++) { + if (std::hypot(filtered_new_observations(i * 2) - observations_global(j * 2), + filtered_new_observations(i * 2 + 1) - observations_global(j * 2 + 1)) < + common_lib::structures::Cone::equality_tolerance) { + associations(j) = -1; + break; + } + } + } { RCLCPP_DEBUG(rclcpp::get_logger("slam"), @@ -136,6 +173,7 @@ void GraphSLAMSolver::add_observations(const std::vector(observations), std::make_shared(associations), std::make_shared(observations_global), + std::make_shared(observations_confidences), cones.at(0).timestamp); if (this->_optimization_under_way_) { this->_observation_data_queue_.push(observation_data); diff --git a/src/slam/src/slam_solver/slam_solver.cpp b/src/slam/src/slam_solver/slam_solver.cpp index 1dc55045f..348e1b82f 100644 --- a/src/slam/src/slam_solver/slam_solver.cpp +++ b/src/slam/src/slam_solver/slam_solver.cpp @@ -3,8 +3,10 @@ SLAMSolver::SLAMSolver(const SLAMParameters& params, std::shared_ptr data_association, std::shared_ptr motion_model, + std::shared_ptr landmark_filter, std::shared_ptr> execution_times) : _params_(params), _data_association_(data_association), _motion_model_(motion_model), + _landmark_filter_(landmark_filter), _execution_times_(execution_times) {} \ No newline at end of file diff --git a/src/slam/test/slam_solver/ekf_slam_solver_test.cpp b/src/slam/test/slam_solver/ekf_slam_solver_test.cpp index 4d9198bf4..f57e91efc 100644 --- a/src/slam/test/slam_solver/ekf_slam_solver_test.cpp +++ b/src/slam/test/slam_solver/ekf_slam_solver_test.cpp @@ -4,6 +4,7 @@ #include "motion_lib/v2p_models/map.hpp" #include "perception_sensor_lib/data_association/map.hpp" +#include "perception_sensor_lib/landmark_filter/map.hpp" class EKFSLAMSolverTest : public ::testing::Test { protected: @@ -17,12 +18,13 @@ class EKFSLAMSolverTest : public ::testing::Test { params.observation_y_noise_)); motion_model = v2p_models_map.at(params.motion_model_name_)(); - ekf_slam_solver = - std::make_shared(params, data_association, motion_model, execution_time); + ekf_slam_solver = std::make_shared(params, data_association, motion_model, + landmark_filter, execution_time); } std::shared_ptr data_association; std::shared_ptr motion_model; + std::shared_ptr landmark_filter; std::shared_ptr ekf_slam_solver; std::shared_ptr> execution_time; std::weak_ptr node; @@ -36,11 +38,8 @@ TEST_F(EKFSLAMSolverTest, stateAugmentation) { Eigen::VectorXd state = Eigen::VectorXd::Zero(3); Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(3, 3); Eigen::VectorXd new_landmarks_coordinates(10); - Eigen::VectorXd new_landmarks_confidences(5); new_landmarks_coordinates << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; - new_landmarks_confidences << 0.1, 0.2, 0.3, 0.4, 0.5; - ekf_slam_solver->state_augmentation(state, covariance, new_landmarks_coordinates, - new_landmarks_confidences); + ekf_slam_solver->state_augmentation(state, covariance, new_landmarks_coordinates); std::vector expected_state = {0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; EXPECT_EQ(state.size(), expected_state.size()); for (int i = 0; i < state.size(); i++) { @@ -59,10 +58,8 @@ TEST_F(EKFSLAMSolverTest, stateAugmentation2) { Eigen::VectorXd new_landmarks_coordinates(10); Eigen::VectorXd new_landmarks_confidences(5); new_landmarks_coordinates << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; - new_landmarks_confidences << 0.1, 0.2, 0.3, 0.4, 0.5; - ekf_slam_solver->state_augmentation(state, covariance, new_landmarks_coordinates, - new_landmarks_confidences); - std::vector expected_state = {0, 0, M_PI / 2, -2, 1, -4, 3, -6, 5, -8, 7, -10, 9}; + ekf_slam_solver->state_augmentation(state, covariance, new_landmarks_coordinates); + std::vector expected_state = {0, 0, M_PI / 2, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; EXPECT_EQ(state.size(), expected_state.size()); for (int i = 0; i < state.size(); i++) { EXPECT_NEAR(state(i), expected_state[i], 0.0001); diff --git a/src/slam/test/slam_solver/graph_slam_solver_test.cpp b/src/slam/test/slam_solver/graph_slam_solver_test.cpp index f117fa726..c4d6f88da 100644 --- a/src/slam/test/slam_solver/graph_slam_solver_test.cpp +++ b/src/slam/test/slam_solver/graph_slam_solver_test.cpp @@ -9,11 +9,19 @@ class MockDataAssociationModel : public DataAssociationModel { public: MOCK_METHOD(Eigen::VectorXi, associate, - (const Eigen::VectorXd& state, const Eigen::MatrixXd& covariance, - const Eigen::VectorXd& observations, const Eigen::VectorXd& observation_confidences), + (const Eigen::VectorXd& state, const Eigen::VectorXd& observations, + const Eigen::MatrixXd& covariance, const Eigen::VectorXd& observation_confidences), (const, override)); }; +class MockLandmarkFilter : public LandmarkFilter { +public: + MOCK_METHOD(Eigen::VectorXd, filter, + (const Eigen::VectorXd& observations, const Eigen::VectorXd& observation_confidences), + (override)); + MOCK_METHOD(void, delete_landmarks, (const Eigen::VectorXd& some_landmarks), (override)); +}; + class MockV2PModel : public V2PMotionModel { public: MOCK_METHOD(Eigen::Vector3d, get_next_pose, @@ -38,16 +46,17 @@ class GraphSlamSolverTest : public testing::Test { mock_motion_model_ptr = std::make_shared(); mock_data_association_ptr = std::make_shared(); motion_model_ptr = mock_motion_model_ptr; + landmark_filter_ptr = std::make_shared(); data_association_ptr = mock_data_association_ptr; - solver = - std::make_shared(params, data_association_ptr, motion_model_ptr, nullptr); - params.slam_optimization_period_ = 0.0; + solver = std::make_shared(params, data_association_ptr, motion_model_ptr, + landmark_filter_ptr, nullptr); } SLAMParameters params; std::shared_ptr mock_motion_model_ptr; std::shared_ptr mock_data_association_ptr; std::shared_ptr motion_model_ptr; + std::shared_ptr landmark_filter_ptr; std::shared_ptr data_association_ptr; std::shared_ptr solver; };