FSF-8582 Perception filter #419

lourenco31 · 2025-04-20T19:55:10Z

No description provided.

…rameters come from config files

…ed comments

notion-workspace · 2025-04-20T19:55:15Z

Copilot

Pull Request Overview

This PR integrates a new LandmarkFilter into the SLAM system to enhance perception filtering, particularly using a consecutive count filter for outlier rejection.

Adds LandmarkFilter as a dependency in SLAM solvers and adjusts their constructors and method signatures.
Updates state augmentation logic in EKFSLAMSolver to remove reliance on landmark confidences.
Introduces configuration parameters and new tests for the consecutive count filter.

Reviewed Changes

Copilot reviewed 20 out of 21 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
src/slam/test/slam_solver/ekf_slam_solver_test.cpp	Updated unit tests to match the redesigned state augmentation interface.
src/slam/src/slam_solver/slam_solver.cpp	Added LandmarkFilter parameter to SLAMSolver constructors.
src/slam/src/slam_solver/graph_slam_solver.cpp	Updated constructor calls to include LandmarkFilter.
src/slam/src/slam_solver/ekf_slam_solver.cpp	Revised state augmentation logic to remove confidences and apply noise from config.
src/slam/include/slam_solver/ekf_slam_solver.hpp	Adjusted the API and documentation of state_augmentation accordingly.
src/slam/include/slam_solver/ map.hpp & related headers	Updated the SLAM solvers’ factory map to accept LandmarkFilter dependencies.
src/slam/include/slam_config/general_config.{cpp,hpp} & YAML files	Added new configuration parameters for landmark filtering.
src/perception_sensor_lib/*	Added new consecutive count filter implementation and tests for perception landmark filtering.

Files not reviewed (1)

src/perception_sensor_lib/CMakeLists.txt: Language not supported

src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp

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

marhcouto

Small changes requested, but a bit of confusion, need to discuss personally

…s rather than the first

Copilot

Pull Request Overview

This PR refactors the perception data association and landmark filtering logic by replacing the combined state vector with a dedicated landmarks vector and updating related method signatures, test cases, and configuration files. Key changes include:

Converting test cases and API calls from a state vector (including vehicle pose and landmarks) to a separated landmarks vector.
Adjusting internal data association computations (e.g. index handling and covariance block accesses) to work with the new landmarks representation.
Updating configuration YAML files to include landmark filter parameters.

Reviewed Changes

Copilot reviewed 33 out of 34 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
src/perception_sensor_lib/test/data_association/nearest_neighbor_test.cpp	Updated test cases to use landmarks instead of state and adjusted expected association indices.
src/perception_sensor_lib/test/data_association/maximum_likelihood_nll_test.cpp	Modified test inputs and expected outputs for maximum likelihood NLL association using landmarks.
src/perception_sensor_lib/test/data_association/maximum_likelihood_md_test.cpp	Similar updates to reflect landmarks API and expected association indices.
src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp	Updated filter logic; minor note on iterative conservative resizing.
src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp	Refactored to use a landmarks vector and adjusted indexing in the association loop.
src/perception_sensor_lib/src/data_association/nearest_neighbor.cpp	Similar changes made to use landmarks and update associated computations.
src/perception_sensor_lib/src/data_association/maximum_likelihood_nll.cpp	Modified association computation to access covariance blocks corresponding to landmarks.
src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp	Adjusted association logic for the landmarks vector and related covariance indexing.
config/slam/vehicle.yaml & config/slam/pacsim.yaml	Added landmark filter configuration parameters and updated model names.

Files not reviewed (1)

src/perception_sensor_lib/CMakeLists.txt: Language not supported

Comments suppressed due to low confidence (2)

src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp:1

The project defines both 'NearestNeighbour' and 'NearestNeighbor' classes across files. It's recommended to standardize the naming convention (e.g., use either British or American spelling consistently) to avoid confusion.

#include "perception_sensor_lib/data_association/nearest_neighbour.hpp"

src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp:23

[nitpick] Consider renaming the variable 'min_index' to a more descriptive name like 'associated_landmark_index' to clarify that it represents the index in the landmarks vector (scaled by 2) and improve code readability.

min_index = 2 * j;

src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp

…sociation

marhcouto

Este problema de aparecer tudo no PR acontecer sempre que damos merge? Secalhar deviamos começar a fazer rebase, que dor

.vscode/settings.json

config/planning/pacsim.yaml

config/velocity_estimation/pacsim.yaml

launch/slam.launch.py

src/evaluator/evaluator/evaluator.py

src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp

src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp

Davide64-dev · 2025-05-13T00:12:27Z

@lourenco31 elf_state_est n funcional

Copilot

Pull Request Overview

This PR refactors data association to operate directly on landmark vectors, introduces a new consecutive-observation filter, and updates tests and configuration to integrate the new filter.

Refactor all associate() APIs and tests to use a flat landmarks vector instead of the full state
Implement ConsecutiveCounterFilter with its parameters and hook it into the build and configs
Update SLAM configuration to expose minimum-observation and detection-frequency settings

Reviewed Changes

Copilot reviewed 37 out of 37 changed files in this pull request and generated 4 comments.

Show a summary per file

File	Description
src/perception_sensor_lib/test/data_association/nearest_neighbor_test.cpp	Updated to pass landmarks vector to `associate()`
src/perception_sensor_lib/test/data_association/maximum_likelihood_nll_test.cpp	Same update for NLL tests
src/perception_sensor_lib/test/data_association/maximum_likelihood_md_test.cpp	Same update for MD tests
src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp	Added new filter implementation
src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp	Declared `ConsecutiveCounterFilter` interface
src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/parameters.hpp	Introduced landmark-filter parameters
src/perception_sensor_lib/src/data_association/nearest_neighbour.cpp	Refactored UK-spelled nearest-neighbour to use landmarks
src/perception_sensor_lib/src/data_association/nearest_neighbor.cpp	Refactored US-spelled nearest-neighbor to use landmarks
src/perception_sensor_lib/src/data_association/maximum_likelihood_nll.cpp	Refactored and simplified NLL implementation
src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp	Refactored and simplified MD implementation
src/perception_sensor_lib/include/perception_sensor_lib/data_association/*.hpp	Updated all `associate()` signatures in headers
src/perception_sensor_lib/CMakeLists.txt	Registered the new filter and its tests
config/slam/vehicle.yaml / pacsim.yaml	Added `landmark_filter_name`, `minimum_observation_count`, and `minimum_frequency_of_detections`

Comments suppressed due to low confidence (2)

src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp:44

This assignment always overwrites the newly added observation with the map position, preventing new observations from ever being returned. Consider moving this assignment into an else branch or removing it if unintended.

filtered_observations.segment(filtered_observations.size() - 2, 2) = this->map.segment(i * 2, 2);

src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp:1

The delete_landmarks method isn’t currently exercised by existing tests—consider adding unit tests to validate landmark removal behavior.

#include "perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp"

Copilot · 2025-05-16T00:06:52Z

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 <iostream>


The <iostream> include is unused—removing it will reduce compile dependencies and improve clarity.

Suggested change

#include <iostream>

Copilot · 2025-05-16T00:06:53Z

...tion_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp

+#define EQUALITY_TOLERANCE 1e-3
+
+class ConsecutiveCounterFilter : public LandmarkFilter {
+private:


[nitpick] Using a macro for a tolerance value can lead to unexpected name collisions; consider using a constexpr double kEqualityTolerance = 1e-3; instead.

Suggested change

#define EQUALITY_TOLERANCE 1e-3

class ConsecutiveCounterFilter : public LandmarkFilter {

private:

class ConsecutiveCounterFilter : public LandmarkFilter {

private:

static constexpr double kEqualityTolerance = 1e-3;

Copilot · 2025-05-16T00:06:53Z

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

+  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) {}


[nitpick] minimum_frequency_of_detections_ is defined but not used by the filter logic. If it's intended, add usage or remove the field to avoid confusion.

Suggested change

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) {}

LandmarkFilterParameters() = default;

LandmarkFilterParameters(const int minimum_observation_count)

: minimum_observation_count_(minimum_observation_count) {}

Copilot · 2025-05-16T00:06:53Z

src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp

@@ -4,51 +4,35 @@
 #include <limits>
 #include <vector>

+#define INFINITESIMAL 1e-9


[nitpick] Consider replacing this macro with a static constexpr double kInfinitesimal = 1e-9; to respect namespace and avoid pollution.

sonarqubecloud · 2025-05-16T00:30:09Z

Quality Gate passed

Issues
0 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
18.3% Duplication on New Code

See analysis details on SonarQube Cloud

marhcouto

Just left a small comment. Make sure you tested this in Simulator.

marhcouto · 2025-05-16T10:11:52Z

src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp

+    return;
+  }
+
+  this->_landmark_filter_->delete_landmarks(filtered_new_observations);


One small comment: I though we were just going to let the landmarks go away when they were not observed. There is something that tells me that this is not the best thing ever (because intrinsically GraphSLAMInstance does not ensure you that all of those landmarks were added, so in terms of good practices I think it is a bad one, one waiting for someone to create some different behaviour inside GraphSLAMInstance and make it not add all new landmarks and capuff. But I think we can improve this later. I would however add a comment note to actually do this later

lourenco31 added 3 commits April 20, 2025 18:24

feat: created filter for perception data

21ef3d1

feat: slam node and slam solver have access to landmark filter and pa…

aca1ab7

…rameters come from config files

EKF using landmark filter, tests follow new constructor format, updat…

805276e

…ed comments

lourenco31 requested review from marhcouto, Davide64-dev and Copilot April 20, 2025 19:55

lourenco31 self-assigned this Apr 20, 2025

Copilot AI reviewed Apr 20, 2025

View reviewed changes

marhcouto requested changes Apr 23, 2025

View reviewed changes

lourenco31 added 6 commits April 24, 2025 12:49

fix: changed data association interface

37cce64

fix: data association tests

a332674

fix: adapt landmark filter to the new data association

aaa8644

feat: landmark filter returns the last coordinates of the observation…

7cd6b00

…s rather than the first

fix: adapted ekf slam and graph slam to new data association

7dace8b

fix: now actually adapted EKF slam to new data association

2930995

lourenco31 requested a review from Copilot April 24, 2025 17:45

Copilot AI reviewed Apr 24, 2025

View reviewed changes

Davide64-dev requested changes Apr 25, 2025

View reviewed changes

src/perception_sensor_lib/src/data_association/maximum_likelihood_md.cpp Outdated Show resolved Hide resolved

lourenco31 requested a review from marhcouto April 29, 2025 19:14

lourenco31 added 3 commits May 12, 2025 09:52

merge dev into perception-filter

e719df7

fix: using perception filter in Graph SLAM and added Macro to data as…

7f61c65

…sociation

fix: adapted EKF slam to new data association interface

b986ec9

lourenco31 requested a review from Davide64-dev May 12, 2025 15:13

marhcouto requested changes May 12, 2025

View reviewed changes

lourenco31 and others added 3 commits May 14, 2025 21:19

trabalho a meio porque fui fazer ve+slam

b7741a1

fix: new perception filter and integrated with EKF and Graph SLAM

805544f

Update slam_node.cpp

fbfb793

lourenco31 requested review from marhcouto and Copilot May 16, 2025 00:04

Copilot AI reviewed May 16, 2025

View reviewed changes

marhcouto approved these changes May 16, 2025

View reviewed changes

		@@ -0,0 +1,87 @@
		#include "perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp"

		#include <iostream>

FSF-8582 Perception filter #419

Are you sure you want to change the base?

FSF-8582 Perception filter #419

Uh oh!

Conversation

lourenco31 commented Apr 20, 2025

Uh oh!

notion-workspace bot commented Apr 20, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

marhcouto left a comment

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Uh oh!

marhcouto left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Davide64-dev commented May 13, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Copilot AI May 16, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI May 16, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI May 16, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI May 16, 2025

Choose a reason for hiding this comment

Uh oh!

sonarqubecloud bot commented May 16, 2025

Quality Gate passed

Uh oh!

marhcouto left a comment

Choose a reason for hiding this comment

Uh oh!

marhcouto May 16, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!