Pose filtering #655

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 44.41748% with 229 lines in your changes missing coverage. Please review.
✅ Project coverage is 36.99%. Comparing base (24b00d6) to head (4072a26).

Files with missing lines	Patch %	Lines
...ring/pose_filtering/src/ros/pose_filtering_ros.cpp	0.00%	110 Missing ⚠️
...ltering/src/ros/pose_filtering_ros_conversions.cpp	0.00%	40 Missing ⚠️
...ose_filtering/src/ros/pose_filtering_ros_debug.cpp	0.00%	40 Missing ⚠️
...tering/pose_filtering/test/test_pose_filtering.cpp	78.16%	2 Missing and 17 partials ⚠️
...ring/pose_filtering/src/lib/orientation_filter.cpp	72.50%	8 Missing and 3 partials ⚠️
...ring/pose_filtering/src/lib/pose_track_manager.cpp	95.40%	1 Missing and 3 partials ⚠️
..._filtering/include/pose_filtering/lib/typedefs.hpp	40.00%	3 Missing ⚠️
.../pose_action_server/src/pose_action_server_ros.cpp	0.00%	1 Missing ⚠️
.../include/pose_filtering/ros/pose_filtering_ros.hpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #655      +/-   ##
==========================================
+ Coverage   35.58%   36.99%   +1.41%     
==========================================
  Files          38       47       +9     
  Lines        2254     2665     +411     
  Branches      686      822     +136     
==========================================
+ Hits          802      986     +184     
- Misses       1273     1477     +204     
- Partials      179      202      +23

Flag	Coverage Δ
unittests	`36.99% <44.41%> (+1.41%)`	⬆️

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

Files with missing lines	Coverage Δ
.../include/pose_filtering/lib/pose_track_manager.hpp	`100.00% <100.00%> (ø)`
.../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	`40.00% <40.00%> (ø)`
...ring/pose_filtering/src/lib/pose_track_manager.cpp	`95.40% <95.40%> (ø)`
...ring/pose_filtering/src/lib/orientation_filter.cpp	`72.50% <72.50%> (ø)`
...tering/pose_filtering/test/test_pose_filtering.cpp	`78.16% <78.16%> (ø)`
...ltering/src/ros/pose_filtering_ros_conversions.cpp	`0.00% <0.00%> (ø)`
...ose_filtering/src/ros/pose_filtering_ros_debug.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.

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

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


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

Pose filtering #655

Are you sure you want to change the base?

Pose filtering #655

Uh oh!

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!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

codecov bot commented Jan 6, 2026 •

edited

Loading