def test(data_file='/media/ao/hd2/gelsight-data/kinect_data.hdf5'):
depths = []
with h5py.File(data_file, 'r') as f:
for frame in [0, 100, 500, 750, 2000]:
dset = f['step_%d' % frame]
depth = dset['camera_depth'][:, :, 0]
depths.append(np.array(depth, 'float32'))
for i in xrange(1, len(depths)):
fit_cylinder(depths[0], depths[i])
ut.toplevel_locals()
def test_plane_fitting(): noise = 0.0 plane1 = np.array([0., 1./2**0.5, 1/2**0.5, 1.]) plane2 = np.array([1., 0., 0., 5.]) pts1 = sample_plane(plane1, 100, 1, noise) pts2 = sample_plane(plane2, 20, 1, noise) pts = np.vstack([pts1, pts2]) plane, _ = fit_plane_ransac(pts, 0.05) #plane = fit_plane(pts1) print plane, 'should be', np.array([0., 1., 1., 1.]) true_ins = plane_inliers(plane, pts, 0.05) print 'ninliers', len(true_ins), 'should be', len(plane_inliers(plane1, pts, 0.05)), 'other hypothesis', len(plane_inliers(plane2, pts, 0.05)) ut.toplevel_locals()
def reencode_subbands(snd,
nbands,
env_sr,
matlab_encode=False,
matlab_decode=False,
do_decode=True):
if matlab_encode:
envs = soundrep.matlab_subband(snd, env_sr)
else:
envs = subband_envs(snd, nbands, env_sr)
if do_decode:
ut.toplevel_locals()
if matlab_decode:
return sound.Sound(
None, snd.rate,
soundrep.matlab_inv_subband(envs,
snd.rate,
env_sr,
nbands=nbands))
return invert_subband_envs(envs, snd.rate, env_sr)
def fit_cylinder(depth0,
depth,
plane_thresh=0.02,
new_thresh=0.02,
inlier_frac=0.9925,
show=True):
# compute occupancy map for the first frame (should precompute when using many frames...)
plane = fit_ground_plane(depth0)
ptm0 = ptm_from_depth(depth0)
pts0 = pts_from_ptm(ptm0)
on_plane0 = planefit.dist_from_plane_ptm(ptm0, plane) < plane_thresh
on_plane0 = in_largest_cc(on_plane0)
proj_plane0 = planefit.project_onto_plane(plane, ok_pts(ptm0[on_plane0]))
ptm = ptm_from_depth(depth)
has_pt = ptm[:, :, 3] != 0
# find points that are very close to the table's surface
pts, inds = pts_from_ptm(ptm, inds=True)
proj_plane = planefit.project_onto_plane(plane, pts)
ok = ut.knnsearch(proj_plane0, proj_plane,
method='kd')[0].flatten() <= 0.005
near_surface = np.zeros(ptm.shape[:2], 'bool')
near_surface[inds[0][ok], inds[1][ok]] = True
# find points that are not in the original point cloud
dist = ut.knnsearch(pts0, pts, method='kd')[0].flatten()
new_pt = np.zeros_like(near_surface)
ok = dist > new_thresh
new_pt[inds[0][ok], inds[1][ok]] = True
# todo: probably want to filter out the robot's gripper, e.g. with a height check
occ_before_cc = new_pt & near_surface & has_pt
occ = in_largest_cc(occ_before_cc)
# segment object and find height
pts = ptm[occ]
pts = pts[pts[:, 3] > 0, :3]
height = np.percentile(np.abs(planefit.dist_to_plane(plane, pts)), 99.7)
print 'Height: %.3fm' % height
# find an ellipse that covers the occupied points
# todo: want to deal with self-occlusion here (kinect won't see the backside of the object)
pts = ok_pts(ptm[occ])
proj = planefit.project_onto_plane(plane, pts)
# add a slight bias toward choosing farther z points, since these will often be self-occluded
#center = np.array(list(np.median(proj[:, :2], axis = 0)) + [np.percentile(proj[:, 2], 70.)])
center = np.array(
list(np.median(proj[:, :2], axis=0)) +
[np.percentile(proj[:, 2], 75.)])
d = np.sqrt(np.percentile(np.sum((proj - center)**2, 1), 95.)) + 0.01
scales = np.array([d, d])
# show ellipse
if show:
# show points in/out of cylinder
#nrm_vis = color_normals(ptm)
ptm_vis = ut.clip_rescale_im(ptm[:, :, 2], 0., 2.)
oval_vis1 = ptm_vis.copy()
oval_missing_vis = ptm_vis.copy()
for y in xrange(ptm.shape[0]):
for x in xrange(ptm.shape[1]):
if ptm[y, x, 3]:
pt = ptm[y, x, :3]
proj = (planefit.project_onto_plane(plane, pt[None]) -
center)[0]
ok = ((proj[:2] / scales[:2])**2).sum() <= 1.
ok = ok and planefit.dist_to_plane(
plane, pt[None], signed=True)[0] <= height
if ok and occ[y, x]:
oval_vis1[y, x] = 255
elif occ[y, x]:
# not covered
oval_missing_vis[y, x] = 255
# show cylinder
axes = parallel_axes(plane)
pts_lo, pts_hi = [], []
for theta in np.linspace(0., 2 * np.pi, 100):
x = np.array([np.cos(theta), np.sin(theta)])
pt = axes.T.dot(x * scales[:2]) + center
pts_lo.append(pt.flatten())
pts_hi.append(pt + height * plane[:3])
print plane[:3]
pts_lo, pts_hi = map(np.array, [pts_lo, pts_hi])
proj_lo = ut.inhomog(camera_matrix().dot(pts_lo.T)).T
proj_hi = ut.inhomog(camera_matrix().dot(pts_hi.T)).T
oval_vis = ptm_vis.copy()
c1 = (128, 0, 0)
c2 = (0, 0, 128)
c3 = (0, 128, 0)
oval_vis = ig.draw_lines(oval_vis,
proj_lo[:-1],
proj_lo[1:],
colors=c1,
width=2)
oval_vis = ig.draw_lines(oval_vis,
proj_hi[:-1],
proj_hi[1:],
colors=c2,
width=2)
oval_vis = ig.draw_lines(oval_vis, [proj_hi[0]], [proj_lo[0]],
colors=c3,
width=2)
oval_vis = ig.draw_lines(oval_vis, [proj_hi[len(proj_hi) / 2]],
[proj_lo[len(proj_hi) / 2]],
colors=c3,
width=2)
def make_vis(x):
v = ptm_vis.copy()
v[x] = 255
return np.flipud(v)
ig.show([
'parametric ellipse:',
np.flipud(oval_vis),
'ellipse:',
np.flipud(oval_vis1),
'missing:',
np.flipud(oval_missing_vis),
'occ:',
make_vis(occ),
'occ_before_cc:',
make_vis(occ_before_cc),
'near_surface',
make_vis(near_surface),
'new_pt',
make_vis(new_pt),
'has_pt',
make_vis(has_pt),
'on_plane0',
make_vis(on_plane0),
'input:',
np.flipud(ptm_vis),
])
ut.toplevel_locals()