def F_test_match2(F, f1, f2, thresh=2.):
""" from PMVS """
line = np.dot(F, ut.homog(f1))
f = (line[0]**2 + line[1]**2)**0.5
if f == 0:
return 0.
line /= f
return np.abs(np.dot(line, ut.homog(f2))) <= thresh
def brute_force_triangulate(Ps,
pt_projs,
min_dist,
max_dist,
sample_step=0.01):
assert len(Ps) == 2
pt_projs = map(np.asarray, pt_projs)
P1, P2 = Ps
K1, R1, t1 = decompose_P(P1)
c1 = center_from_P(P1)
D1 = pixel_ray_matrix(P1)
vd1 = np.dot(D1, ut.homog(np.array(pt_projs[0]).T))
vd1 = vd1 / np.sqrt(np.sum(vd1**2, axis=0))
best_err = np.inf + np.zeros(pt_projs[0].shape[0])
best_dist = np.inf + np.zeros(pt_projs[0].shape[0])
dists = np.arange(min_dist, max_dist, sample_step)
for d in dists:
X = c1[:, np.newaxis] + d * vd1
proj = ut.homog_transform(P2, X)
err = np.sum((proj.T - pt_projs[1])**2, axis=1)
good = (err < best_err)
best_err[good] = err[good]
best_dist[good] = d
return c1 + (best_dist * vd1).T
def F_from_Ps(P1, P2, cam2_center=None):
if cam2_center is None:
cam2_center = center_from_P(P2)
e = np.dot(P1, ut.homog(cam2_center))
F = np.dot(np.dot(np.linalg.pinv(P2.T), P1.T), cross_product_matrix(e))
F = F / F[2, 2]
return F
def precondition_Ps(Ps, projs, im_shapes):
assert len(Ps) == len(projs) == len(im_shapes)
assert all([len(projs[0]) == len(projs[i]) for i in xrange(len(projs))])
new_Ps = []
new_projs = []
for P, xs, sz in itl.izip(Ps, projs, im_shapes):
H = norm_homog(sz[1], sz[0])
new_P = np.dot(H, P)
new_Ps.append(new_P)
new_projs.append([ut.inhomog(np.dot(H, ut.homog(pt))) for pt in xs])
return new_Ps, new_projs
def F_test_matches(F, f1, f2, thresh=2.):
lines = F.dot(ut.homog(f1.T)).T
lines /= ut.normax_padzero(lines[:, :2], 1)[:, np.newaxis]
ok = np.abs(np.sum(lines * ut.homog(f2.T).T, 1)) <= thresh
#assert np.all(ok == np.array([F_test_match(F, x1, x2) for x1, x2 in zip(f1, f2)]))
return ok
