diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/decisions.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/decisions.md new file mode 100644 index 0000000..769b0c3 --- /dev/null +++ b/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. diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md new file mode 100644 index 0000000..e69de29 diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md new file mode 100644 index 0000000..5b37fb2 --- /dev/null +++ b/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. diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/problems.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/problems.md new file mode 100644 index 0000000..e69de29 diff --git a/py_workspace/aruco/alignment.py b/py_workspace/aruco/alignment.py new file mode 100644 index 0000000..e872505 --- /dev/null +++ b/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