# Source code for kornia.geometry.calibration.pnp

```
from typing import Optional, Tuple
import torch
from kornia.geometry.conversions import convert_points_to_homogeneous
from kornia.geometry.linalg import transform_points
from kornia.utils import eye_like
from kornia.utils._compat import linalg_qr
def _mean_isotropic_scale_normalize(
points: torch.Tensor, eps: float = 1e-8
) -> Tuple[torch.Tensor, torch.Tensor]:
r"""Normalizes points.
Args:
points : Tensor containing the points to be normalized with shape :math:`(B, N, D)`.
eps : Small value to avoid division by zero error.
Returns:
Tuple containing the normalized points in the shape :math:`(B, N, D)` and the transformation matrix
in the shape :math:`(B, D+1, D+1)`.
"""
if not isinstance(points, torch.Tensor):
raise AssertionError(f"points is not an instance of torch.Tensor. Type of points is {type(points)}")
if len(points.shape) != 3:
raise AssertionError(f"points must be of shape (B, N, D). Got shape {points.shape}.")
x_mean = torch.mean(points, dim=1, keepdim=True) # Bx1xD
scale = (points - x_mean).norm(dim=-1, p=2).mean(dim=-1) # B
D_int = points.shape[-1]
D_float = torch.tensor(points.shape[-1], dtype=torch.float64, device=points.device)
scale = torch.sqrt(D_float) / (scale + eps) # B
transform = eye_like(D_int + 1, points) # (B, D+1, D+1)
idxs = torch.arange(D_int, dtype=torch.int64, device=points.device)
transform[:, idxs, idxs] = transform[:, idxs, idxs] * scale[:, None]
transform[:, idxs, D_int] = transform[:, idxs, D_int] + (-scale[:, None] * x_mean[:, 0, idxs])
points_norm = transform_points(transform, points) # BxNxD
return (points_norm, transform)
[docs]def solve_pnp_dlt(
world_points: torch.Tensor, img_points: torch.Tensor,
intrinsics: torch.Tensor, weights: Optional[torch.Tensor] = None,
svd_eps: float = 1e-4,
) -> torch.Tensor:
r"""This function attempts to solve the Perspective-n-Point (PnP)
problem using Direct Linear Transform (DLT).
Given a batch (where batch size is :math:`B`) of :math:`N` 3D points
(where :math:`N \geq 6`) in the world space, a batch of :math:`N`
corresponding 2D points in the image space and a batch of
intrinsic matrices, this function tries to estimate a batch of
world to camera transformation matrices.
This implementation needs at least 6 points (i.e. :math:`N \geq 6`) to
provide solutions.
This function cannot be used if all the 3D world points (of any element
of the batch) lie on a line or if all the 3D world points (of any element
of the batch) lie on a plane. This function attempts to check for these
conditions and throws an AssertionError if found. Do note that this check
is sensitive to the value of the svd_eps parameter.
Another bad condition occurs when the camera and the points lie on a
twisted cubic. However, this function does not check for this condition.
Args:
world_points : A tensor with shape :math:`(B, N, 3)` representing
the points in the world space.
img_points : A tensor with shape :math:`(B, N, 2)` representing
the points in the image space.
intrinsics : A tensor with shape :math:`(B, 3, 3)` representing
the intrinsic matrices.
weights : This parameter is not used currently and is just a
placeholder for API consistency.
svd_eps : A small float value to avoid numerical precision issues.
Returns:
A tensor with shape :math:`(B, 3, 4)` representing the estimated world to
camera transformation matrices (also known as the extrinsic matrices).
Example:
>>> world_points = torch.tensor([[
... [ 5. , -5. , 0. ], [ 0. , 0. , 1.5],
... [ 2.5, 3. , 6. ], [ 9. , -2. , 3. ],
... [-4. , 5. , 2. ], [-5. , 5. , 1. ],
... ]], dtype=torch.float64)
>>>
>>> img_points = torch.tensor([[
... [1409.1504, -800.936 ], [ 407.0207, -182.1229],
... [ 392.7021, 177.9428], [1016.838 , -2.9416],
... [ -63.1116, 142.9204], [-219.3874, 99.666 ],
... ]], dtype=torch.float64)
>>>
>>> intrinsics = torch.tensor([[
... [ 500., 0., 250.],
... [ 0., 500., 250.],
... [ 0., 0., 1.],
... ]], dtype=torch.float64)
>>>
>>> print(world_points.shape, img_points.shape, intrinsics.shape)
torch.Size([1, 6, 3]) torch.Size([1, 6, 2]) torch.Size([1, 3, 3])
>>>
>>> pred_world_to_cam = kornia.geometry.solve_pnp_dlt(world_points, img_points, intrinsics)
>>>
>>> print(pred_world_to_cam.shape)
torch.Size([1, 3, 4])
>>>
>>> pred_world_to_cam
tensor([[[ 0.9392, -0.3432, -0.0130, 1.6734],
[ 0.3390, 0.9324, -0.1254, -4.3634],
[ 0.0552, 0.1134, 0.9920, 3.7785]]], dtype=torch.float64)
"""
# This function was implemented based on ideas inspired from multiple references.
# ============
# References:
# ============
# 1. https://team.inria.fr/lagadic/camera_localization/tutorial-pose-dlt-opencv.html
# 2. https://github.com/opencv/opencv/blob/68d15fc62edad980f1ffa15ee478438335f39cc3/modules/calib3d/src/calibration.cpp # noqa: E501
# 3. http://rpg.ifi.uzh.ch/docs/teaching/2020/03_camera_calibration.pdf
# 4. http://www.cs.cmu.edu/~16385/s17/Slides/11.3_Pose_Estimation.pdf
# 5. https://www.ece.mcmaster.ca/~shirani/vision/hartley_ch7.pdf
if not isinstance(world_points, torch.Tensor):
raise AssertionError(
f"world_points is not an instance of torch.Tensor. Type of world_points is {type(world_points)}"
)
if not isinstance(img_points, torch.Tensor):
raise AssertionError(
f"img_points is not an instance of torch.Tensor. Type of img_points is {type(img_points)}"
)
if not isinstance(intrinsics, torch.Tensor):
raise AssertionError(
f"intrinsics is not an instance of torch.Tensor. Type of intrinsics is {type(intrinsics)}"
)
if (weights is not None) and (not isinstance(weights, torch.Tensor)):
raise AssertionError(
f"If weights is not None, then weights should be an instance "
f"of torch.Tensor. Type of weights is {type(weights)}"
)
if type(svd_eps) is not float:
raise AssertionError(f"Type of svd_eps is not float. Got {type(svd_eps)}")
accepted_dtypes = (torch.float32, torch.float64)
if world_points.dtype not in accepted_dtypes:
raise AssertionError(
f"world_points must have one of the following dtypes {accepted_dtypes}. "
f"Currently it has {world_points.dtype}."
)
if img_points.dtype not in accepted_dtypes:
raise AssertionError(
f"img_points must have one of the following dtypes {accepted_dtypes}. "
f"Currently it has {img_points.dtype}."
)
if intrinsics.dtype not in accepted_dtypes:
raise AssertionError(
f"intrinsics must have one of the following dtypes {accepted_dtypes}. "
f"Currently it has {intrinsics.dtype}."
)
if (len(world_points.shape) != 3) or (world_points.shape[2] != 3):
raise AssertionError(
f"world_points must be of shape (B, N, 3). Got shape {world_points.shape}."
)
if (len(img_points.shape) != 3) or (img_points.shape[2] != 2):
raise AssertionError(
f"img_points must be of shape (B, N, 2). Got shape {img_points.shape}."
)
if (len(intrinsics.shape) != 3) or (intrinsics.shape[1:] != (3, 3)):
raise AssertionError(
f"intrinsics must be of shape (B, 3, 3). Got shape {intrinsics.shape}."
)
if world_points.shape[1] != img_points.shape[1]:
raise AssertionError("world_points and img_points must have equal number of points.")
if (world_points.shape[0] != img_points.shape[0]) or (world_points.shape[0] != intrinsics.shape[0]):
raise AssertionError("world_points, img_points and intrinsics must have the same batch size.")
if world_points.shape[1] < 6:
raise AssertionError(
f"At least 6 points are required to use this function. "
f"Got {world_points.shape[1]} points."
)
B, N = world_points.shape[:2]
# Getting normalized world points.
world_points_norm, world_transform_norm = _mean_isotropic_scale_normalize(world_points)
# Checking if world_points_norm (of any element of the batch) has rank = 3. This
# function cannot be used if all world points (of any element of the batch) lie
# on a line or if all world points (of any element of the batch) lie on a plane.
_, s, _ = torch.svd(world_points_norm)
if torch.any(s[:, -1] < svd_eps):
raise AssertionError(
f"The last singular value of one/more of the elements of the batch is smaller "
f"than {svd_eps}. This function cannot be used if all world_points (of any "
f"element of the batch) lie on a line or if all world_points (of any "
f"element of the batch) lie on a plane."
)
intrinsics_inv = torch.inverse(intrinsics)
world_points_norm_h = convert_points_to_homogeneous(world_points_norm)
# Transforming img_points with intrinsics_inv to get img_points_inv
img_points_inv = transform_points(intrinsics_inv, img_points)
# Normalizing img_points_inv
img_points_norm, img_transform_norm = _mean_isotropic_scale_normalize(img_points_inv)
inv_img_transform_norm = torch.inverse(img_transform_norm)
# Setting up the system (the matrix A in Ax=0)
system = torch.zeros((B, 2 * N, 12), dtype=world_points.dtype, device=world_points.device)
system[:, 0::2, 0:4] = world_points_norm_h
system[:, 1::2, 4:8] = world_points_norm_h
system[:, 0::2, 8:12] = world_points_norm_h * (-1) * img_points_norm[..., 0:1]
system[:, 1::2, 8:12] = world_points_norm_h * (-1) * img_points_norm[..., 1:2]
# Getting the solution vectors.
_, _, v = torch.svd(system)
solution = v[..., -1]
# Reshaping the solution vectors to the correct shape.
solution = solution.reshape(B, 3, 4)
# Creating solution_4x4
solution_4x4 = eye_like(4, solution)
solution_4x4[:, :3, :] = solution
# De-normalizing the solution
intermediate = torch.bmm(solution_4x4, world_transform_norm)
solution = torch.bmm(inv_img_transform_norm, intermediate[:, :3, :])
# We obtained one solution for each element of the batch. We may
# need to multiply each solution with a scalar. This is because
# if x is a solution to Ax=0, then cx is also a solution. We can
# find the required scalars by using the properties of
# rotation matrices. We do this in two parts:
# First, we fix the sign by making sure that the determinant of
# the all the rotation matrices are non negative (since determinant
# of a rotation matrix should be 1).
det = torch.det(solution[:, :3, :3])
ones = torch.ones_like(det)
sign_fix = torch.where(det < 0, ones * -1, ones)
solution = solution * sign_fix[:, None, None]
# Then, we make sure that norm of the 0th columns of the rotation
# matrices are 1. Do note that the norm of any column of a rotation
# matrix should be 1. Here we use the 0th column to calculate norm_col.
# We then multiply solution with mul_factor.
norm_col = torch.norm(input=solution[:, :3, 0], p=2, dim=1)
mul_factor = (1 / norm_col)[:, None, None]
temp = solution * mul_factor
# To make sure that the rotation matrix would be orthogonal, we apply
# QR decomposition.
ortho, right = linalg_qr(temp[:, :3, :3])
# We may need to fix the signs of the columns of the ortho matrix.
# If right[i, j, j] is negative, then we need to flip the signs of
# the column ortho[i, :, j]. The below code performs the necessary
# operations in an better way.
mask = eye_like(3, ortho)
col_sign_fix = torch.sign(mask * right)
rot_mat = torch.bmm(ortho, col_sign_fix)
# Preparing the final output.
pred_world_to_cam = torch.cat([rot_mat, temp[:, :3, 3:4]], dim=-1)
# TODO: Implement algorithm to refine the solution.
return pred_world_to_cam
```