def sample_plane(plane, n, width, noise = 0): plane_repok(plane) e1, e2, _ = np.eye(3) v1 = e1 if (plane[0] == 0 and plane[1] == 0) else ut.normalized(np.array([-plane[1], plane[0], 0], 'd')) v2 = ut.normalized(np.cross(plane[:3], v1)) #print 'dot', np.dot(v1, plane[:3]), np.dot(v2, plane[:3]) #print 'sample', np.sum(np.dot(np.array([v1, v2]).T, np.random.uniform(-width/2, width/2, (2, n))).T * plane[:3], 1) center = -plane[3]*plane[:3] #print 'dot2', np.dot(center, plane[:3]) + plane[3], plane pts = np.dot(np.array([v1, v2]).T, np.random.uniform(-width/2, width/2, (2, n))).T + center #print 'ins', len(plane_inliers(plane, pts, 0.05)) pts += np.random.randn(*pts.shape)*noise return pts
def parallel_axes(plane):
x, y, z = plane[:3]
a = np.array
if x == 0:
u = a([0., z, -y])
elif y == 0:
u = a([z, 0., -x])
else:
u = a([-y, x, 0.])
v = ut.normalized(np.cross(plane[:3], u))
return a([u, v])
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 pixel_ray(P, x, y):
K, R, t = decompose_P(P)
return np.dot(
R.T,
ut.normalized(
np.array([(x - K[0, 2]) / K[0, 0], (y - K[1, 2]) / K[1, 1], 1.0])))
def plane_from_3(pts): assert pts.shape == (3, 3) w = ut.normalized(np.cross(pts[1] - pts[0], pts[2] - pts[0])) return np.array(list(w) + [-np.dot(w, pts[0])])