Some more optimization

Author: hakon-molven-4ss

src/smsfusion/_coning_sculling.py

@@ -1,7 +1,7 @@
 import numpy as np
 from numpy.typing import ArrayLike
 
-from smsfusion._vectorops import _cross, _determinant_3_by_3, _inverse_3_by_3
+from smsfusion._vectorops import _adjugate_and_det_3_by_3, _cross
 
 
 class ConingScullingAlg:
@@ -196,15 +196,21 @@ def __init__(
         b_f: np.ndarray = np.zeros(3),
         bias_alt: bool = False,
     ):
-        W_w_det = _determinant_3_by_3(W_w)
-        W_w_inv = _inverse_3_by_3(W_w, determinant=W_w_det)
+        adj_W_w, W_w_det = _adjugate_and_det_3_by_3(W_w)
+        if W_w_det == 0:
+            raise ValueError("W_w must be invertible")
+        W_w_inv = adj_W_w / W_w_det
         self.cof_W = W_w_inv.T * W_w_det
         self.W_star = W_w_inv @ W_f
         if bias_alt:
             self.b_f_star = b_f
             self.b_w_star = b_w
         else:
-            self.b_f_star = _inverse_3_by_3(W_f) @ b_f
+            adj_W_f, W_f_det = _adjugate_and_det_3_by_3(W_f)
+            if W_f_det == 0:
+                raise ValueError("W_f must be invertible.")
+            W_f_inv = adj_W_f / W_f_det
+            self.b_f_star = W_f_inv @ b_f
             self.b_w_star = W_w_inv @ b_w
         self.W_w = W_w
         super().__init__(fs)

src/smsfusion/_vectorops.py

@@ -93,23 +93,29 @@ def _skew_symmetric(a: NDArray[np.float64]) -> NDArray[np.float64]:
 
 
 @njit  # type: ignore[misc]
-def _determinant_3_by_3(m: np.ndarray) -> float:
+def _adjugate_and_det_3_by_3(
+    m: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], float]:
     """
-    Calculates and returns the determinant of the matrix
+    Calculates and returns the adjugate matrix and determinant of the matrix
     ```python
     m = [[a, b, c],
          [d, e, f],
          [g, h, i]]
     ```
+    If the determinant is non-zero, one can calculate the inverse of m as
+    ``inv(m) = adj(m) / det(m)``.
 
     Parameters
     ----------
-    m : array-like, shape (3, 3)
-        The matrix for which to calculate the determinant.
+    m : numpy.ndarray, shape (3, 3)
+        The matrix for which to calculate the adjugate and determinant.
 
     Returns
     -------
-    det : float
+    adj_m : numpy.ndarray, shape (3, 3)
+        The adjugate of the input matrix m.
+    det_m : float
         The determinant of the input matrix m.
     """
     a, b, c = m[0]
@@ -118,49 +124,13 @@ def _determinant_3_by_3(m: np.ndarray) -> float:
     A = e * i - f * h
     B = -(d * i - f * g)
     C = d * h - e * g
-
-    return a * A + b * B + c * C
-
-
-@njit  # type: ignore[misc]
-def _inverse_3_by_3(m: np.ndarray, determinant: float | None = None):
-    """
-    Calculates and returns the inverse of the matrix
-    ```python
-    m = [[a, b, c],
-         [d, e, f],
-         [g, h, i]]
-    ```
-
-    Parameters
-    ----------
-    m : array-like, shape (3, 3)
-        The matrix to be inverted.
-    determinant : float, optional
-        The determinant of the input matrix m. If not provided, it will be calculated
-        internally.
-
-    Returns
-    -------
-    inv_m : ndarray, shape (3, 3)
-        The inverse of the input matrix m.
-    """
-    if m.shape != (3, 3):
-        raise ValueError("Input matrix must be 3x3.")
-    a, b, c = m[0]
-    d, e, f = m[1]
-    g, h, i = m[2]
-    A = e * i - f * h
-    B = -(d * i - f * g)
-    C = d * h - e * g
     D = -(b * i - c * h)
     E = a * i - c * g
     F = -(a * h - b * g)
     G = b * f - c * e
     H = -(a * f - c * d)
-    I = a * e - b * d
-    if determinant is None:
-        determinant = _determinant_3_by_3(m)
-    if determinant == 0:
-        raise ValueError("Input matrix is singular and cannot be inverted.")
-    return np.array([[A, D, G], [B, E, H], [C, F, I]]) / determinant
+    I_ = a * e - b * d
+    # Equations fetched from https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices
+    adj_m = np.array([[A, D, G], [B, E, H], [C, F, I_]])
+    det_m = a * A + b * B + c * C
+    return adj_m, det_m

tests/test_coning_sculling.py

@@ -389,3 +389,28 @@ def test_verify_calibration(self, data_ag, data_dtheta_dvel):
         np.testing.assert_allclose(dvel_calibrated_alt, dvel_true, atol=1e-8)
         np.testing.assert_allclose(dtheta_naive, dtheta_true, atol=1e-8)
         np.testing.assert_allclose(dvel_naive, dvel_true, atol=1e-8)
+
+    @pytest.mark.parametrize(
+        "w_singular, f_singular", [(True, False), (False, True), (True, True)]
+    )
+    def test_raises_singular_matrix(self, w_singular, f_singular):
+        fs = 100.0
+        if w_singular:
+            W_w = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]])
+        else:
+            W_w = np.eye(3)
+        if f_singular:
+            W_f = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]])
+        else:
+            W_f = np.eye(3)
+        b_w = np.zeros(3)
+        b_f = np.zeros(3)
+        with pytest.raises(ValueError, match="must be invertible"):
+            sf.ConingScullingAlgCalibrated(
+                fs, W_w=W_w, W_f=W_f, b_w=b_w, b_f=b_f, bias_alt=False
+            )
+        if w_singular:
+            with pytest.raises(ValueError, match="must be invertible"):
+                sf.ConingScullingAlgCalibrated(
+                    fs, W_w=W_w, W_f=W_f, b_w=b_w, b_f=b_f, bias_alt=True
+                )

tests/test_vectorops.py

@@ -72,23 +72,14 @@ def test__skew_symmetric():
     np.testing.assert_array_equal(out, expected)
 
 
-def test__inverse_and_determinant_3_by_3():
-    """Test matrix inversion and determinant against numpy"""
+def test__adjugate_and_det_3_by_3():
+    """Test matrix inversion against numpy"""
     rng = np.random.default_rng(42)
     for _ in range(5):
         m = rng.random((3, 3))
-        det = _vectorops._determinant_3_by_3(m)
-        inv = _vectorops._inverse_3_by_3(m)
-        inv_with_det = _vectorops._inverse_3_by_3(m, determinant=det)
-        det_expected = np.linalg.det(m)
-        inv_expected = np.linalg.inv(m)
-        np.testing.assert_allclose(det, det_expected, rtol=1e-12, atol=1e-12)
-        np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)
-        np.testing.assert_allclose(inv_with_det, inv_expected, rtol=1e-12, atol=1e-12)
 
+        adj, det = _vectorops._adjugate_and_det_3_by_3(m)
+        inv = adj / det
 
-def test__inverse_3_by_3_singular():
-    """Test that the inverse function raises an error for singular matrices"""
-    singular_matrix = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
-    with pytest.raises(ValueError):
-        _vectorops._inverse_3_by_3(singular_matrix)
+        inv_expected = np.linalg.inv(m)
+        np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)