chore(metadata): update beads and sisyphus planning artifacts
This commit is contained in:
@@ -29,8 +29,21 @@ from aruco.alignment import (
|
||||
detect_ground_face,
|
||||
rotation_align_vectors,
|
||||
apply_alignment_to_pose,
|
||||
Vec3,
|
||||
Mat44,
|
||||
)
|
||||
from loguru import logger
|
||||
from jaxtyping import Float
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
# Type aliases
|
||||
if TYPE_CHECKING:
|
||||
Mat33 = Float[np.ndarray, "3 3"]
|
||||
CornersNC = Float[np.ndarray, "N 3"]
|
||||
else:
|
||||
Mat33 = np.ndarray
|
||||
CornersNC = np.ndarray
|
||||
|
||||
|
||||
ARUCO_DICT_MAP = {
|
||||
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
|
||||
@@ -590,11 +603,11 @@ def main(
|
||||
)
|
||||
|
||||
if ground_normal is not None:
|
||||
R_align = rotation_align_vectors(ground_normal, np.array([0, 1, 0]))
|
||||
R_align: Mat33 = rotation_align_vectors(ground_normal, np.array([0, 1, 0]))
|
||||
logger.info(f"Computed alignment rotation for face '{target_face}'")
|
||||
|
||||
for serial, data in results.items():
|
||||
T_mean = np.fromstring(data["pose"], sep=" ").reshape(4, 4)
|
||||
T_mean: Mat44 = np.fromstring(data["pose"], sep=" ").reshape(4, 4)
|
||||
T_aligned = apply_alignment_to_pose(T_mean, R_align)
|
||||
data["pose"] = " ".join(f"{x:.6f}" for x in T_aligned.flatten())
|
||||
logger.debug(f"Applied alignment to camera {serial}")
|
||||
|
||||
Reference in New Issue
Block a user