feat(aruco): implement core alignment utilities for ground plane alignment

py_workspace/.sisyphus/notepads/ground-plane-alignment/decisions.md

@@ -0,0 +1,2 @@
+- Alignment is applied via pre-multiplication to the 4x4 pose matrix, consistent with global frame rotation.
+- Chose to raise ValueError for degenerate cases (collinear corners) in compute_face_normal.

py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md

@@ -0,0 +1,2 @@
+- Implemented core alignment utilities in aruco/alignment.py.
+- Used Rodrigues' rotation formula for vector alignment with explicit handling for parallel and anti-parallel cases.

py_workspace/aruco/alignment.py

@@ -0,0 +1,99 @@
+import numpy as np
+
+
+def compute_face_normal(corners: np.ndarray) -> np.ndarray:
+    """
+    Compute the normal vector of a face defined by its corners.
+    Assumes corners are in order (e.g., clockwise or counter-clockwise).
+
+    Args:
+        corners: (N, 3) array of corner coordinates.
+
+    Returns:
+        (3,) normalized normal vector.
+    """
+    if corners.shape[0] < 3:
+        raise ValueError("At least 3 corners are required to compute a normal.")
+
+    # Use the cross product of two edges
+    v1 = corners[1] - corners[0]
+    v2 = corners[2] - corners[0]
+
+    normal = np.cross(v1, v2)
+    norm = np.linalg.norm(normal)
+
+    if norm < 1e-10:
+        raise ValueError("Corners are collinear or degenerate; cannot compute normal.")
+
+    return normal / norm
+
+
+def rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarray:
+    """
+    Compute the 3x3 rotation matrix that aligns from_vec to to_vec.
+
+    Args:
+        from_vec: (3,) source vector.
+        to_vec: (3,) target vector.
+
+    Returns:
+        (3, 3) rotation matrix.
+    """
+    # Normalize inputs
+    a = from_vec / np.linalg.norm(from_vec)
+    b = to_vec / np.linalg.norm(to_vec)
+
+    v = np.cross(a, b)
+    c = np.dot(a, b)
+    s = np.linalg.norm(v)
+
+    # Handle parallel case
+    if s < 1e-10:
+        if c > 0:
+            return np.eye(3)
+        else:
+            # Anti-parallel case: 180 degree rotation around an orthogonal axis
+            # Find an orthogonal axis
+            if abs(a[0]) < 0.9:
+                ortho = np.array([1, 0, 0])
+            else:
+                ortho = np.array([0, 1, 0])
+
+            axis = np.cross(a, ortho)
+            axis /= np.linalg.norm(axis)
+
+            # Rodrigues formula for 180 degrees
+            K = np.array(
+                [[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]
+            )
+            return np.eye(3) + 2 * (K @ K)
+
+    # General case using Rodrigues' rotation formula
+    # R = I + [v]_x + [v]_x^2 * (1-c)/s^2
+    vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
+
+    R = np.eye(3) + vx + (vx @ vx) * ((1 - c) / (s**2))
+    return R
+
+
+def apply_alignment_to_pose(T: np.ndarray, R_align: np.ndarray) -> np.ndarray:
+    """
+    Apply an alignment rotation to a 4x4 pose matrix.
+    The alignment is applied in the global frame (pre-multiplication of rotation).
+
+    Args:
+        T: (4, 4) homogeneous transformation matrix.
+        R_align: (3, 3) alignment rotation matrix.
+
+    Returns:
+        (4, 4) aligned transformation matrix.
+    """
+    if T.shape != (4, 4):
+        raise ValueError(f"Expected 4x4 matrix, got {T.shape}")
+    if R_align.shape != (3, 3):
+        raise ValueError(f"Expected 3x3 matrix, got {R_align.shape}")
+
+    T_align = np.eye(4)
+    T_align[:3, :3] = R_align
+
+    return T_align @ T