Pose filtering by jorgenfj · Pull Request #655 · vortexntnu/vortex-auv

jorgenfj · 2026-01-06T16:35:49Z

Pose filtering ros package plus track manager library.
Mainly used for testing and development.
Intended use case is to have the landmark server include the library and do the landmark filtering

codecov · 2026-01-06T18:16:52Z

Codecov Report

❌ Patch coverage is 40.44444% with 268 lines in your changes missing coverage. Please review.
✅ Project coverage is 34.38%. Comparing base (eebcf9b) to head (2e49c7b).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...ring/pose_filtering/src/ros/pose_filtering_ros.cpp	0.00%	108 Missing ⚠️
...ltering/src/ros/pose_filtering_ros_conversions.cpp	0.00%	76 Missing ⚠️
...ose_filtering/src/ros/pose_filtering_ros_debug.cpp	0.00%	40 Missing ⚠️
...tering/pose_filtering/test/test_pose_filtering.cpp	75.32%	2 Missing and 17 partials ⚠️
...ring/pose_filtering/src/lib/pose_track_manager.cpp	89.25%	8 Missing and 5 partials ⚠️
.../include/pose_filtering/lib/pose_track_manager.hpp	73.68%	2 Missing and 3 partials ⚠️
..._filtering/include/pose_filtering/lib/typedefs.hpp	33.33%	3 Missing and 1 partial ⚠️
.../include/pose_filtering/ros/pose_filtering_ros.hpp	0.00%	2 Missing ⚠️
.../pose_action_server/src/pose_action_server_ros.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #655      +/-   ##
==========================================
+ Coverage   33.49%   34.38%   +0.88%     
==========================================
  Files          44       52       +8     
  Lines        3003     3452     +449     
  Branches      780      922     +142     
==========================================
+ Hits         1006     1187     +181     
- Misses       1814     2056     +242     
- Partials      183      209      +26

Flag	Coverage Δ
unittests	`34.38% <40.44%> (+0.88%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
.../pose_action_server/src/pose_action_server_ros.cpp	`0.00% <0.00%> (ø)`
.../include/pose_filtering/ros/pose_filtering_ros.hpp	`0.00% <0.00%> (ø)`
..._filtering/include/pose_filtering/lib/typedefs.hpp	`33.33% <33.33%> (ø)`
.../include/pose_filtering/lib/pose_track_manager.hpp	`73.68% <73.68%> (ø)`
...ring/pose_filtering/src/lib/pose_track_manager.cpp	`89.25% <89.25%> (ø)`
...tering/pose_filtering/test/test_pose_filtering.cpp	`75.32% <75.32%> (ø)`
...ose_filtering/src/ros/pose_filtering_ros_debug.cpp	`0.00% <0.00%> (ø)`
...ltering/src/ros/pose_filtering_ros_conversions.cpp	`0.00% <0.00%> (ø)`
...ring/pose_filtering/src/ros/pose_filtering_ros.cpp	`0.00% <0.00%> (ø)`

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Andeshog

part1

Andeshog · 2026-01-07T07:23:14Z

filtering/pose_filtering/config/pose_filtering.yaml

+    pose_sub_topic: "/aruco_detector/board"
+    pose_array_pub_topic: "/filtered_pose_array"
+    landmark_pub_topic: "/filtered_landmarks"


Any reason these topics are not part of the topic list in orca.yaml, like the rest of the packages in vortex-auv?

This package is not supposed to be used in production. Only for tuning/testing and debugging. The plan is to have the landmark server import the /lib part of this package. So this package will mainly be used to tune for various different obejcts etc.

Andeshog · 2026-01-07T07:33:03Z

filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp

+    /**
+     * @brief Get the list of currently maintained tracks.
+     * @return const reference to internal track vector
+     */
+    const std::vector<Track>& get_tracks() { return tracks_; }


Would also add const at the end to signal that the function does not modify

const std::vector<Track>& get_tracks() const;

Andeshog · 2026-01-08T14:07:32Z

filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp

+    // Internal bookkeeping
+    int track_id_counter_ = 0;


Would use brace init for consistency

Andeshog · 2026-01-08T14:10:46Z

filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp

+    void setup_debug_publishers();
+    void publish_meas_debug();
+    void publish_state_debug();


picky, but nice to group methods/member variables

Andeshog · 2026-01-08T14:19:55Z

filtering/pose_filtering/launch/pose_filtering.launch.py

+                output='screen',
+                parameters=[config, {'use_sim_time': True}],


Why is use_sim_time default to true? Do we even have a \clock topic in sim?

I just used this for testing with a rosbag and then tf2 is very quick to complain about timestamps. I can add a comment explaining its use case

Andeshog · 2026-01-08T14:22:51Z

filtering/pose_filtering/src/lib/orientation_filter.cpp

+
+    Eigen::Matrix<double, 3, Eigen::Dynamic> Z(3, measurements.size());


Z? Perhaps a more descriptive name? 😅

Andeshog · 2026-01-08T14:25:49Z

filtering/pose_filtering/src/lib/orientation_filter.cpp

+    if (q.w() < 0.0) {
+        q.coeffs() *= -1.0;
+    }
+
+    double norm_v = q.vec().norm();
+
+    if (norm_v < 1e-6) {
+        return 2.0 * q.vec();
+    }


for these "magic operations" maybe a comment would be nice

Andeshog · 2026-01-08T14:27:36Z

filtering/pose_filtering/src/lib/pose_track_manager.cpp

+    initial_position_std_ = config.initial_position_std;
+    initial_orientation_std_ = config.initial_orientation_std;
+    ipda_config_ = config.ipda;
+    existence_config_ = config.existence;
+    max_angle_gate_threshold_ = config.max_angle_gate_threshold;


Perhaps have the config as a member instead of the individual fields?

I think it might become more cluttered if i do that as the code is now. The most structured solution would probably be to refactor and separate out a position filter similar to the orientation filter class. I will work on that later if I have time.

I should probably add some dynamic reconfiguration of ros parameters to aid in the tuning process and that might reveal a more intuitive struct setup.

Andeshog · 2026-02-12T20:12:26Z

filtering/pose_filtering/launch/pose_filtering.launch.py

+                    config,
+                    # {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped


Same as the other pr, that this could just be default to false instead of commented out

Andeshog · 2026-02-12T20:18:02Z

filtering/pose_filtering/src/ros/pose_filtering_ros.cpp

+
+    int timer_rate_ms = this->declare_parameter<int>("timer_rate_ms");
+
+    filter_dt_seconds_ = static_cast<double>(timer_rate_ms) / 1000;


Here is a case where auto is nice, since you already declare it as int so its not necessary to type it twice

const auto timer_rate_ms = this->declare_parameter<int>("timer_rate_ms");

Also consider using chrono for time, as it is safer and more expressive when both handing and converting between units (you use chrono in the pub timer anyway)

Andeshog · 2026-02-12T20:18:49Z

filtering/pose_filtering/src/ros/pose_filtering_ros.cpp

+        std::bind(&PoseFilteringNode::timer_callback, this));
+


lambda 🙏

Andeshog · 2026-02-12T20:19:45Z

filtering/pose_filtering/src/ros/pose_filtering_ros.cpp

+    const std::string pose_sub_topic =
+        this->declare_parameter<std::string>("pose_sub_topic");


here too regarding using auto for repetitive type-typing

Andeshog · 2026-02-12T20:21:07Z

filtering/pose_filtering/src/ros/pose_filtering_ros.cpp

+            RCLCPP_WARN(
+                this->get_logger(), "TF transform failed from '%s' to '%s': %s",
+                msg->header.frame_id.c_str(), target_frame_.c_str(), ex.what());


why not spdlog?

Andeshog

lgtm

jorgenfj · 2026-02-14T12:06:42Z

Summary of changes:

Added landmark type and subtype based pre-filtering.
Removed orientation filter in favor of a unified pose filter.
Added custom gating function for the pose filter.

This PR is now blocked by this vortexntnu/vortex-vkf#34

jorgenfj added 20 commits October 19, 2025 21:52

basic package setup

9812d10

removed 'lib' from gitignore

e4e0639

generic pose sub setup

acaba30

removed unused member in favor of variant

72e8211

config setup

8a117c8

track manager impl

5d7b9b7

ValidPoseMsg concept

4ceeeb9

Variant alias

1c9c7f9

refactor eluer to quat

b9f1891

Merge branch 'main' into ipda-pose-filtering

ced39d2

added confirm_tracks function

8cc0e02

landmark support

9be9e2a

added debug pose publisher

7acc72c

renamed pkg: ipda_pose -> pose

5396c1e

tests and doxygen comments

10bf57c

changed alpha calculation. Added README

882f985

pdaf orientation error-state

b73fec7

added landmark array publisher

aeac20c

added landmarkarray pub

65e31a6

added enu-ned rotation

610210a

jorgenfj requested review from Andeshog, Q3rkses and kluge7 January 6, 2026 16:35

jorgenfj self-assigned this Jan 6, 2026

Merge branch 'main' into ipda-pose-filtering

9d65c95

Andeshog reviewed Jan 8, 2026

View reviewed changes

jorgenfj added 2 commits January 9, 2026 11:13

pr fixes

5fee31b

removed unused stuff. Praise clangd

4072a26

Andeshog reviewed Feb 12, 2026

View reviewed changes

Andeshog approved these changes Feb 12, 2026

View reviewed changes

landmark filtering refactor

6877e66

jorgenfj requested a review from Andeshog February 14, 2026 12:08

jorgenfj added 10 commits February 15, 2026 11:10

Merge branch 'main' into ipda-pose-filtering

32915d1

Merge branch 'main' into ipda-pose-filtering

b9afd4c

Merge branch 'main' into ipda-pose-filtering

e39ff3f

update for new landmark_msg type def

bb8ab35

fixed tests, update default noise levels

ce07185

Merge branch 'main' into ipda-pose-filtering

15ef772

Merge branch 'main' into ipda-pose-filtering

9028d66

sleep for stable op mode in sim tests

633dc01

fix bug in get_tracks_by_type

fa728e6

Merge branch 'main' into ipda-pose-filtering

2e49c7b

jorgenfj merged commit 678c9f7 into main Mar 7, 2026
14 checks passed

jorgenfj deleted the ipda-pose-filtering branch March 7, 2026 10:01

		output='screen',
		parameters=[config, {'use_sim_time': True}],


		Eigen::Matrix<double, 3, Eigen::Dynamic> Z(3, measurements.size());

		config,
		# {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped


		int timer_rate_ms = this->declare_parameter<int>("timer_rate_ms");

		filter_dt_seconds_ = static_cast<double>(timer_rate_ms) / 1000;

		const std::string pose_sub_topic =
		this->declare_parameter<std::string>("pose_sub_topic");

Conversation

jorgenfj commented Jan 6, 2026

Uh oh!

codecov bot commented Jan 6, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Andeshog left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andeshog left a comment

Choose a reason for hiding this comment

Uh oh!

jorgenfj commented Feb 14, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

codecov bot commented Jan 6, 2026 •

edited

Loading

jorgenfj commented Feb 14, 2026 •

edited

Loading