Feat/triangulation by CurryMonsters · Pull Request #14 · vortexntnu/vortex-cv

CurryMonsters · 2026-03-26T17:56:59Z

ROS 2 node that estimates the 3D position of a target using bearing-only triangulation. It collects directional measurements from different viewpoints and solves for the point in space that best fits all rays.

codecov · 2026-03-26T18:23:13Z

Codecov Report

❌ Patch coverage is 0% with 378 lines in your changes missing coverage. Please review.
✅ Project coverage is 11.48%. Comparing base (fca8a61) to head (769d387).
⚠️ Report is 12 commits behind head on main.

Files with missing lines	Patch %	Lines
...localization/src/ros/bearing_localization_node.cpp	0.00%	114 Missing ⚠️
bearing_localization/src/ros/debug_markers.cpp	0.00%	57 Missing ⚠️
bearing_localization/src/lib/geometry_checks.cpp	0.00%	56 Missing ⚠️
...ring_localization/src/lib/triangulation_solver.cpp	0.00%	54 Missing ⚠️
bearing_localization/src/lib/bearing_localizer.cpp	0.00%	33 Missing ⚠️
...earing_localization/src/lib/measurement_buffer.cpp	0.00%	33 Missing ⚠️
...g_localization/lib/bearing_localization_config.hpp	0.00%	25 Missing ⚠️
bearing_localization/src/ros/main.cpp	0.00%	6 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main      #14      +/-   ##
==========================================
- Coverage   16.34%   11.48%   -4.87%     
==========================================
  Files          20       28       +8     
  Lines         893     1271     +378     
  Branches      241      297      +56     
==========================================
  Hits          146      146              
- Misses        661     1039     +378     
  Partials       86       86

Flag	Coverage Δ
unittests	`11.48% <0.00%> (-4.87%)`	⬇️

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

Files with missing lines	Coverage Δ
bearing_localization/src/ros/main.cpp	`0.00% <0.00%> (ø)`
...g_localization/lib/bearing_localization_config.hpp	`0.00% <0.00%> (ø)`
bearing_localization/src/lib/bearing_localizer.cpp	`0.00% <0.00%> (ø)`
...earing_localization/src/lib/measurement_buffer.cpp	`0.00% <0.00%> (ø)`
...ring_localization/src/lib/triangulation_solver.cpp	`0.00% <0.00%> (ø)`
bearing_localization/src/lib/geometry_checks.cpp	`0.00% <0.00%> (ø)`
bearing_localization/src/ros/debug_markers.cpp	`0.00% <0.00%> (ø)`
...localization/src/ros/bearing_localization_node.cpp	`0.00% <0.00%> (ø)`

🚀 New features to boost your workflow:

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

jorgenfj

Try splitting the code into lib/ and ros/ folders. Try to make the ros2 node file as minimalistic as possible.

RayMeasurement should be independent from ros.

Use spdlog instead of rclcpp log

jorgenfj · 2026-03-26T18:42:16Z

bearing_localization/config/bearing_localization_config.yaml

+    target_frame: orca/odom # TF frame to transform measurements into
+    window_size: 30 # Max number of measurements kept in buffer
+    max_measurement_age_sec: 5.0 # Remove measurements older than this (s)
+    min_measurements: 5 # Min measurements needed to triangulate
+    min_baseline_m: 0.5 # Min distance between first and last measurement (m)
+    min_ray_angle_deg: 5.0 # Min angle between rays to accept solution (deg)
+    outlier_residual_threshold_m: 1.0 # Residual above this marks a measurement as outlier (m)
+    max_outlier_iterations: 2 # Max rounds of outlier rejection
+    publish_markers: true # Publish Foxglove markers for debugging
+    landmark_type: 3 # Landmark type ID sent in output message
+    landmark_subtype: 2 # Landmark subtype ID sent in output message
+
+    aruco:
+      target_frame: orca/odom
+      window_size: 15
+      max_measurement_age_sec: 2
+      min_measurements: 4
+      min_baseline_m: 0.1
+      min_ray_angle_deg: 1.0
+      outlier_residual_threshold_m: 0.1
+      max_outlier_iterations: 2
+      publish_markers: true


Could have separate non-ros yaml files for the different configs, similar to how waypoint utils solves it. So that if we ever want to run this as a pure cpp executable we are not dependent on ros for the config values

jorgenfj · 2026-03-26T18:42:38Z

bearing_localization/include/bearing_localization/bearing_localization_node.hpp

@@ -0,0 +1,75 @@
+#pragma once


Use the conventional vortex #ifndef pattern

jorgenfj · 2026-03-26T18:44:56Z

bearing_localization/src/bearing_localization_node.cpp

+void BearingLocalizationNode::declare_parameters() {
+    declare_parameter("profile", "default");
+    declare_parameter("target_frame", "orca/odom");
+    declare_parameter("window_size", 30);
+    declare_parameter("max_measurement_age_sec", 5.0);
+    declare_parameter("min_measurements", 5);
+    declare_parameter("min_baseline_m", 0.5);
+    declare_parameter("min_ray_angle_deg", 5.0);
+    declare_parameter("outlier_residual_threshold_m", 1.0);
+    declare_parameter("max_outlier_iterations", 2);
+    declare_parameter("publish_markers", true);
+    declare_parameter("landmark_type", 0);
+    declare_parameter("landmark_subtype", 0);
+    declare_parameter("landmark_id", 0);
+
+    declare_parameter("topics.bearing_measurement", "bearing_measurement");
+    declare_parameter("topics.bearing_array",
+                      "/aruco_detector/marker_directions");
+    declare_parameter("topics.landmarks", "landmarks");
+    declare_parameter("topics.bearing_localization_markers",
+                      "bearing_localization/markers");
+}


Should avoid default values for ros parameters. Default values here can create hidden bugs where the config value is not applied if there is a type somewhere.

jorgenfj · 2026-03-26T18:47:59Z

bearing_localization/src/bearing_localization_node.cpp

+void BearingLocalizationNode::publish_result(const Eigen::Vector3d& position,
+                                             double residual) {
+    auto stamp = now();
+
+    vortex_msgs::msg::Landmark landmark;
+    landmark.header.stamp = stamp;
+    landmark.header.frame_id = target_frame_;
+    landmark.id = 0;
+    landmark.type.value = static_cast<uint16_t>(landmark_type_);
+    landmark.subtype.value = static_cast<uint16_t>(landmark_subtype_);
+    landmark.pose.pose.position.x = position.x();
+    landmark.pose.pose.position.y = position.y();
+    landmark.pose.pose.position.z = position.z();
+    landmark.pose.pose.orientation.w = 1.0;
+    const double var = residual * residual;
+    landmark.pose.covariance[0] = var;
+    landmark.pose.covariance[7] = var;
+    landmark.pose.covariance[14] = var;
+
+    vortex_msgs::msg::LandmarkArray landmark_array;
+    landmark_array.header.stamp = stamp;
+    landmark_array.header.frame_id = target_frame_;
+    landmark_array.landmarks.push_back(landmark);
+    landmark_pub_->publish(landmark_array);
+}


This could be moved into a separate utils file to avoid cluttering the main node file

jorgenfj · 2026-03-26T18:48:59Z

bearing_localization/config/bearing_localization_config.yaml

+    min_ray_angle_deg: 5.0 # Min angle between rays to accept solution (deg)
+    outlier_residual_threshold_m: 1.0 # Residual above this marks a measurement as outlier (m)
+    max_outlier_iterations: 2 # Max rounds of outlier rejection
+    publish_markers: true # Publish Foxglove markers for debugging


maybe rename to publish_debug_markers to be more explicit that this is a debug topic

jorgenfj · 2026-03-26T18:49:39Z

bearing_localization/src/bearing_localization_node.cpp

+
+void BearingLocalizationNode::setup_pubsub() {
+    bearing_sub_ = create_subscription<geometry_msgs::msg::Vector3Stamped>(
+        get_parameter("topics.bearing_measurement").as_string(), 10,


qos here. Use the helper qos function from vortex utils

bearing_localization/src/bearing_localization_node.cpp

jorgenfj

Add doxygen to headers

bearing_localization/config/aruco.yaml

bearing_localization/include/bearing_localization/lib/bearing_localization_config.hpp

bearing_localization/include/bearing_localization/lib/ray_measurement.hpp

bearing_localization/include/bearing_localization/lib/measurement_buffer.hpp

bearing_localization/src/ros/bearing_localization_node.cpp

jorgenfj

lgtm

CurryMonsters added 7 commits March 25, 2026 10:55

changed from LDLT to SVD decomposition

c85284e

using drone configs

9c82e1c

removed config declaration

a98d638

changed some params

3473e91

added comments for config file

ea088a2

changed from rmw_qos to vortex-qos

a6f13cb

added readme and made topics visible in config

104c5cf

CurryMonsters requested a review from jorgenfj March 26, 2026 17:56

CurryMonsters and others added 2 commits March 26, 2026 19:07

fixed issue with ci failing

6e49559

Merge branch 'main' into feat/triangulation

524accb

jorgenfj requested changes Mar 26, 2026

View reviewed changes

fixed pr(refactored into lib and ros)

11c8b01

jorgenfj requested changes Mar 27, 2026

View reviewed changes

fixed pr comments(docs by copilot :))

769d387

jorgenfj approved these changes Mar 28, 2026

View reviewed changes

CurryMonsters merged commit f860da4 into main Mar 28, 2026
5 checks passed

Conversation

CurryMonsters commented Mar 26, 2026

Uh oh!

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

Uh oh!

Codecov Report

Uh oh!

jorgenfj left a comment

Choose a reason for hiding this comment

Uh oh!

jorgenfj Mar 26, 2026

Choose a reason for hiding this comment

Uh oh!

jorgenfj Mar 26, 2026

Choose a reason for hiding this comment

Uh oh!

jorgenfj Mar 26, 2026

Choose a reason for hiding this comment

Uh oh!

jorgenfj Mar 26, 2026

Choose a reason for hiding this comment

Uh oh!

jorgenfj Mar 26, 2026

Choose a reason for hiding this comment

Uh oh!

jorgenfj Mar 26, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

jorgenfj left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

jorgenfj left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

codecov bot commented Mar 26, 2026 •

edited

Loading