Feat/triangulation

bearing_localization/CMakeLists.txt

@@ -0,0 +1,108 @@
+cmake_minimum_required(VERSION 3.8)
+project(bearing_localization)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(spdlog REQUIRED)
+
+include_directories(include)
+
+# --- ROS-independent library ---
+add_library(bearing_localization_core SHARED
+  src/lib/measurement_buffer.cpp
+  src/lib/triangulation_solver.cpp
+  src/lib/geometry_checks.cpp
+  src/lib/bearing_localizer.cpp
+)
+
+target_include_directories(bearing_localization_core
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+target_link_libraries(bearing_localization_core
+  yaml-cpp
+  spdlog::spdlog
+)
+
+# --- ROS node executable ---
+add_executable(bearing_localization_node
+  src/ros/main.cpp
+  src/ros/bearing_localization_node.cpp
+  src/ros/debug_markers.cpp
+)
+
+target_include_directories(bearing_localization_node
+  PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+ament_target_dependencies(bearing_localization_node
+  rclcpp
+  geometry_msgs
+  std_msgs
+  visualization_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  vortex_msgs
+  Eigen3
+  vortex_utils_ros
+)
+
+target_link_libraries(bearing_localization_node
+  bearing_localization_core
+)
+
+install(
+  TARGETS
+    bearing_localization_core
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  TARGETS
+    bearing_localization_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_include_directories(include)
+ament_export_libraries(bearing_localization_core)
+
+ament_package()

bearing_localization/README.md

@@ -0,0 +1,26 @@
+# Bearing Localization
+
+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.
+
+
+## Topics
+
+| Direction | Parameter | Default | Type |
+|---|---|---|---|
+| Sub | `topics.bearing_measurement` | `bearing_measurement` | `geometry_msgs/Vector3Stamped` |
+| Sub | `topics.bearing_array` | `/aruco_detector/marker_directions` | `vortex_msgs/Vector3Array` |
+| Pub | `topics.landmarks` | `landmarks` | `vortex_msgs/LandmarkArray` |
+| Pub | `topics.bearing_localization_markers` | `bearing_localization/markers` | `visualization_msgs/MarkerArray` |
+
+## Key Parameters
+
+| Parameter | Default | Description |
+|---|---|---|
+| `profile` | `default` | Parameter profile (`default`, `aruco`) |
+| `target_frame` | `orca/odom` | Reference frame for output |
+| `window_size` | 30 | Max buffered measurements |
+| `max_measurement_age_sec` | 5.0 | Measurement expiry (seconds) |
+| `min_measurements` | 5 | Min rays to attempt solve |
+| `min_baseline_m` | 0.5 | Min distance between viewpoints |
+| `min_ray_angle_deg` | 5.0 | Min angular spread between rays |
+| `outlier_residual_threshold_m` | 1.0 | Outlier rejection threshold |

bearing_localization/config/aruco.yaml

@@ -0,0 +1,7 @@
+window_size: 15
+max_measurement_age_sec: 2.0
+min_measurements: 4
+min_baseline_m: 0.1
+min_ray_angle_deg: 1.0
+outlier_residual_threshold_m: 0.1
+max_outlier_iterations: 2

bearing_localization/config/bearing_localization_config.yaml

@@ -0,0 +1,10 @@
+/**:
+  ros__parameters:
+    target_frame: orca/odom # TF frame to transform measurements into
+    publish_debug_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
+    topics:
+      bearing_measurement: bearing_measurement # Single bearing input
+      bearing_array: /aruco_detector/marker_directions # Array bearing input
+      bearing_localization_markers: bearing_localization/markers # Debug markers

bearing_localization/config/default.yaml

@@ -0,0 +1,7 @@
+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

bearing_localization/include/bearing_localization/lib/bearing_localization_config.hpp

@@ -0,0 +1,71 @@
+#ifndef BEARING_LOCALIZATION__LIB__BEARING_LOCALIZATION_CONFIG_HPP_
+#define BEARING_LOCALIZATION__LIB__BEARING_LOCALIZATION_CONFIG_HPP_
+
+#include <yaml-cpp/yaml.h>
+
+#include <stdexcept>
+#include <string>
+
+namespace bearing_localization {
+
+/**
+ * @brief Configuration parameters for the bearing localization triangulator.
+ *
+ * Holds algorithm settings that control the measurement buffer,
+ * geometry checks, outlier rejection, and output landmark.
+ * Loaded from a YAML profile file via from_yaml().
+ */
+struct BearingLocalizationConfig {
+    int window_size = 30;  // Max measurements kept in the buffer.
+    double max_measurement_age_sec =
+        5.0;                   // Discard measurements older than this (s).
+    int min_measurements = 5;  // Min measurements required to triangulate.
+    double min_baseline_m =
+        0.5;  // Min distance between first and last origin (m).
+    double min_ray_angle_deg =
+        5.0;  // Min angle between rays to accept a solution (deg).
+    double outlier_residual_threshold_m =
+        1.0;  // Residual above this marks a measurement as outlier (m).
+    int max_outlier_iterations =
+        2;  // Max rounds of iterative outlier rejection.
+
+    /**
+     * @brief Load configuration from a YAML file.
+     * @param path Filesystem path to the YAML profile.
+     * @return Populated BearingLocalizationConfig.
+     * @throws std::runtime_error if the file cannot be loaded.
+     */
+    static BearingLocalizationConfig from_yaml(const std::string& path) {
+        YAML::Node yaml;
+        try {
+            yaml = YAML::LoadFile(path);
+        } catch (const YAML::Exception& e) {
+            throw std::runtime_error("Failed to load config from '" + path +
+                                     "': " + e.what());
+        }
+
+        BearingLocalizationConfig cfg;
+        if (yaml["window_size"])
+            cfg.window_size = yaml["window_size"].as<int>();
+        if (yaml["max_measurement_age_sec"])
+            cfg.max_measurement_age_sec =
+                yaml["max_measurement_age_sec"].as<double>();
+        if (yaml["min_measurements"])
+            cfg.min_measurements = yaml["min_measurements"].as<int>();
+        if (yaml["min_baseline_m"])
+            cfg.min_baseline_m = yaml["min_baseline_m"].as<double>();
+        if (yaml["min_ray_angle_deg"])
+            cfg.min_ray_angle_deg = yaml["min_ray_angle_deg"].as<double>();
+        if (yaml["outlier_residual_threshold_m"])
+            cfg.outlier_residual_threshold_m =
+                yaml["outlier_residual_threshold_m"].as<double>();
+        if (yaml["max_outlier_iterations"])
+            cfg.max_outlier_iterations =
+                yaml["max_outlier_iterations"].as<int>();
+        return cfg;
+    }
+};
+
+}  // namespace bearing_localization
+
+#endif  // BEARING_LOCALIZATION__LIB__BEARING_LOCALIZATION_CONFIG_HPP_

bearing_localization/include/bearing_localization/lib/bearing_localizer.hpp

@@ -0,0 +1,63 @@
+#ifndef BEARING_LOCALIZATION__LIB__BEARING_LOCALIZER_HPP_
+#define BEARING_LOCALIZATION__LIB__BEARING_LOCALIZER_HPP_
+
+#include "bearing_localization/lib/bearing_localization_config.hpp"
+#include "bearing_localization/lib/geometry_checks.hpp"
+#include "bearing_localization/lib/measurement_buffer.hpp"
+#include "bearing_localization/lib/ray_measurement.hpp"
+#include "bearing_localization/lib/triangulation_solver.hpp"
+
+#include <Eigen/Dense>
+#include <optional>
+#include <vector>
+
+namespace bearing_localization {
+
+/**
+ * @brief Output of a successful bearing-only localisation solve.
+ */
+struct LocalizationResult {
+    Eigen::Vector3d position{
+        Eigen::Vector3d::Zero()};  // Estimated 3-D target position.
+    double mean_residual{0.0};     // Mean point-to-ray residual (m).
+    std::vector<RayMeasurement>
+        inlier_rays;  // Rays retained after outlier rejection.
+};
+
+/**
+ * @brief High-level bearing-only localiser.
+ *
+ * Buffers incoming ray measurements, runs geometry checks, and invokes the
+ * triangulation solver with iterative outlier rejection to estimate the
+ * 3-D position of a target observed from multiple viewpoints.
+ */
+class BearingLocalizer {
+   public:
+    explicit BearingLocalizer(const BearingLocalizationConfig& cfg);
+
+    /**
+     * @brief Add a new ray measurement to the internal buffer.
+     * @param ray World-frame ray measurement.
+     */
+    void add_measurement(const RayMeasurement& ray);
+
+    /**
+     * @brief Attempt to triangulate the target position.
+     *
+     * Prunes stale measurements, runs geometry checks, solves via
+     * least-squares with outlier rejection, and returns the result.
+     *
+     * @param now_sec Current time in seconds (used for pruning).
+     * @return LocalizationResult on success, std::nullopt if checks fail.
+     */
+    std::optional<LocalizationResult> solve(double now_sec);
+
+   private:
+    BearingLocalizationConfig cfg_;
+    MeasurementBuffer buffer_;
+    TriangulationSolver solver_;
+};
+
+}  // namespace bearing_localization
+
+#endif  // BEARING_LOCALIZATION__LIB__BEARING_LOCALIZER_HPP_

bearing_localization/include/bearing_localization/lib/geometry_checks.hpp

@@ -0,0 +1,90 @@
+#ifndef BEARING_LOCALIZATION__LIB__GEOMETRY_CHECKS_HPP_
+#define BEARING_LOCALIZATION__LIB__GEOMETRY_CHECKS_HPP_
+
+#include "bearing_localization/lib/ray_measurement.hpp"
+
+#include <Eigen/Dense>
+#include <string>
+#include <vector>
+
+namespace bearing_localization {
+
+/**
+ * @brief Static utility class for geometric validation of ray sets.
+ *
+ * Provides pre-solve and post-solve sanity checks (minimum ray count,
+ * baseline distance, angular spread, positive depth) to guard the
+ * triangulation solver against degenerate configurations.
+ */
+class GeometryChecks {
+   public:
+    /**
+     * @brief Result of a single geometry check.
+     */
+    struct CheckResult {
+        bool passed{false};  // True if the check succeeded.
+        std::string reason;  // failure reason.
+    };
+
+    /**
+     * @brief Verify that enough rays are available.
+     * @param rays   Ray measurements to check.
+     * @param min_rays  Minimum number required.
+     */
+    static CheckResult check_minimum_rays(
+        const std::vector<RayMeasurement>& rays,
+        size_t min_rays);
+
+    /**
+     * @brief Verify that the spatial baseline between the first and last
+     *        ray origins meets the minimum distance.
+     * @param rays  Ray measurements to check.
+     * @param min_baseline_m  Minimum baseline distance (m).
+     */
+    static CheckResult check_minimum_baseline(
+        const std::vector<RayMeasurement>& rays,
+        double min_baseline_m);
+
+    /**
+     * @brief Verify that the maximum angle between any two rays exceeds
+     *        the minimum threshold.
+     * @param rays  Ray measurements to check.
+     * @param min_angle_deg  Minimum angular spread (degrees).
+     */
+    static CheckResult check_angular_spread(
+        const std::vector<RayMeasurement>& rays,
+        double min_angle_deg);
+
+    /**
+     * @brief Verify that a sufficient fraction of rays have the estimated
+     *        target in front of them (positive depth).
+     * @param rays  Ray measurements to check.
+     * @param point  Estimated target position.
+     * @param min_fraction  Fraction of rays that must pass (default 0.6).
+     */
+    static CheckResult check_positive_depth(
+        const std::vector<RayMeasurement>& rays,
+        const Eigen::Vector3d& point,
+        double min_fraction = 0.6);
+
+    /**
+     * @brief Run all pre-solve checks (ray count, baseline, angular spread).
+     * @return Combined CheckResult; fails on the first violated condition.
+     */
+    static CheckResult check_pre_solve(const std::vector<RayMeasurement>& rays,
+                                       size_t min_rays,
+                                       double min_baseline_m,
+                                       double min_angle_deg);
+
+    /**
+     * @brief Run all post-solve checks (positive depth).
+     * @param rays  Inlier rays used in the solution.
+     * @param point  Estimated target position.
+     */
+    static CheckResult check_post_solve(const std::vector<RayMeasurement>& rays,
+                                        const Eigen::Vector3d& point);
+};
+
+}  // namespace bearing_localization
+
+#endif  // BEARING_LOCALIZATION__LIB__GEOMETRY_CHECKS_HPP_