Tests for magnetic compensation (ellipsoid)

src/subjugator/drivers/magnetic_compensation/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -25,6 +26,10 @@ install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
 # Install project configuration files
 install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config)
 
+# Install the pure-Python calibration module (importable as
+# magnetic_compensation.ellipsoid by the config generator and the tests)
+ament_python_install_package(${PROJECT_NAME})
+
 # create the component library
 add_library(
   ${PROJECT_NAME} SHARED src/component.cpp # adjust if your source file is named
@@ -59,20 +64,22 @@ ament_export_dependencies(rclcpp rclcpp_components sensor_msgs tf2 tf2_eigen
                           Eigen3)
 
 if(BUILD_TESTING)
+  # C++ unit tests: correction math + parameter validation (compensator.hpp)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_compensator test/test_compensator.cpp)
+  target_include_directories(test_compensator
+                             PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+  target_link_libraries(test_compensator Eigen3::Eigen)
+
+  # Pure-math Python tests for the ellipsoid fit (numpy/scipy only)
   find_package(ament_cmake_pytest REQUIRED)
-  set(_pytest_tests) # test/test_basic_shift.py)
-  foreach(_test_path ${_pytest_tests})
-    get_filename_component(_test_name ${_test_path} NAME_WE)
-    ament_add_pytest_test(
-      ${_test_name}
-      ${_test_path}
-      APPEND_ENV
-      PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
-      TIMEOUT
-      60
-      WORKING_DIRECTORY
-      ${CMAKE_SOURCE_DIR})
-  endforeach()
+  ament_add_pytest_test(test_ellipsoid test/test_ellipsoid.py TIMEOUT 60
+                        WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+
+  # End-to-end launch tests: live correction path + constructor param validation
+  find_package(launch_testing_ament_cmake REQUIRED)
+  add_launch_test(test/test_basic_shift.py TIMEOUT 60)
+  add_launch_test(test/test_param_validation.py TIMEOUT 60)
 endif()
 
 ament_package()

src/subjugator/drivers/magnetic_compensation/include/magnetic_compensation/compensator.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace mil::magnetic_compensation
+{
+
+// Apply the hard/soft-iron correction: undo the offset, then the distortion.
+inline Eigen::Vector3d compensate(Eigen::Matrix3d const &scale_inverse, Eigen::Vector3d const &shift,
+                                  Eigen::Vector3d const &raw)
+{
+    return scale_inverse * (raw - shift);
+}
+
+}  // namespace mil::magnetic_compensation

src/subjugator/drivers/magnetic_compensation/magnetic_compensation/ellipsoid.py

@@ -0,0 +1,93 @@
+"""Pure-math hard/soft-iron ellipsoid fitting for magnetometer calibration.
+
+A clean magnetometer, swept through all orientations, traces a sphere centered
+at the origin. The sub's hard-iron (a constant offset) and soft-iron (a linear
+distortion) warp that sphere into an off-center, tilted ellipsoid.
+
+``fit_ellipsoid`` recovers the offset (``shift``) and the distortion
+(``scale``) from points on that ellipsoid so the live node can undo them with
+``scale^-1 @ (raw - shift)``.
+
+This module has no ROS or plotting dependencies so the math can be unit tested
+in isolation (numpy + scipy only).
+"""
+
+import numpy as np
+import scipy.linalg
+
+
+def normalized_matrix(m):
+    assert np.linalg.det(m) > 0
+    return m / np.linalg.det(m) ** (1 / m.shape[0])
+
+
+def calculate_error(points):
+    radii = list(map(np.linalg.norm, points))
+    error = np.std(radii) / np.mean(radii)
+    return error
+
+
+def fit_ellipsoid(points):
+    points = np.array(points)
+
+    # The fit solves for 9 coefficients; fewer points leaves it underdetermined
+    # and lstsq returns a confident-looking but meaningless calibration. Reject
+    # that loudly rather than silently emitting a bogus scale/shift.
+    if points.shape[0] < 9:
+        raise ValueError(
+            f"fit_ellipsoid needs at least 9 points, got {points.shape[0]}",
+        )
+
+    # Reject (near-)coplanar/collinear data. If the points don't span all three
+    # axes the ellipsoid is underdetermined along an axis, and whether the fit
+    # then errors out is numerically fragile (it depends on the BLAS backend).
+    # The singular values of the mean-centered points measure spread per axis; a
+    # vanishing smallest one means the calibration sweep was degenerate (e.g. the
+    # vehicle only rotated about one axis), so fail loudly here instead.
+    singular_values = np.linalg.svd(points - points.mean(axis=0), compute_uv=False)
+    if singular_values[-1] < 1e-6 * singular_values[0]:
+        raise ValueError(
+            "fit_ellipsoid: points are (near-)coplanar; calibration data must "
+            "cover all three axes",
+        )
+
+    A = np.zeros((points.shape[0], 9))
+    A[:, 0] = points[:, 0] ** 2
+    A[:, 1] = points[:, 1] ** 2
+    A[:, 2] = points[:, 2] ** 2
+    A[:, 3] = 2 * points[:, 0] * points[:, 1]
+    A[:, 4] = 2 * points[:, 0] * points[:, 2]
+    A[:, 5] = 2 * points[:, 1] * points[:, 2]
+    A[:, 6] = -2 * points[:, 0]
+    A[:, 7] = -2 * points[:, 1]
+    A[:, 8] = -2 * points[:, 2]
+
+    B = np.ones((points.shape[0], 1))
+
+    X = np.linalg.lstsq(A, B, rcond=None)[0].flatten()
+    if X[0] < 0:
+        X = -X
+
+    ka = np.linalg.inv(
+        np.array(
+            [
+                [X[0], X[3], X[4]],
+                [X[3], X[1], X[5]],
+                [X[4], X[5], X[2]],
+            ],
+        ),
+    )
+    shift = ka.dot(X[6:9])
+
+    scale = scipy.linalg.sqrtm(ka)
+    assert np.isreal(scale).all(), scale
+    scale = np.real(scale)
+    scale = normalized_matrix(scale)
+
+    scale_inv = np.linalg.inv(scale)
+    compensated = [scale_inv.dot(p - shift) for p in points]
+
+    for axis in range(3):
+        assert min(p[axis] for p in compensated) < 0 < max(p[axis] for p in compensated)
+
+    return scale, shift, compensated

src/subjugator/drivers/magnetic_compensation/package.xml

@@ -8,9 +8,20 @@
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>rclcpp_components</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/subjugator/drivers/magnetic_compensation/scripts/generate_config.py

@@ -6,72 +6,20 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import rclpy
-import scipy.linalg
 import yaml
 from geometry_msgs.msg import Vector3
+from magnetic_compensation.ellipsoid import (
+    calculate_error,
+    fit_ellipsoid,
+    normalized_matrix,
+)
 from mpl_toolkits.mplot3d import Axes3D
 from rclpy.serialization import deserialize_message
 from rosbag2_py import ConverterOptions, SequentialReader, StorageOptions
 from rosidl_runtime_py.utilities import get_message
 from tf_transformations import random_rotation_matrix, unit_vector
 
 
-def normalized_matrix(m):
-    assert np.linalg.det(m) > 0
-    return m / np.linalg.det(m) ** (1 / m.shape[0])
-
-
-def calculate_error(points):
-    radii = list(map(np.linalg.norm, points))
-    error = np.std(radii) / np.mean(radii)
-    return error
-
-
-def fit_ellipsoid(points):
-    points = np.array(points)
-
-    A = np.zeros((points.shape[0], 9))
-    A[:, 0] = points[:, 0] ** 2
-    A[:, 1] = points[:, 1] ** 2
-    A[:, 2] = points[:, 2] ** 2
-    A[:, 3] = 2 * points[:, 0] * points[:, 1]
-    A[:, 4] = 2 * points[:, 0] * points[:, 2]
-    A[:, 5] = 2 * points[:, 1] * points[:, 2]
-    A[:, 6] = -2 * points[:, 0]
-    A[:, 7] = -2 * points[:, 1]
-    A[:, 8] = -2 * points[:, 2]
-
-    B = np.ones((points.shape[0], 1))
-
-    X = np.linalg.lstsq(A, B, rcond=None)[0].flatten()
-    if X[0] < 0:
-        X = -X
-
-    ka = np.linalg.inv(
-        np.array(
-            [
-                [X[0], X[3], X[4]],
-                [X[3], X[1], X[5]],
-                [X[4], X[5], X[2]],
-            ],
-        ),
-    )
-    shift = ka.dot(X[6:9])
-
-    scale = scipy.linalg.sqrtm(ka)
-    assert np.isreal(scale).all(), scale
-    scale = np.real(scale)
-    scale = normalized_matrix(scale)
-
-    scale_inv = np.linalg.inv(scale)
-    compensated = [scale_inv.dot(p - shift) for p in points]
-
-    for axis in range(3):
-        assert min(p[axis] for p in compensated) < 0 < max(p[axis] for p in compensated)
-
-    return scale, shift, compensated
-
-
 def axisEqual3D(ax):
     ax.axis("tight")
     extents = np.array([getattr(ax, f"get_{dim}lim")() for dim in "xyz"])

src/subjugator/drivers/magnetic_compensation/src/component.cpp

@@ -3,6 +3,7 @@
 #include <Eigen/Dense>
 #include <rclcpp/rclcpp.hpp>
 
+#include <magnetic_compensation/compensator.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
@@ -73,7 +74,7 @@ class HardsoftCompensator : public rclcpp::Node
 
         Eigen::Vector3d raw;
         tf2::fromMsg(msg->magnetic_field, raw);
-        Eigen::Vector3d processed = scale_inverse_ * (raw - shift_);
+        Eigen::Vector3d processed = compensate(scale_inverse_, shift_, raw);
 
         sensor_msgs::msg::MagneticField result;
         result.header = msg->header;