def cv_triangulate(Ps, pt_projs, im_shape=None):
assert len(Ps) == 2
from ext import cv2
Xs = cv2.triangulatePoints(np.array(Ps[0], 'd'), np.array(Ps[1], 'd'),
np.array(pt_projs[0], 'd').T,
np.array(pt_projs[1], 'd').T)
return ut.inhomog(Xs).T
def triangulate_linear(Ps, projs, im_shapes):
Ps, projs = precondition_Ps(Ps, projs, im_shapes)
X = []
for i in xrange(len(projs[0])):
A = []
for P, P_xs in itl.izip(Ps, projs):
x = P_xs[i]
A.append(-x[0] * P[2, :] + P[0, :])
A.append(x[1] * P[2, :] - P[1, :])
X.append(ut.inhomog(solve_0_system(np.vstack(A))))
return np.array(X)
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 pix_from_kinect(pt):
x, y = ut.inhomog(camera_matrix().dot(pt))
# correct for larger width of rgb image
#x = (x - 724./2) + 1066/2.
#return x, y
#x = (x - 724./2) + 1066/2.
dx = 1.45779823139
dy = 1.43586159674
tx = 134.330831472
ty = -44.8725110988
return x * dx + tx, y * dy + ty
def center_from_P(P):
return ut.inhomog(solve_0_system(P))
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()