def find_H_bruteforce(P1, P2, plane):
pts = planefit.sample_plane(plane, 100, 1., noise=0)
proj1 = ut.homog_transform(P1, pts.T).T
proj2 = ut.homog_transform(P2, pts.T).T
H, ins = cv2.findHomography(proj1, proj2)
assert np.all(ins == 1)
assert np.allclose(ut.homog_transform(H, proj1.T).T, proj2)
return H
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 resid(X):
errs = np.zeros(2 * len(Ps))
for i in xrange(len(Ps)):
x = ut.homog_transform(Ps[i], X)
errs[0 + 2 * i] = x[0] - projs[i, 0]
errs[1 + 2 * i] = x[1] - projs[i, 1]
return errs
def test_plane_H():
if 1:
npts = 20
n = ut.normalized(np.array([1., 1, -1]))
d = 2.
X = pylab.randn(3, npts)
# X[:2,:]*n[:2] + d + n[2]*X[2] = 0 ==> X[2] = (-d - X[:2,:]*n[:2])/n[2]
X[2] = (-d - np.sum(X[:2] * n[:2, np.newaxis], axis=0)) / n[2]
assert np.all(np.abs(np.dot(X.T, n) + d) <= 0.001)
K1 = np.eye(3)
P1 = compose_P(K1, np.eye(3), np.zeros(3))
x1 = ut.homog_transform(P1, X)
K2 = 2 * np.eye(3)
R2 = np.eye(3)
t2 = np.array([0.5, 0, 0])
P2 = compose_P(K2, R2, t2)
x2 = ut.homog_transform(P2, X)
H = plane_H(n, d, K1, K2, R2, t2)
x2_est = ut.homog_transform(H, x1)
assert ut.a_eq(x2, x2_est)
if 1:
n = np.array([-0.09576725, -0.02749329, -0.995024])
d = 12.842613230422947
X = pylab.randn(3, npts)
X[2] = (-d - np.sum(X[:2] * n[:2, np.newaxis], axis=0)) / n[2]
# P1
K1 = np.array([[184.46153519, 0., 320.5], [0., -184.46153519, 240.5],
[0., 0., 1.]])
P1 = compose_P(K1, np.eye(3), np.zeros(3))
x1 = ut.homog_transform(P1, X)
# P2
K2 = np.array([[184.46153519, 0., 320.5], [0., -184.46153519, 240.5],
[0., 0., 1.]])
R2 = np.array([[0.99540027, -0.00263395, 0.09576725],
[0., 0.99962199, 0.02749329],
[-0.09580347, -0.02736683, 0.995024]])
t2 = np.array([-3.42297712, 6.86145016, -1.94439297])
P2 = compose_P(K2, R2, t2)
x2 = ut.homog_transform(P2, X)
H = plane_H(n, d, K1, K2, R2, t2)
x2_est = ut.homog_transform(H, x1)
assert ut.a_eq(x2, x2_est)
def proj_dist(P, X, x):
return pylab.dist(ut.homog_transform(P, X), x)
def reproj_error(P, X, x):
d = ut.homog_transform(P, X) - x
return np.dot(d, d)