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.17476% with 230 lines in your changes missing coverage. Please review.
✅ Project coverage is 36.92%. Comparing base (24b00d6) to head (9d65c95).

Files with missing lines	Patch %	Lines
...ring/pose_filtering/src/ros/pose_filtering_ros.cpp	0.00%	111 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	71.79%	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.92%   +1.34%     
==========================================
  Files          38       47       +9     
  Lines        2254     2665     +411     
  Branches      686      821     +135     
==========================================
+ Hits          802      984     +182     
- Misses       1273     1479     +206     
- Partials      179      202      +23

Flag	Coverage Δ
unittests	`36.92% <44.17%> (+1.34%)`	⬆️

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	`71.79% <71.79%> (ø)`
...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%> (ø)`

🚀 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?

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?

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?

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Pose filtering #655

Pose filtering #655

Uh oh!

jorgenfj commented Jan 6, 2026

Uh oh!

codecov bot commented Jan 6, 2026

Uh oh!

Andeshog left a comment

Uh oh!

Andeshog Jan 7, 2026

Uh oh!

Andeshog Jan 7, 2026

Uh oh!

Andeshog Jan 8, 2026

Uh oh!

Andeshog Jan 8, 2026

Uh oh!

Andeshog Jan 8, 2026

Uh oh!

Andeshog Jan 8, 2026

Uh oh!

Andeshog Jan 8, 2026

Uh oh!

Andeshog Jan 8, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

		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

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!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants