def pnp_va(pts_2d, pts_3d, K, eps=1e-9, max_iters=2500, verbose=False):
"""Compute object poses from point 2D-3D correspondences.
Variant A:
- The redundant row orthonormality constained is removed.
Arguments:
pts_2d -- n x 2 np.array of 2D pixels
pts_3d -- n x 3 np.array of 3D points
K -- 3 x 3 np.array with the camera intrinsics
eps -- numerical precision of the solver
max_iters -- maximum number of iterations the solver is allowed to perform
verbose -- print additional solver information to the console
"""
# Extract point constraints
(C1, C2, C3), (N1, N2, N3) = _point_constraints(pts_2d, pts_3d, K)
# Compose block matrices
C = np.vstack((C1, C2, C3))
N = np.vstack((N1, N2, N3))
B = np.linalg.solve(N.T @ N, N.T) @ C
A = C - N @ B
# Solve the QCQP using shor's relaxation
return _solve_relaxation_va(A,
B,
eps=eps,
max_iters=max_iters,
verbose=verbose)
def pnp_null(pts_2d, pts_3d, K):
"""Compute object poses from point 2D-3D correspondences.
Variant A:
- The redundant row orthonormality constained is removed.
Arguments:
pts_2d -- n x 2 np.array of 2D pixels
pts_3d -- n x 3 np.array of 3D points
K -- 3 x 3 np.array with the camera intrinsics
eps -- numerical precision of the solver
max_iters -- maximum number of iterations the solver is allowed to perform
verbose -- print additional solver information to the console
"""
# Extract point constraints
(C1, C2, C3), (N1, N2, N3) = _point_constraints(pts_2d, pts_3d, K)
# Compose block matrices
C = np.vstack((C1, C2, C3))
N = np.vstack((N1, N2, N3))
B = np.linalg.solve(N.T @ N, N.T) @ C
A = C - N @ B
# Pick the smallest singular vector
R = np.linalg.svd(A)[2][-1].reshape((3, 3)).T
# Project to the orthogonal space
U, _, Vt = np.linalg.svd(R)
R = U @ Vt
R *= np.sign(np.linalg.det(R))
t = -B @ R.ravel("F")
return [(R, t)]
def pnpl_va(pts_2d,
line_2d,
pts_3d,
line_3d,
K,
eps=1e-9,
max_iters=2500,
verbose=False):
"""Compute object poses from point and line 2D-3D correspondences.
Variant A:
- The redundant row orthonormality constained is removed.
Arguments:
pts_2d -- n x 2 np.array of 2D pixels
line_2d -- n x 2 x 2 np.array organized as (line, pt, dim). Each line is defined
by sampling 2 points from it. Each point is a pixel in 2D.
pts_3d -- n x 3 np.array of 3D points
line_3d -- A n x 2 x 3 np.array organized as (line, pt, dim). Each line is defined
by 2 points. The points reside in 3D.
K -- 3 x 3 np.array with the camera intrinsics.
eps -- numerical precision of the solver
max_iters -- maximum number of iterations the solver is allowed to perform
verbose -- print additional solver information to the console
"""
# Extract point constraints
(Cp1, Cp2, Cp3), (Np1, Np2,
Np3) = _point_constraints(pts_2d=pts_2d.reshape((-1, 2)),
pts_3d=pts_3d.reshape((-1, 3)),
K=K)
# Extract line constraints
Cl, Nl = _line_constraints(line_2d.reshape((-1, 2, 2)), line_3d, K)
# Compose block matrices
C = np.vstack((Cp1, Cp2, Cp3, Cl))
N = np.vstack((Np1, Np2, Np3, Nl))
# Compose block matrices
B = np.linalg.solve(N.T @ N, N.T @ C)
A = C - N @ B
# Solve the QCQP using shor's relaxation
return _solve_relaxation_va(A,
B,
eps=eps,
max_iters=max_iters,
verbose=verbose)