courtvision.geometry

camera_space_to_world_space(camera_point, translation_vector, rotation_vector)

Transform a point from camera space to world space.

Parameters:

Name	Type	Description	Default
camera_point	np.array	3D point in camera space with shape (3,)	required
translation_vector	np.array	Translation vector of the camera with shape (3,)	required
rotation_vector	np.array	Rotation vector of the camera with shape (3,)	required

Raises:

Type	Description
ValueError	If the camera point is not a 3D point with shape (3,)

Returns:

Type	Description
np.array	np.array: 3D point in world space with shape (3,)

Source code in courtvision/geometry.py

def camera_space_to_world_space(
    camera_point: np.array,
    translation_vector: np.array,
    rotation_vector: np.array,
) -> np.array:
    """Transform a point from camera space to world space.

    Args:
        camera_point (np.array): 3D point in camera space with shape (3,)
        translation_vector (np.array): Translation vector of the camera with shape (3,)
        rotation_vector (np.array): Rotation vector of the camera with shape (3,)

    Raises:
        ValueError: If the camera point is not a 3D point with shape (3,)

    Returns:
        np.array: 3D point in world space with shape (3,)
    """
    if not camera_point.shape[0] == 3:
        raise ValueError("Camera point must be a 3D point with shape (3,)")
    R, _ = cv2.Rodrigues(rotation_vector)
    world_point = (R.T @ (camera_point - translation_vector)).T
    return world_point

compute_ray_intersecting_plane(point_a_on_ray, point_b_on_ray, plane_normal=np.array([[0, 0, 1]]), plane_point=np.array([0, 0, 0]))

Given two points on a ray, compute the point of intersection with a plane. The plane is defined as a normal vector and a point on the plane.

Parameters:

Name	Type	Description	Default
point_a_on_ray	np.array	3D point on the ray with shape (3, 1)	required
point_b_on_ray	np.array	3D point on the ray with shape (3, 1)	required
plane_normal	np.array	Unit vector pointing out the plane. Defaults to np.array([[0, 0, 1]]). Shape (1, 3)	np.array([[0, 0, 1]])
plane_point	np.array	Point on the plane. Defaults to np.array([0, 0, 0]). Shape (3,)	np.array([0, 0, 0])

Returns:

Type	Description
	np.array: Returns the 3D point of intersection with shape (3,)

Source code in courtvision/geometry.py

def compute_ray_intersecting_plane(
    point_a_on_ray: np.array,
    point_b_on_ray: np.array,
    plane_normal: np.array = np.array([[0, 0, 1]]),
    plane_point: np.array = np.array([0, 0, 0]),
):
    """Given two points on a ray, compute the point of intersection with a plane.
    The plane is defined as a normal vector and a point on the plane.

    Args:
        point_a_on_ray (np.array): 3D point on the ray with shape (3, 1)
        point_b_on_ray (np.array): 3D point on the ray with shape (3, 1)
        plane_normal (np.array, optional): Unit vector pointing out the plane. Defaults to np.array([[0, 0, 1]]). Shape (1, 3)
        plane_point (np.array, optional): Point on the plane. Defaults to np.array([0, 0, 0]). Shape (3,)

    Returns:
        np.array: Returns the 3D point of intersection with shape (3,)
    """
    # Vector along the ray direction A -> B
    if not (point_a_on_ray.shape == point_b_on_ray.shape == (3, 1)):
        raise ValueError(
            f"Point A and B must be 3D points with shape (3, 1). {point_a_on_ray.shape=} {point_b_on_ray.shape=}"
        )
    if not plane_normal.shape == (1, 3) and np.linalg.norm(plane_normal) == 1:
        raise ValueError(
            "Plane normal must be a unit vector with shape (1, 3) and norm 1"
        )
    if not plane_point.shape == (3,):
        raise ValueError("Plane point must be a 3D point with shape (3,)")

    ray_direction = point_b_on_ray - point_a_on_ray
    # Finding parameter t
    t = -(np.dot(plane_normal, point_a_on_ray) + plane_point) / np.dot(
        plane_normal, ray_direction
    )
    # Finding point of intersection
    intersection = (point_a_on_ray.T + t * ray_direction.T).squeeze(0)
    return intersection

convert_corners_to_coords(corners)

Convert corners_world_xx_n to a numpy array of shape (12,)

Parameters:

Name	Type	Description	Default
corners	dict		required

Returns:

Type	Description
np.ndarray	np.ndarray: numpy array of shape (12,)

Source code in courtvision/geometry.py

def convert_corners_to_coords(corners: dict) -> np.ndarray:
    """Convert `corners_world_xx_n` to a numpy array of shape (12,)

    Args:
        corners (dict):

    Returns:
        np.ndarray: numpy array of shape (12,)
    """
    vec_of_positions = convert_corners_to_vec(corners=corners)
    if "z" in vec_of_positions:
        return np.array(
            [
                (x, y, z)
                for x, y, z in zip(
                    vec_of_positions["x"], vec_of_positions["y"], vec_of_positions["z"]
                )
            ],
            dtype=np.float32,
        )
    return np.array(
        [(x, y) for x, y in zip(vec_of_positions["x"], vec_of_positions["y"])],
        dtype=np.float32,
    )

convert_corners_to_vec(corners)

Convert corners_world_xx_n to a dict of vectors

Parameters:

Name	Type	Description	Default
corners	dict		required

Returns:

Name	Type	Description
dict	dict	dict with keys x, y, z and numpy array of each

Source code in courtvision/geometry.py

def convert_corners_to_vec(corners: dict) -> dict:
    """Convert `corners_world_xx_n` to a dict of vectors

    Args:
        corners (dict):

    Returns:
        dict: dict with keys x, y, z and numpy array of each
    """
    vec_of_positions = defaultdict(partial(np.ndarray, 0))
    for _, x_y_z in corners.items():
        for axis, value in zip(["x", "y", "z"], x_y_z, strict=False):
            vec_of_positions[axis] = np.append(vec_of_positions[axis], value)
    return vec_of_positions

convert_obj_points_to_planar(object_points)

Converts object points to planar points by finding the common axis and permuting the points so that the common axis is the last axis. Assumes that the object points are planar.

Parameters:

Name	Type	Description	Default
object_points	np.array	description	required

Raises:

Type	Description
ValueError	When points are not planar

Returns:

Type	Description
np.array	np.array: description

Source code in courtvision/geometry.py

def convert_obj_points_to_planar(object_points: np.array) -> np.array:
    """Converts object points to planar points by finding the common axis and permuting the points so that the common axis is the last axis.
    Assumes that the object points are planar.

    Args:
        object_points (np.array): _description_

    Raises:
        ValueError: When points are not planar

    Returns:
        np.array: _description_
    """
    common_axis = None
    for axis in [0, 1, 2]:
        if all(object_points[0, axis] == o for o in object_points[:, axis]):
            common_axis = axis
            break
    if common_axis is None:
        raise ValueError("Could not find common axis")
    # permute the object points so that the common axis is the last axis
    return np.concatenate(
        [
            object_points[:, [i for i in range(3) if i != common_axis]],
            np.zeros((object_points.shape[0], 1)),
        ],
        axis=1,
    ).astype(np.float32)

denormalize_as_named_points(normalised_named_points, image_width, image_height)

Transforms a dict of normalized points 0 to 1 to image points using the supplied image dimension.

Parameters:

Name	Type	Description	Default
normalised_named_points	dict[str, Point2D]	Dict of points normalised from 0.0 to 1.0	required
image_width	int	Image width to expand to.	required
image_height	int	Image height to expand to.	required

Returns:

Type	Description
dict[str, Point2D]	dict[str, Point2D]: Retruns a dict of similar struture but with image points.

Source code in courtvision/geometry.py

def denormalize_as_named_points(
    normalised_named_points: dict[str, Point2D], image_width: int, image_height: int
) -> dict[str, Point2D]:
    """Transforms a dict of normalized points `0 to 1` to image points using the
    supplied image dimension.

    Args:
        normalised_named_points (dict[str, Point2D]): Dict of points normalised from `0.0` to `1.0`
        image_width (int): Image width to expand to.
        image_height (int): Image height to expand to.

    Returns:
        dict[str, Point2D]: Retruns a dict of similar struture but with image points.
    """
    return {
        k: (v[0] * image_width, v[1] * image_height)
        for k, v in normalised_named_points.items()
    }

find_optimal_calibration_and_pose(valid_clip_ids, calibration_correspondences, pose_correspondences, image_width, image_height, all_image_points, all_world_points)

Givern a set of calibration and pose correspondences, find the optimal calibration and pose. This is done by building up combinations of these sets and evaluating the reprojection error. The reprojection error is the mean of the euclidean distance between the reprojected points and the actual points. The evvaluation is on all all_image_points and all_world_points.

Parameters:

Name	Type	Description	Default
valid_clip_ids	set[str]	description	required
calibration_correspondences	list[tuple[np.array, np.array]]	description	required
pose_correspondences	list[tuple[np.array, np.array]]	description	required
image_width	int	Image width	required
image_height	int	Image height	required
all_image_points	np.array	3D points that we want to reproject.	required
all_world_points	np.array	2D points that are where we expect the 3D points to be reprojected to.	required

Raises:

Type	Description
RuntimeError	description

Returns:

Name	Type	Description
CameraInfo	CameraInfo	description

Source code in courtvision/geometry.py

def find_optimal_calibration_and_pose(
    valid_clip_ids: set[str],
    calibration_correspondences: list[tuple[np.array, np.array]],
    pose_correspondences: list[tuple[np.array, np.array]],
    image_width: int,
    image_height: int,
    all_image_points: np.array,
    all_world_points: np.array,
) -> CameraInfo:
    """
    Givern a set of calibration and pose correspondences, find the optimal calibration and pose.
    This is done by building up combinations of these sets and evaluating the reprojection error.
    The reprojection error is the mean of the euclidean distance between the reprojected points and the actual points.
    The evvaluation is on all `all_image_points` and `all_world_points`.

    Args:
        valid_clip_ids (set[str]): _description_
        calibration_correspondences (list[tuple[np.array, np.array]]): _description_
        pose_correspondences (list[tuple[np.array, np.array]]): _description_
        image_width (int): Image width
        image_height (int): Image height
        all_image_points (np.array): 3D points that we want to reproject.
        all_world_points (np.array): 2D points that are where we expect the 3D points to be reprojected to.

    Raises:
        RuntimeError: _description_

    Returns:
        CameraInfo: _description_
    """
    CALIBRATION_MIN_PAIRS = 4
    CALIBRATION_MAX_PAIRS = min(8, len(calibration_correspondences))

    POSE_MIN_PAIRS = 4
    POSE_MAX_PAIRS = min(8, len(pose_correspondences))

    calibration_indexes = [o for o in range(len(calibration_correspondences))]
    calibration_selected_pairs: list[tuple[int, ...]] = list(
        chain.from_iterable(
            (combinations(calibration_indexes, num_pairs_to_use))
            for num_pairs_to_use in range(CALIBRATION_MIN_PAIRS, CALIBRATION_MAX_PAIRS)
        )
    )

    pose_indexes = [o for o in range(len(pose_correspondences))]
    pose_selected_pairs: list[tuple[int, ...]] = list(
        chain.from_iterable(
            (combinations(pose_indexes, num_pairs_to_use))
            for num_pairs_to_use in range(POSE_MIN_PAIRS, POSE_MAX_PAIRS)
        )
    )

    best_error_in_reprojected_points = 10000.0
    best_camera_info = None

    for calibration_pair, pose_pair in product(
        calibration_selected_pairs, pose_selected_pairs
    ):
        calibration_correspondences_selection = [
            calibration_correspondences[o] for o in calibration_pair
        ]
        pose_correspondences_selection = [pose_correspondences[o] for o in pose_pair]

        camera_info = calibrate_and_evaluate(
            valid_clip_ids=valid_clip_ids,
            calibration_correspondences_selected=calibration_correspondences_selection,
            pose_correspondences_selected=pose_correspondences_selection,
            image_width=image_width,
            image_height=image_height,
            all_image_points=all_image_points,
            all_world_points=all_world_points,
        )
        if camera_info.error_in_reprojected_points < best_error_in_reprojected_points:
            best_camera_info = camera_info
    if best_camera_info is None:
        raise RuntimeError("Failed to find optimal calibration and pose")
    return best_camera_info

get_planar_point_correspondences(world_points, image_points, available_labels=None, minimal_set_count=4)

Given a set of named points in the world and image, return a list of point correspondences where all points are coplanar. If a specified set available_labels is given, only return point correspondences where all points are in that set.

Parameters:

Name	Type	Description	Default
world_points	dict[str, tuple[float, float]]	Dict of named points in the world coordinate frame.	required
image_points	dict[str, tuple[float, float]]	Dict of named points in the image coordinate frame.	required
available_labels	Optional[set[str]]	Set of labels to use if None all labels are used. Defaults to None.	None
minimal_set_count	int	Sets of corresponding points . Defaults to 4.	4

Returns:

Type	Description
list[tuple[np.ndarray, np.ndarray]]	list[tuple[np.ndarray, np.ndarray]]: Returns a list of point correspondences where all points are coplanar.
list[tuple[np.ndarray, np.ndarray]]	list[tuple[Nx3, Nx2]]

Source code in courtvision/geometry.py

def get_planar_point_correspondences(
    world_points: dict[str, tuple[float, float]],
    image_points: dict[str, tuple[float, float]],
    available_labels: Optional[set[str]] = None,
    minimal_set_count: int = 4,
) -> list[tuple[np.ndarray, np.ndarray]]:
    """Given a set of named points in the world and image, return a list of point correspondences
    where all points are coplanar.
    If a specified set `available_labels` is given, only return point correspondences where all
    points are in that set.
    Args:
        world_points (dict[str, tuple[float, float]]): Dict of named points in the world coordinate frame.
        image_points (dict[str, tuple[float, float]]): Dict of named points in the image coordinate frame.
        available_labels (Optional[set[str]], optional): Set of labels to use if None all labels are used. Defaults to None.
        minimal_set_count (int, optional): Sets of corresponding points . Defaults to 4.

    Returns:
        list[tuple[np.ndarray, np.ndarray]]: Returns a list of point correspondences where all points are coplanar.
        list[tuple[Nx3, Nx2]]
    """
    available_labels = available_labels or set(image_points.keys())
    available_planes_for_calibration = get_planar_points_padel_court(
        available_labels=available_labels,
        minimal_set_count=minimal_set_count,
    )
    from courtvision.data import dict_to_points

    planar_point_correspondences = []
    for plane in available_planes_for_calibration:
        world_points_on_plane_dict = {k: world_points[k] for k in plane}
        image_points_on_plane_dict = {k: image_points[k] for k in plane}
        world_points_on_plane, _ = dict_to_points(world_points_on_plane_dict)
        image_points_on_plane, _ = dict_to_points(image_points_on_plane_dict)
        planar_point_correspondences.append(
            (world_points_on_plane, image_points_on_plane)
        )
    return planar_point_correspondences

project_points_to_base_plane(points, H)

Given homogeneous points or 2D points and a homography, project the points to the base plane

Parameters:

Name	Type	Description	Default
points	torch.tensor	Homogeneous points or 2D points	required
H	torch.tensor	Homography 3x3 matrix	required

Raises:

Type	Description
ValueError	If points is not of length 2 or 3

Returns:

Type	Description
torch.tensor	torch.tensor: Projected points in either homogeneous or 2D corrdinates. Same as points

Source code in courtvision/geometry.py

def project_points_to_base_plane(points: torch.tensor, H: torch.tensor) -> torch.tensor:
    """Given homogeneous points or 2D points and a homography, project the points to the base plane

    Args:
        points (torch.tensor): Homogeneous points or 2D points
        H (torch.tensor): Homography 3x3 matrix

    Raises:
        ValueError: If `points` is not of length 2 or 3

    Returns:
        torch.tensor: Projected points in either homogeneous or 2D corrdinates. Same as `points`
    """
    if len(points.shape) == 2:
        return convert_points_from_homogeneous(
            convert_points_to_homogeneous(points) @ H.T
        )
    elif len(points.shape) == 3:
        return points @ H.T
    else:
        raise ValueError(f"{points.shape=} must be of length 2 or 3.")

solve_for_camera_matrix(world_points, image_points, image_size, repo_erro_threshold=0.1)

From a set of world points and image points, solve for the camera matrix and distortion coefficients. Note: All world points must have the same z value. i.e lie on the same plane.

Parameters:

Name	Type	Description	Default
world_points	torch.Tensor	Tensor of world points.	required
image_points	torch.Tensor	Tensor of image points.	required
image_size	tuple[int, int]	Image dimensions as (Width, Height).	required
repo_error	float	Reprojection error measured in pixels. Defaults to 1e-1.	required

Returns (Tuple[torch.Tensor, torch.Tensor, float]): camera_matrix (3x3), dist_coeffs (1x5), repo_erro

Source code in courtvision/geometry.py

def solve_for_camera_matrix(
    world_points: torch.Tensor,
    image_points: torch.Tensor,
    image_size: tuple[int, int],
    repo_erro_threshold: float = 1e-1,
) -> tuple[torch.Tensor, torch.Tensor, float]:
    """From a set of world points and image points, solve for the camera matrix and distortion coefficients.
    Note: All world points must have the same z value. i.e lie on the same plane.

    Args:
        world_points (torch.Tensor): Tensor of world points.
        image_points (torch.Tensor): Tensor of image points.
        image_size (tuple[int, int]): Image dimensions as (Width, Height).
        repo_error (float, optional): Reprojection error measured in pixels. Defaults to 1e-1.

    Returns (Tuple[torch.Tensor, torch.Tensor, float]): camera_matrix (3x3), dist_coeffs (1x5), repo_erro

    """
    if len(world_points.shape) == 3:
        _world_points = [world_points.squeeze(0).numpy().astype(np.float32)]
    elif len(world_points.shape) == 2:
        _world_points = [world_points.numpy().astype(np.float32)]
    else:
        raise RuntimeError(f"{world_points.shape=} must be of length 2 or 3.")
    if len(image_points.shape) == 3:
        _image_points = [image_points.squeeze(0).numpy().astype(np.float32)]
    elif len(image_points.shape) == 2:
        _image_points = [image_points.numpy().astype(np.float32)]
    else:
        raise RuntimeError(f"{image_points.shape=} must be of length 2 or 3.")

    # if not all(o[-1] == _world_points[0][0][-1] for o in _world_points[0]):
    # raise RuntimeError(f"{_world_points=} must have same z value")
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1000, 0.001)

    repo_erro, camera_matrix, dist_coeffs, *_ = cv2.calibrateCamera(
        objectPoints=_world_points,
        imagePoints=_image_points,
        imageSize=image_size,
        cameraMatrix=None,
        distCoeffs=None,
        criteria=criteria,
    )
    if repo_erro > repo_erro_threshold:
        raise RuntimeError(f"{repo_erro=} must be less than 1e-6")
    print(f"{repo_erro=}")
    return camera_matrix, dist_coeffs, repo_erro