def estimate_aff_hom(cams, vps):
# compute 3D Vanishing points
V = rc.estimate_3d_points(cams[0], cams[1], vps[0].T, vps[1].T)
# compute plane at infinity
p = mth.nullspace(V.T)
p = p / p[3, :]
# compute homography
aff_hom = np.r_[np.c_[np.eye(3), np.zeros(3)], p.T]
return aff_hom
def compute_proj_camera(F, i):
# Result 9.15 of MVG (v = 0, lambda = 1). It assumes P1 = [I|0]
# compute epipole e'
e = mth.nullspace(F.T)
# build [e']_x
ske = mth.hat_operator(e)
# compute P
P = np.concatenate((ske @ F, e), axis=1)
return P
def estimate_euc_hom(cams, vps):
def _eqn(vp1, vp2):
r = np.array([vp1[0] * vp2[0],
vp1[0] * vp2[1] + vp1[1] * vp2[0],
vp1[0] * vp2[2] + vp1[2] * vp2[0],
vp1[1] * vp2[1],
vp1[1] * vp2[2] + vp1[2] * vp2[1],
vp1[2] * vp2[2]])
return r
# make points homogeneous
vps_h = fd.make_homogeneous(vps)
# combine constraints to create a system of equations A_sys
vp1, vp2, vp3 = vps_h
eqn1 = _eqn(vp1, vp2)
eqn2 = _eqn(vp1, vp3)
eqn3 = _eqn(vp2, vp3)
A_sys = np.array([[*eqn1],
[*eqn2],
[*eqn3],
[0, 1, 0, 0, 0, 0],
[1, 0, 0, -1, 0, 0]])
# compute w_v as the null vector of A_sys
w_v = mth.nullspace(A_sys)
w_v = np.squeeze(w_v)
# create matrix w from vector w_v
w = np.array([[w_v[0], w_v[1], w_v[2]],
[w_v[1], w_v[3], w_v[4]],
[w_v[2], w_v[4], w_v[5]]])
# compute A
M = cams[:, :3]
A = scipy.linalg.cholesky(LA.inv(M.T @ w @ M), lower=False)
# create the corresponding homography using inv(A)
invA = LA.inv(A)
euc_hom = np.array([[invA[0][0], invA[0][1], invA[0][2], 0],
[invA[1][0], invA[1][1], invA[1][2], 0],
[invA[2][0], invA[2][1], invA[2][2], 0],
[0, 0, 0, 1]])
return euc_hom
def compute_proj_camera(F, i):
# Result 9.15 of MVG (v = 0, lambda = 1). It assumes P1 = [I|0]
# P' = [S @ F | e']
# A good choice is: S = [e']x .
# The epipole may be computed from e'.T @ F = 0
# compute the epipole
e = mth.nullspace(F.T, atol=1e-13, rtol=0)
# obtain S
S = mth.hat_operator(e)
# compute P
P = np.column_stack((S@F, e))
return P
def estimate_aff_hom(cams, vps):
"""
MVG: 10.4 Stratified reconstruction, Parallel lines (pages 269-270)
MVG: 10.4.1 The step to affine reconstruction (page 268)
"""
# compute 3D Vanishing points
vp3d = rc.estimate_3d_points_2(cams[0], cams[1], vps[0].T, vps[1].T)
# compute plane at infinity
pinf = mth.nullspace(vp3d.T)
pinf = pinf/pinf[-1,:]
# create affine homography using the plane at infinity
aff_hom = np.row_stack((np.eye(3,4), pinf.T))
return aff_hom
def estimate_euc_hom(cams, vps):
# make points homogeneous
vpsh = fd.make_homogeneous(vps)
# build A
u, v, z = vpsh
A = np.array([[
u[0] * v[0], u[0] * v[1] + u[1] * v[0], u[0] * v[2] + u[2] * v[0],
u[1] * v[1], u[1] * v[2] + u[2] * v[1], u[2] * v[2]
],
[
u[0] * z[0], u[0] * z[1] + u[1] * z[0],
u[0] * z[2] + u[2] * z[0], u[1] * z[1],
u[1] * z[2] + u[2] * z[1], u[2] * z[2]
],
[
v[0] * z[0], v[0] * z[1] + v[1] * z[0],
v[0] * z[2] + v[2] * z[0], v[1] * z[1],
v[1] * z[2] + v[2] * z[1], v[2] * z[2]
], [0, 1, 0, 0, 0, 0], [1, 0, 0, -1, 0, 0]])
# find w_v
w_v = mth.nullspace(A)
w_v = np.squeeze(w_v)
# build w
w = np.array([[w_v[0], w_v[1], w_v[2]], [w_v[1], w_v[3], w_v[4]],
[w_v[2], w_v[4], w_v[5]]])
# obtain A
M = cams[:, :3]
A = scipy.linalg.cholesky(np.linalg.inv(M.T @ w @ M), lower=False)
# build euc_hom
euc_hom = np.r_[np.c_[np.linalg.inv(A), np.zeros(3)],
np.array([[0, 0, 0, 1]])]
return euc_hom