Merge pull request #51 from AGH-CEAI/feature/add_calib_extrinsics

macmacal · web-flow · commit 4133e3e2e819 · 2025-08-19T14:59:28.000+02:00
Add calibration of camera extrinsics
diff --git a/aegis_utils/CHANGELOG.md b/aegis_utils/CHANGELOG.md
@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* [PR-51](https://github.com/AGH-CEAI/aegis_ros/pull/51) - Added calibration of camera extrinsics program.
 * [PR-50](https://github.com/AGH-CEAI/aegis_ros/pull/50) - Added calibration of camera intrinsics program.
 * [PR-47](https://github.com/AGH-CEAI/aegis_ros/pull/47) - Initial version of the `aegis_utils` package. Added calibration data collection program and corresponding positions configuration for Luxonis OAK-D SR tool camera.
 
diff --git a/aegis_utils/README.md b/aegis_utils/README.md
@@ -45,10 +45,22 @@ ros2 run aegis_utils calib_data_collect -c tool_front_right -p ~/Documents/calib
 
 ## Calibration of camera intrinsics
 This tool computes the intrinsic parameters of a camera using previously collected calibration images.
-It processes a set of chessboard pattern images and saves the resulting camera matrix and distortion coefficients.
+
+It outputs the resulting camera matrix and distortion coefficients and saves it to the calibration data folder.
 
 ### Example usage
 It accepts the same arguments and uses the same default data path as the data collection tool described above. For example:
 ```bash
 ros2 run aegis_utils calib_intrinsics -c tool_front_right
 ```
+
+## Calibration of camera extrinsics
+This tool computes the extrinsic parameters of a camera using previously collected calibration images, TCP poses and the computed camera intrinsics.
+
+It outputs the resulting transformation matrix form the robot TCP to the camera frame and saves it to the calibration data folder.
+
+### Example usage
+It accepts the same arguments and uses the same default data path as the data collection tool described above. For example:
+```bash
+ros2 run aegis_utils calib_extrinsics -c tool_front_right
+```
diff --git a/aegis_utils/aegis_utils/calib_data_collect.py b/aegis_utils/aegis_utils/calib_data_collect.py
@@ -2,23 +2,20 @@
 import threading
 import time
 from pathlib import Path
-from typing import List, Dict, Optional
+from typing import Dict, List, Optional
 
 import cv2
 import numpy as np
-import yaml
-
 import rclpy
-from rclpy.executors import SingleThreadedExecutor
-from rclpy.node import Node
+import yaml
+from aegis_director.robot_director import RobotDirector
 from ament_index_python.packages import get_package_share_directory
 from builtin_interfaces.msg import Time
 from cv_bridge import CvBridge
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
 from sensor_msgs.msg import Image
 
-from aegis_director.robot_director import RobotDirector
-
-
 CAMERA_CONFIG = {
     "scene": {"pos_config": "cam_scene.yaml", "topic": "/cam_scene/rgb/image_rect"},
     "tool_front_right": {
@@ -40,6 +37,26 @@
 }
 
 
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "-c",
+        "--camera",
+        type=str,
+        required=True,
+        choices=CAMERA_CONFIG.keys(),
+        help="Which camera: scene, tool_front_right, tool_front_left, tool_right, tool_left",
+    )
+    parser.add_argument(
+        "-p",
+        "--path",
+        type=str,
+        default=None,
+        help="Optional path to calibration data folder",
+    )
+    return parser.parse_args()
+
+
 def load_positions(config_file_path: Path) -> List[Dict[str, float]]:
     with open(config_file_path, "r") as f:
         data = yaml.safe_load(f)
@@ -155,23 +172,7 @@ def collect_data(
 
 
 def main() -> None:
-    parser = argparse.ArgumentParser()
-    parser.add_argument(
-        "-c",
-        "--camera",
-        type=str,
-        required=True,
-        choices=CAMERA_CONFIG.keys(),
-        help="Which camera: scene, tool_front_right, tool_front_left, tool_right, tool_left",
-    )
-    parser.add_argument(
-        "-p",
-        "--path",
-        type=str,
-        default=None,
-        help="Optional path to calibration data folder",
-    )
-    args = parser.parse_args()
+    args = parse_args()
 
     if args.path:
         data_path = Path(args.path).expanduser() / args.camera
diff --git a/aegis_utils/aegis_utils/calib_extrinsics.py b/aegis_utils/aegis_utils/calib_extrinsics.py
@@ -0,0 +1,227 @@
+import argparse
+import json
+from pathlib import Path
+from typing import Optional, Tuple
+
+import cv2
+import numpy as np
+import yaml
+from natsort import natsorted
+from scipy.spatial.transform import Rotation as R
+
+CAMERA_CONFIG = {
+    "scene": {},
+    "tool_front_right": {},
+    "tool_front_left": {},
+    "tool_right": {},
+    "tool_left": {},
+}
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "-c",
+        "--camera",
+        type=str,
+        required=True,
+        choices=CAMERA_CONFIG.keys(),
+        help="Which camera: scene, tool_front_right, tool_front_left, tool_right, tool_left",
+    )
+    parser.add_argument(
+        "-p",
+        "--path",
+        type=str,
+        default=None,
+        help="Optional path to calibration data folder",
+    )
+    return parser.parse_args()
+
+
+def load_tcp(tcp_path: Path) -> Tuple[np.ndarray, np.ndarray]:
+    with open(tcp_path, "r") as f:
+        data = yaml.safe_load(f)
+    pos = np.array(data["tcp_pose"]["position"], dtype=float)
+    ori = np.array(data["tcp_pose"]["orientation"], dtype=float)
+    return pos, ori
+
+
+def load_intrinsics(intrinsics_path: Path) -> Tuple[np.ndarray, np.ndarray]:
+    with open(intrinsics_path, "r") as f:
+        data = json.load(f)
+    camera_matrix = np.array(data["camera_matrix"], dtype=float)
+    dist_coeffs = np.array(data["dist_coeffs"], dtype=float)
+    return camera_matrix, dist_coeffs
+
+
+def process_view(
+    image_path: Path,
+    tcp_path: Path,
+    board: cv2.aruco_CharucoBoard,
+    aruco_dict: cv2.aruco_Dictionary,
+    camera_matrix: np.ndarray,
+    dist_coeffs: np.ndarray,
+) -> Optional[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]]:
+    try:
+        image_idx = int(image_path.stem.split("_")[-1])
+        tcp_idx = int(tcp_path.stem.split("_")[-1])
+    except ValueError:
+        print(f"Invalid index in filenames: {image_path}, {tcp_path}\nSkipping")
+        return None
+
+    if image_idx != tcp_idx:
+        print(f"Index mismatch: {image_path} vs {tcp_path}\nSkipping")
+        return None
+
+    img = cv2.imread(str(image_path))
+    img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+    corners, ids, _ = cv2.aruco.detectMarkers(img_gray, aruco_dict)
+
+    if ids is None or len(ids) == 0:
+        print(f"No ArUco markers detected in {image_path}\nSkipping")
+        return None
+
+    retval, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco(
+        corners, ids, img_gray, board
+    )
+
+    if retval < 4:
+        print(f"Not enough Charuco corners detected in view {image_path}\nSkipping")
+        return None
+
+    valid, rvec, tvec = cv2.aruco.estimatePoseBoard(
+        charuco_corners, charuco_ids, board, camera_matrix, dist_coeffs, None, None
+    )
+
+    if not valid:
+        print(f"Could not estimate pose for {image_path}\nSkipping")
+        return None
+
+    tcp_pos, tcp_ori = load_tcp(tcp_path)
+
+    R_robot2tcp = R.from_quat(tcp_ori).as_matrix()
+    t_robot2tcp = tcp_pos.reshape(3, 1)
+    R_cam2target, _ = cv2.Rodrigues(rvec)
+    t_cam2target = tvec.reshape(3, 1)
+
+    return R_robot2tcp, t_robot2tcp, R_cam2target, t_cam2target
+
+
+def calibrate_extrinsics(
+    data_path: Path,
+    squares_x: int,
+    squares_y: int,
+    square_size: float,
+    marker_size: float,
+    aruco_dict: cv2.aruco_Dictionary,
+) -> None:
+    board = cv2.aruco.CharucoBoard(
+        (squares_x, squares_y),
+        square_size,
+        marker_size,
+        aruco_dict,
+    )
+    board.setLegacyPattern(True)
+
+    image_paths = natsorted(data_path.glob("*_image_*.png"))
+    tcp_paths = natsorted(data_path.glob("*_tcp_*.yaml"))
+    intrinsics_path = data_path / f"{data_path.name}_intrinsics.json"
+
+    if not image_paths:
+        print(f"No images found in {data_path}")
+        return
+    if not tcp_paths:
+        print(f"No TCP poses found in {data_path}")
+        return
+    if not intrinsics_path.is_file():
+        print(f"Intrinsics file not found in {intrinsics_path}")
+        return
+    if len(image_paths) != len(tcp_paths):
+        print(
+            f"Number of images ({len(image_paths)}) and TCP poses ({len(tcp_paths)}) do not match!"
+        )
+
+    camera_matrix, dist_coeffs = load_intrinsics(intrinsics_path)
+
+    tcp_poses_R = []
+    tcp_poses_t = []
+    target_poses_R = []
+    target_poses_t = []
+
+    valid_views = 0
+
+    for image_path, tcp_path in zip(image_paths, tcp_paths):
+        poses = process_view(
+            image_path, tcp_path, board, aruco_dict, camera_matrix, dist_coeffs
+        )
+        if poses is None:
+            continue
+        R_robot2tcp, t_robot2tcp, R_cam2target, t_cam2target = poses
+
+        tcp_poses_R.append(R_robot2tcp)
+        tcp_poses_t.append(t_robot2tcp)
+        target_poses_R.append(R_cam2target)
+        target_poses_t.append(t_cam2target)
+
+        valid_views += 1
+
+    print(f"Processed {valid_views} valid views for extrinsic calibration")
+
+    if valid_views < 3:
+        print("Not enough valid views for calibration")
+        return
+
+    R_tcp2cam, t_tcp2cam = cv2.calibrateHandEye(
+        tcp_poses_R,
+        tcp_poses_t,
+        target_poses_R,
+        target_poses_t,
+        method=cv2.CALIB_HAND_EYE_TSAI,
+    )
+
+    T_tcp2cam = np.eye(4)
+    T_tcp2cam[:3, :3] = R_tcp2cam
+    T_tcp2cam[:3, 3] = t_tcp2cam.ravel()
+
+    print("Transformation matrix (TCP to camera)\n")
+    print(T_tcp2cam)
+
+    extrinsics_path = data_path / f"{data_path.name}_extrinsics.json"
+    with open(extrinsics_path, "w") as f:
+        json.dump({"T_tcp2cam": T_tcp2cam.tolist()}, f, indent=2)
+
+    print(f"Extrinsics saved to {extrinsics_path}")
+
+
+def main() -> None:
+    args = parse_args()
+
+    if args.path:
+        data_path = Path(args.path).expanduser() / args.camera
+    else:
+        data_path = Path("~/ceai_ws/calibration_data").expanduser() / args.camera
+
+    board_path = Path(__file__).parent.parent / "config" / "charuco_board.yaml"
+    with open(board_path, "r") as f:
+        board_cfg = yaml.safe_load(f)
+
+    if args.camera.startswith("tool"):
+        board_params = board_cfg["tool"]
+    else:
+        board_params = board_cfg["scene"]
+
+    squares_x = board_params["squares_x"]
+    squares_y = board_params["squares_y"]
+    square_size = board_params["square_size"]
+    marker_size = board_params["marker_size"]
+    aruco_dict = cv2.aruco.getPredefinedDictionary(
+        getattr(cv2.aruco, board_params["aruco_dict"])
+    )
+
+    calibrate_extrinsics(
+        data_path, squares_x, squares_y, square_size, marker_size, aruco_dict
+    )
+
+
+if __name__ == "__main__":
+    main()
diff --git a/aegis_utils/aegis_utils/calib_intrinsics.py b/aegis_utils/aegis_utils/calib_intrinsics.py
diff --git a/aegis_utils/setup.py b/aegis_utils/setup.py
