Camera Calibration

cailb.py - Camera calibration module.

class mapit.calib.CalibratedCamera(R_cam2tcp: numpy.ndarray, T_cam2tcp: numpy.ndarray, distortion_coefficients: numpy.ndarray, intrinsics: numpy.ndarray, frame_grabber: Callable[[], numpy.ndarray], image_width: int, image_height: int)

Bases: object

Collects camera-related parameters in a single object.

Parameters:

  • R_cam2tcp (numpy.ndarray) – Rotation matrix from camera to TCP.

  • T_cam2tcp (numpy.ndarray) – Translation vector from camera to TCP.

  • distortion_coefficients (numpy.ndarray) – Camera distortion coefficients.

  • intrinsics (numpy.ndarray) – Camera intrinsic matrix.

  • frame_grabber (callable) – Device-specific callable that grabs a single frame.

  • image_width (int) – Image width in pixels.

  • image_height (int) – Image height in pixels.

export_to_config(path)

Export CalibratedCamera parameters to a YAML file.

Parameters:

path (str) – The path to the YAML config file destination.

grab_frame()

Invoke the .frame_grabber() method.

Raises:

AssertionError – If frame_grabber is not callable, i.e. not set or configured incorrectly.

classmethod new_calibration(images, R_gripper2base, t_gripper2base, board: tuple, frame_grapper: Callable[[], numpy.ndarray])

Perform lens and hand-eye calibration to instantiate a CalibratedCamera instance.

Parameters:

  • images (list of numpy.array) – List of input images of calibration board.

  • R_gripper2base (list of numpy.array) – List of rotation vectors of the robot pose at each image, following the image sequence order.

  • t_gripper2base (list of numpy.array) – List of translation vectors of the robot pose at each image, following the image sequence order.

  • board (tuple) – A tuple containing the number of inner chessboard-corners (width, height) and square size (mm), e.g (5, 8, 0.03).

  • frame_grapper (callable) – A callable method to get an image frame.

Returns:

A CalibratedCamera object.

Return type:

CalibratedCamera

Raises:

  • AssertionError – If the length of the input lists does not match.

  • RuntimeError – If the camera calibration failed.

classmethod new_from_config(path: str, frame_grapper: Callable[[], numpy.ndarray])

Create a CalibratedCamera from a YAML configuration file.

Parameters:

  • path (str) – The path to the YAML config file.

  • frame_grapper (callable) – A callable method to get an image frame.

Returns:

A CalibratedCamera object.

Return type:

CalibratedCamera

set_frame_grabber(frame_grabber: Callable[[], numpy.ndarray])

Set the frame_grabber of a CalibratedCamera instance.

Parameters:

frame_grabber (callable) – A callable method to get an image frame.

size()

Get image size in pixels.

Returns:

Image size in pixels as (width, height).

Return type:

tuple

ArUco Detector

class mapit.detector.Detector(aruco_dict: int, aruco_params: cv2.aruco.DetectorParameters | None, camera: CalibratedCamera)

Bases: object

ArUco marker detector wrapper with camera-aware pose estimation support.

This class employ OpenCV’s cv.aruco.ArucoDetector and binds it together with camera calibration data to provide a convenient interface for ArUco marker detection.

Parameters:

  • ar_dict (int) – OpenCV ArUco dictionary identifier (e.g. cv.aruco.DICT_4X4_50). If None or 0, DICT_4X4_50 is used as default.

  • aruco_params (cv.aruco.DetectorParameters) – Detector parameters controlling thresholding, refinement, and detection behavior. If None, default parameters are created with sub-pixel corner refinement enabled.

  • camera (CalibratedCamera) – A CalibratedCamera instance.

dictionary

The ArUco dictionary used for marker detection.

Type:

cv.aruco.Dictionary

aruco_params

Detector configuration parameters.

Type:

cv.aruco.DetectorParameters

detector

OpenCV ArUco detector instance.

Type:

cv.aruco.ArucoDetector

camera

CalibratedCamera object used for pose estimation.

Type:

CalibratedCamera

roi

Region of interest returned by cv.getOptimalNewCameraMatrix for cropping image frames.

Type:

tuple[int, int, int, int]

align_camera_to_point(origin, target, R_tcp2base, T_tcp2base, method: int = 0)

Generate a new robot orientation to align the camera optical center with a 3D point.

Parameters:

  • origin (numpy.array) – A 3D point of dimensions 3x1, describing the camera position in the world frame.

  • target (numpy.array) – A 3D point of dimensions 3x1, describing the target in the world frame.

  • R_tcp2base (numpy.array) – A 3x3 rotation matrix to transfrom from robot tool to robot base coordinate.

  • T_tcp2base (numpy.array) – A 3x1 translation vector to transform from robot tool to robot base coordinate.

  • method (int) – Flag to select which method to use for aligning camera. Defaults to ALIGN_AXIS_ANGLE.

Returns:

A rotation vector, which when applied to the robot tool aligns the camera to the desired point.

Return type:

numpy.narray

detect_markers() → dict

Capture an image frame and detect ArUco markers.

This method takes a single image from the associated camera and detects ArUco markers using OpenCV’s ArucoDetector.

Returns:

Dictionary mapping marker IDs to their detected corner coordinates. Each value is a (4, 2) array of image pixel coordinates in the order returned by OpenCV (clockwise starting from top-left).

If no markers are detected, an empty dictionary is returned.

Return type:

dict[int, np.ndarray]

Notes

  • Corner coordinates are given in image pixel coordinates, not in the camera coordinate frame.

  • No pose estimation or distortion correction is performed here. See estimate_marker_pose().

estimate_marker_pose(img_points) → tuple

Estimate marker pose in world coordinate frame, with respect to the camera frame.

This method a single (4,2) array of ArUco marker corners, and estimates the position and orientation using Perspective-n-Point (PnP).

Parameters:

img_points (numpy.ndarray) – A single (4, 2) array-like structure representing the pixel coordinates of the 4 marker corners in the image frame. This can be obtained from detect_markers()

Returns:

A np.array containing the translation vector and rotation (wrt. camera origin), represented as a homogeneous transformation matrix. A bool reflecting the state of the solvePnP (false if solvePnP failed)

Return type:

tuple

Utilities

utilities.py - A collection of helper functions.

mapit.utils.apply_rotation_and_translation(point: numpy.ndarray, R_matrix: numpy.ndarray, t_vector: numpy.ndarray) → numpy.ndarray

Apply a rotation and translation to a 3D point.

Parameters:

  • point (numpy.array) – A 3D point of dimensions 3x1.

  • R_matrix (numpy.array) – A 3x3 rotation matrix.

  • t_vector (numpy.array) – A translation vector of dimensions 3x1

Returns:

The transformed 3D point.

Return type:

numpy.narray

Raises:

AssertionError – If the shape of any input parameter is incorrect.

mapit.utils.apply_transformation(target: numpy.ndarray, transformation: numpy.ndarray) → numpy.ndarray

Apply a 4x4 homogeneous transformation to either another 4x4 homogeneous transformation matrix or 3x1 point.

The shape of the output is identical to the input target.

Parameters:

  • target (numpy.array) – A 4x4 homogeneous transformation matrix.

  • transformation (numpy.array) – A 4x4 homogeneous transformation matrix.

Returns:

The transformed target.

Return type:

numpy.narray

Raises:

AssertionError – If the shape of any input parameter is incorrect.

mapit.utils.axis_angle_align(camera_position, target, R_cam2base)

Compute a tool orientation that aligns a camera optical center to a point in 3D, using the axis-angle method.

Parameters:

  • camera_position (np.ndarray) – A 3x1 vector representing the camera position in the world frame.

  • target (np.ndarray) – A 3x1 vector representing the target position in the world frame.

  • R_cam2base (np.ndarray) – A 3x3 rotation matrix used to compute orientation from camera frame to world frame.

Returns:

  • np.ndarray – A 3x3 rotation matrix.

  • ——-

mapit.utils.build_pose(tvec, rvec)

Combine a translation vector and rotation vector into a 1x6 array, used by the ur_commander.TaskPose.

Parameters:

  • tvec (array) – A 1x3 array representing a 3D position.

  • rvec (array) – A 1x3 array representing the rotation vector.

Returns:

A 1x6 array containing the tvec and rvec.

Return type:

array

mapit.utils.extract_pose_and_orientation(H_matrix: numpy.ndarray) → tuple[numpy.ndarray, numpy.ndarray]

Extract the translation and rotation vector from a 4x4 homogeneous transformation matrix.

Parameters:

H_matrix (numpy.ndarray) – A 4x4 homogeneous transformation matrix.

Returns:

A tuple containing the 3x1 rotation vector and 3x1 translation vector.

Return type:

tuple

Raises:

AssertionError – If the shape of the input matrix is not 4x4.

mapit.utils.look_at(camera_position: numpy.ndarray, target_position: numpy.ndarray)

Compute a tool orientation that aligns a camera optical center to a point in 3D, using the LookAt method.

Parameters:

  • camera_position (np.ndarray) – A 3x1 vector representing the camera position in the world frame.

  • target_position (np.ndarray) – A 3x1 vector representing the target position in the world frame.

Returns:

  • np.ndarray – A 3x3 rotation matrix.

  • ——-

mapit.utils.make_homogeneous(R_matrix: numpy.ndarray, t_vector: numpy.ndarray) → numpy.ndarray

Create a 4x4 homogeneous transformation matrix from a rotation and translation.

Parameters:

  • R_matrix (numpy.array) – A 3x3 rotation matrix or 3x1 rotation vector.

  • t_vector (numpy.array) – A 3x1 translation vector.

Returns:

A 4x4 homogeneous transformation matrix.

Return type:

numpy.narray

Raises:

AssertionError – If the shape of any input parameter is incorrect.

mapit.utils.rot_tran_from_tool_pose(tool_pose: ur_commander.TaskPose)

Extract the rotation matrix and translation vector from a ur_commander.TaskPose.

Parameters:

tool_pose (ur_commander.TaskPose) – A 1x6 array containg a translation and rotation vector.

Returns:

A tuple containing the rotation vector as a 3x3 rotation matrix and the 3x1 translation vector.

Return type:

tuple

Raises:

AssertionError – If the size of the input tool_pose is not 6.

Models

Data model definitions used throughout the toolbox.

This module currently defines the Marker class, which represents a 3D marker described by a homogeneous transformation matrix.

class mapit.models.Marker(id: int, H: numpy.ndarray)

Bases: object

Represents a 3D marker with pose information.

A marker is defined by a unique identifier and a 4x4 homogeneous transformation matrix encoding its position and orientation in space.

Parameters:

  • id (int) – Unique identifier of the marker.

  • H (numpy.ndarray) – A (4, 4) homogeneous transformation matrix representing the marker pose.

id

Unique identifier of the marker.

Type:

int

H

Homogeneous transformation matrix of shape (4, 4).

Type:

numpy.ndarray

Notes

The transformation matrix H is assumed to follow the standard homogeneous transformation convention:

  • H[:3, :3] contains the 3x3 rotation matrix.

  • H[:3, 3] contains the 3D translation vector.

property orientation: numpy.ndarray

Return the 3x3 rotation matrix.

Extracted from the transformation matrix (shape: (3, 3)).

property position: numpy.ndarray

Return the 3D position vector.

Extracted from the transformation matrix (shape: (3,)).