def create_grid(brf,tlb,resolution,pos_list,scan_list,l1,l2,
display_flag=False,show_pts=True,rotation_angle=0.,
occupancy_threshold=1):
''' rotation angle - about the Z axis.
'''
max_dist = np.linalg.norm(tlb) + 0.2
min_dist = brf[0,0]
min_angle,max_angle=math.radians(-60),math.radians(60)
all_pts = generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2,
max_dist=max_dist,min_dist=min_dist)
rot_mat = tr.Rz(rotation_angle)
all_pts_rot = rot_mat*all_pts
gr = og3d.occupancy_grid_3d(brf,tlb,resolution,rotation_z=rotation_angle)
gr.fill_grid(all_pts_rot)
gr.to_binary(occupancy_threshold)
if display_flag == True:
if show_pts:
d3m.plot_points(all_pts,color=(0.,0.,0.))
cube_tups = gr.grid_lines(rotation_angle=rotation_angle)
d3m.plot_cuboid(cube_tups)
return gr
def test_plane_finding():
''' visualize plane finding.
'''
# brf = pt + np.matrix([-0.4,-0.2,-0.3]).T
# brf[0,0] = max(brf[0,0],0.05)
# print 'brf:', brf.T
#
# tlb = pt + np.matrix([0.3, 0.2,0.3]).T
# resolution = np.matrix([0.01,0.01,0.0025]).T
brf = pt + np.matrix([-0.15, -0.25, -0.2]).T
brf[0, 0] = max(0.07, brf[0, 0])
tlb = pt + np.matrix([0.25, 0.25, 0.2]).T
resolution = np.matrix([0.01, 0.01, 0.0025]).T
max_dist = np.linalg.norm(tlb) + 0.2
min_dist = brf[0, 0]
all_pts = generate_pointcloud(pos_list,
scan_list,
min_angle,
max_angle,
l1,
l2,
save_scan=False,
max_dist=max_dist,
min_dist=min_dist)
#max_dist=2.0,min_dist=min_dist)
gr = og3d.occupancy_grid_3d(brf, tlb, resolution)
gr.fill_grid(all_pts)
gr.to_binary(1)
l = gr.find_plane_indices(assume_plane=True)
z_min = min(l) * gr.resolution[2, 0] + gr.brf[2, 0]
z_max = max(l) * gr.resolution[2, 0] + gr.brf[2, 0]
print 'height of plane:', (z_max + z_min) / 2
pts = gr.grid_to_points()
plane_pts_bool = np.multiply(pts[2, :] >= z_min, pts[2, :] <= z_max)
plane_pts = pts[:, np.where(plane_pts_bool)[1].A1.tolist()]
above_pts = pts[:, np.where(pts[2, :] > z_max)[1].A1.tolist()]
below_pts = pts[:, np.where(pts[2, :] < z_min)[1].A1.tolist()]
d3m.plot_points(pt, color=(0, 1, 0.), mode='sphere')
d3m.plot_points(plane_pts, color=(0, 0, 1.))
d3m.plot_points(above_pts, color=(1.0, 1.0, 1.0))
d3m.plot_points(below_pts, color=(1., 0., 0.))
cube_tups = gr.grid_lines()
d3m.plot_cuboid(cube_tups)
d3m.show()
def test_plane_finding():
''' visualize plane finding.
'''
# brf = pt + np.matrix([-0.4,-0.2,-0.3]).T
# brf[0,0] = max(brf[0,0],0.05)
# print 'brf:', brf.T
#
# tlb = pt + np.matrix([0.3, 0.2,0.3]).T
# resolution = np.matrix([0.01,0.01,0.0025]).T
brf = pt+np.matrix([-0.15,-0.25,-0.2]).T
brf[0,0] = max(0.07,brf[0,0])
tlb = pt+np.matrix([0.25, 0.25,0.2]).T
resolution = np.matrix([0.01,0.01,0.0025]).T
max_dist = np.linalg.norm(tlb) + 0.2
min_dist = brf[0,0]
all_pts = generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2,save_scan=False,
max_dist=max_dist,min_dist=min_dist)
#max_dist=2.0,min_dist=min_dist)
gr = og3d.occupancy_grid_3d(brf,tlb,resolution)
gr.fill_grid(all_pts)
gr.to_binary(1)
l = gr.find_plane_indices(assume_plane=True)
z_min = min(l)*gr.resolution[2,0]+gr.brf[2,0]
z_max = max(l)*gr.resolution[2,0]+gr.brf[2,0]
print 'height of plane:', (z_max+z_min)/2
pts = gr.grid_to_points()
plane_pts_bool = np.multiply(pts[2,:]>=z_min,pts[2,:]<=z_max)
plane_pts = pts[:,np.where(plane_pts_bool)[1].A1.tolist()]
above_pts =pts[:,np.where(pts[2,:]>z_max)[1].A1.tolist()]
below_pts =pts[:,np.where(pts[2,:]<z_min)[1].A1.tolist()]
d3m.plot_points(pt,color=(0,1,0.),mode='sphere')
d3m.plot_points(plane_pts,color=(0,0,1.))
d3m.plot_points(above_pts,color=(1.0,1.0,1.0))
d3m.plot_points(below_pts,color=(1.,0.,0.))
cube_tups = gr.grid_lines()
d3m.plot_cuboid(cube_tups)
d3m.show()
def create_grid(brf,
tlb,
resolution,
pos_list,
scan_list,
l1,
l2,
display_flag=False,
show_pts=True,
rotation_angle=0.,
occupancy_threshold=1):
''' rotation angle - about the Z axis.
'''
max_dist = np.linalg.norm(tlb) + 0.2
min_dist = brf[0, 0]
min_angle, max_angle = math.radians(-60), math.radians(60)
all_pts = generate_pointcloud(pos_list,
scan_list,
min_angle,
max_angle,
l1,
l2,
max_dist=max_dist,
min_dist=min_dist)
rot_mat = tr.Rz(rotation_angle)
all_pts_rot = rot_mat * all_pts
gr = og3d.occupancy_grid_3d(brf,
tlb,
resolution,
rotation_z=rotation_angle)
gr.fill_grid(all_pts_rot)
gr.to_binary(occupancy_threshold)
if display_flag == True:
if show_pts:
d3m.plot_points(all_pts, color=(0., 0., 0.))
cube_tups = gr.grid_lines(rotation_angle=rotation_angle)
d3m.plot_cuboid(cube_tups)
return gr
def max_fwd_without_collision(all_pts, z_height, max_dist, display_list=None):
''' find the max distance that it is possible to move fwd by
without collision.
all_pts - 3xN matrix of 3D points in thok frame.
z - height of zenither while taking the scan.
max_dist - how far do I want to check for a possible collision.
returns max_dist that the thok can be moved fwd without collision.
'''
brf = np.matrix([0.2, -0.4, -z_height - 0.1]).T
tlb = np.matrix([max_dist, 0.4, -z_height + 1.8]).T
resolution = np.matrix([0.05, 0.05, 0.02]).T
gr = og3d.occupancy_grid_3d(brf, tlb, resolution)
gr.fill_grid(all_pts)
gr.to_binary(4)
ground_z = tr.thok0Tglobal(np.matrix([0, 0, -z_height]).T)[2, 0]
# gr.remove_horizontal_plane(hmax=ground_z+0.1)
gr.remove_horizontal_plane(extra_layers=2)
collide_pts = np.row_stack(np.where(gr.grid != 0))
x_coords = collide_pts[0]
# print x_coords
if x_coords.shape[0] == 0:
max_x = max_dist
# height_mat = np.arrary([np.Inf])
else:
max_x_gr = np.min(x_coords)
# height_mat = collide_pts[1,np.where(x_coords==max_x_gr)[0]]
# height_mat = height_mat*gr.resolution[2,0]+gr.brf[2,0]
max_x = max_x_gr * gr.resolution[0, 0] + gr.brf[0, 0]
if display_list != None:
collide_grid = gr.grid_to_points()
display_list.append(pu.PointCloud(all_pts, (200, 200, 200)))
display_list.append(
pu.CubeCloud(collide_grid, (200, 200, 200), resolution))
display_list.append(
pu.CubeCloud(np.matrix((max_x, 0., 0.)).T, (0, 0, 200),
size=np.matrix([0.05, .05, 0.05]).T))
# return max_x,np.matrix(height_mat)
return max_x
def max_fwd_without_collision(all_pts,z_height,max_dist,display_list=None):
''' find the max distance that it is possible to move fwd by
without collision.
all_pts - 3xN matrix of 3D points in thok frame.
z - height of zenither while taking the scan.
max_dist - how far do I want to check for a possible collision.
returns max_dist that the thok can be moved fwd without collision.
'''
brf = np.matrix([0.2,-0.4,-z_height-0.1]).T
tlb = np.matrix([max_dist, 0.4,-z_height+1.8]).T
resolution = np.matrix([0.05,0.05,0.02]).T
gr = og3d.occupancy_grid_3d(brf,tlb,resolution)
gr.fill_grid(all_pts)
gr.to_binary(4)
ground_z = tr.thok0Tglobal(np.matrix([0,0,-z_height]).T)[2,0]
# gr.remove_horizontal_plane(hmax=ground_z+0.1)
gr.remove_horizontal_plane(extra_layers=2)
collide_pts = np.row_stack(np.where(gr.grid!=0))
x_coords = collide_pts[0]
# print x_coords
if x_coords.shape[0] == 0:
max_x = max_dist
# height_mat = np.arrary([np.Inf])
else:
max_x_gr = np.min(x_coords)
# height_mat = collide_pts[1,np.where(x_coords==max_x_gr)[0]]
# height_mat = height_mat*gr.resolution[2,0]+gr.brf[2,0]
max_x = max_x_gr*gr.resolution[0,0]+gr.brf[0,0]
if display_list != None:
collide_grid = gr.grid_to_points()
display_list.append(pu.PointCloud(all_pts,(200,200,200)))
display_list.append(pu.CubeCloud(collide_grid,(200,200,200),resolution))
display_list.append(pu.CubeCloud(np.matrix((max_x,0.,0.)).T,(0,0,200),size=np.matrix([0.05,.05,0.05]).T))
# return max_x,np.matrix(height_mat)
return max_x
def grasp_location_on_object(obj,display_list=None):
''' obj - 3xN numpy matrix of points of the object.
'''
pts_2d = obj[0:2,:]
centroid_2d = pts_2d.mean(1)
pts_2d_zeromean = pts_2d-centroid_2d
e_vals,e_vecs = np.linalg.eig(pts_2d_zeromean*pts_2d_zeromean.T)
# get the min size
min_index = np.argmin(e_vals)
min_evec = e_vecs[:,min_index]
print 'min eigenvector:', min_evec.T
pts_1d = min_evec.T * pts_2d
min_size = pts_1d.max() - pts_1d.min()
print 'spread along min eigenvector:', min_size
max_height = obj[2,:].max()
tlb = obj.max(1)
brf = obj.min(1)
print 'tlb:', tlb.T
print 'brf:', brf.T
resolution = np.matrix([0.005,0.005,0.005]).T
gr = og3d.occupancy_grid_3d(brf,tlb,resolution)
gr.fill_grid(obj)
gr.to_binary(1)
obj = gr.grid_to_points()
grid_2d = gr.grid.max(2)
grid_2d_filled = ni.binary_fill_holes(grid_2d)
gr.grid[:,:,0] = gr.grid[:,:,0]+grid_2d_filled-grid_2d
p = np.matrix(np.row_stack(np.where(grid_2d_filled==1))).astype('float')
p[0,:] = p[0,:]*gr.resolution[0,0]
p[1,:] = p[1,:]*gr.resolution[1,0]
p += gr.brf[0:2,0]
print 'new mean:', p.mean(1).T
print 'centroid_2d:', centroid_2d.T
centroid_2d = p.mean(1)
if min_size<0.12:
# grasp at centroid.
grasp_point = np.row_stack((centroid_2d,np.matrix([max_height+gr.resolution[2,0]*2])))
# grasp_point[2,0] = max_height
gripper_angle = -math.atan2(-min_evec[0,0],min_evec[1,0])
grasp_vec = min_evec
if display_list != None:
max_index = np.argmax(e_vals)
max_evec = e_vecs[:,max_index]
pts_1d = max_evec.T * pts_2d
max_size = pts_1d.max() - pts_1d.min()
v = np.row_stack((max_evec,np.matrix([0.])))
max_end_pt1 = grasp_point + v*max_size/2.
max_end_pt2 = grasp_point - v*max_size/2.
display_list.append(pu.Line(max_end_pt1,max_end_pt2,color=(0,0,0)))
else:
#----- more complicated grasping location finder.
for i in range(gr.grid_shape[2,0]):
gr.grid[:,:,i] = gr.grid[:,:,i]*(i+1)
height_map = gr.grid.max(2) * gr.resolution[2,0]
# print height_map
print 'height std deviation:',math.sqrt(height_map[np.where(height_map>0.)].var())
# connect_structure = np.empty((3,3),dtype='int')
# connect_structure[:,:] = 1
# for i in range(gr.grid_shape[2,0]):
# slice = gr.grid[:,:,i]
# slice_filled = ni.binary_fill_holes(slice)
# slice_edge = slice_filled - ni.binary_erosion(slice_filled,connect_structure)
# gr.grid[:,:,i] = slice_edge*(i+1)
#
# height_map = gr.grid.max(2) * gr.resolution[2,0]
# # print height_map
# print 'contoured height std deviation:',math.sqrt(height_map[np.where(height_map>0.)].var())
#
high_pts_2d = obj[0:2,np.where(obj[2,:]>max_height-0.005)[1].A1]
#high_pts_1d = min_evec.T * high_pts_2d
high_pts_1d = ut.norm(high_pts_2d)
idx1 = np.argmin(high_pts_1d)
pt1 = high_pts_2d[:,idx1]
idx2 = np.argmax(high_pts_1d)
pt2 = high_pts_2d[:,idx2]
if np.linalg.norm(pt1)<np.linalg.norm(pt2):
grasp_point = np.row_stack((pt1,np.matrix([max_height])))
else:
grasp_point = np.row_stack((pt2,np.matrix([max_height])))
vec = centroid_2d-grasp_point[0:2,0]
gripper_angle = -math.atan2(-vec[0,0],vec[1,0])
grasp_vec = vec/np.linalg.norm(vec)
if display_list != None:
pt1 = np.row_stack((pt1,np.matrix([max_height])))
pt2 = np.row_stack((pt2,np.matrix([max_height])))
# display_list.insert(0,pu.CubeCloud(pt1,(0,0,200),size=(0.005,0.005,0.005)))
# display_list.insert(0,pu.CubeCloud(pt2,(200,0,200),size=(0.005,0.005,0.005)))
if display_list != None:
pts = gr.grid_to_points()
size = resolution
# size = resolution*2
# size[2,0] = size[2,0]*2
#display_list.insert(0,pu.PointCloud(pts,(200,0,0)))
display_list.append(pu.CubeCloud(pts,color=(200,0,0),size=size))
display_list.append(pu.CubeCloud(grasp_point,(0,200,200),size=(0.007,0.007,0.007)))
v = np.row_stack((grasp_vec,np.matrix([0.])))
min_end_pt1 = grasp_point + v*min_size/2.
min_end_pt2 = grasp_point - v*min_size/2.
max_evec = np.matrix((min_evec[1,0],-min_evec[0,0])).T
pts_1d = max_evec.T * pts_2d
max_size = pts_1d.max() - pts_1d.min()
display_list.append(pu.Line(min_end_pt1,min_end_pt2,color=(0,255,0)))
return grasp_point,gripper_angle
def find_door_handle(grid,pt,list = None,rotation_angle=math.radians(0.),
occupancy_threshold=None,resolution=None):
grid.remove_vertical_plane()
pts = grid.grid_to_points()
rot_mat = tr.Rz(rotation_angle)
t_pt = rot_mat*pt
brf = t_pt+np.matrix([-0.2,-0.3,-0.2]).T
tlb = t_pt+np.matrix([0.2, 0.3,0.2]).T
#resolution = np.matrix([0.02,0.0025,0.02]).T
grid = og3d.occupancy_grid_3d(brf,tlb,resolution,rotation_z=rotation_angle)
if pts.shape[1] == 0:
return None
grid.fill_grid(tr.Rz(rotation_angle)*pts)
grid.to_binary(occupancy_threshold)
labeled_arr,n_labels = grid.find_objects()
if list == None:
object_points_list = []
else:
object_points_list = list
for l in range(n_labels):
pts = grid.labeled_array_to_points(labeled_arr,l+1)
obj_height = np.max(pts[2,:])-np.min(pts[2,:])
print 'object_height:', obj_height
if obj_height > 0.1:
#remove the big objects
grid.grid[np.where(labeled_arr==l+1)] = 0
connect_structure = np.empty((3,3,3),dtype='int')
connect_structure[:,:,:] = 0
connect_structure[1,:,1] = 1
# dilate away - actual width of the door handle is not affected
# because that I will get from the actual point cloud!
grid.grid = ni.binary_dilation(grid.grid,connect_structure,iterations=7)
labeled_arr,n_labels = grid.find_objects()
for l in range(n_labels):
pts = grid.labeled_array_to_points(labeled_arr,l+1)
pts2d = pts[1:3,:] # only the y-z coordinates.
obj_width = (pts2d.max(1)-pts2d.min(1))[0,0]
print 'processing_3d.find_door_handle: object width = ', obj_width
if obj_width < 0.05:
continue
pts2d_zeromean = pts2d-pts2d.mean(1)
e_vals,e_vecs = np.linalg.eig(pts2d_zeromean*pts2d_zeromean.T)
max_index = np.argmax(e_vals)
max_evec = e_vecs[:,max_index]
ang = math.atan2(max_evec[1,0],max_evec[0,0])
print 'processing_3d.find_door_handle: ang = ', math.degrees(ang)
if (ang>math.radians(45) and ang<math.radians(135)) or \
(ang>math.radians(-135) and ang<math.radians(-45)):
# assumption is that door handles are horizontal.
continue
object_points_list.append(pts)
print 'processing_3d.find_door_handle: found %d objects'%(len(object_points_list))
closest_obj = find_closest_object(object_points_list,pt)
return closest_obj
def grasp_location_on_object(obj, display_list=None):
''' obj - 3xN numpy matrix of points of the object.
'''
pts_2d = obj[0:2, :]
centroid_2d = pts_2d.mean(1)
pts_2d_zeromean = pts_2d - centroid_2d
e_vals, e_vecs = np.linalg.eig(pts_2d_zeromean * pts_2d_zeromean.T)
# get the min size
min_index = np.argmin(e_vals)
min_evec = e_vecs[:, min_index]
print 'min eigenvector:', min_evec.T
pts_1d = min_evec.T * pts_2d
min_size = pts_1d.max() - pts_1d.min()
print 'spread along min eigenvector:', min_size
max_height = obj[2, :].max()
tlb = obj.max(1)
brf = obj.min(1)
print 'tlb:', tlb.T
print 'brf:', brf.T
resolution = np.matrix([0.005, 0.005, 0.005]).T
gr = og3d.occupancy_grid_3d(brf, tlb, resolution)
gr.fill_grid(obj)
gr.to_binary(1)
obj = gr.grid_to_points()
grid_2d = gr.grid.max(2)
grid_2d_filled = ni.binary_fill_holes(grid_2d)
gr.grid[:, :, 0] = gr.grid[:, :, 0] + grid_2d_filled - grid_2d
p = np.matrix(np.row_stack(np.where(grid_2d_filled == 1))).astype('float')
p[0, :] = p[0, :] * gr.resolution[0, 0]
p[1, :] = p[1, :] * gr.resolution[1, 0]
p += gr.brf[0:2, 0]
print 'new mean:', p.mean(1).T
print 'centroid_2d:', centroid_2d.T
centroid_2d = p.mean(1)
if min_size < 0.12:
# grasp at centroid.
grasp_point = np.row_stack(
(centroid_2d, np.matrix([max_height + gr.resolution[2, 0] * 2])))
# grasp_point[2,0] = max_height
gripper_angle = -math.atan2(-min_evec[0, 0], min_evec[1, 0])
grasp_vec = min_evec
if display_list != None:
max_index = np.argmax(e_vals)
max_evec = e_vecs[:, max_index]
pts_1d = max_evec.T * pts_2d
max_size = pts_1d.max() - pts_1d.min()
v = np.row_stack((max_evec, np.matrix([0.])))
max_end_pt1 = grasp_point + v * max_size / 2.
max_end_pt2 = grasp_point - v * max_size / 2.
display_list.append(
pu.Line(max_end_pt1, max_end_pt2, color=(0, 0, 0)))
else:
#----- more complicated grasping location finder.
for i in range(gr.grid_shape[2, 0]):
gr.grid[:, :, i] = gr.grid[:, :, i] * (i + 1)
height_map = gr.grid.max(2) * gr.resolution[2, 0]
# print height_map
print 'height std deviation:', math.sqrt(
height_map[np.where(height_map > 0.)].var())
# connect_structure = np.empty((3,3),dtype='int')
# connect_structure[:,:] = 1
# for i in range(gr.grid_shape[2,0]):
# slice = gr.grid[:,:,i]
# slice_filled = ni.binary_fill_holes(slice)
# slice_edge = slice_filled - ni.binary_erosion(slice_filled,connect_structure)
# gr.grid[:,:,i] = slice_edge*(i+1)
#
# height_map = gr.grid.max(2) * gr.resolution[2,0]
# # print height_map
# print 'contoured height std deviation:',math.sqrt(height_map[np.where(height_map>0.)].var())
#
high_pts_2d = obj[0:2, np.where(obj[2, :] > max_height - 0.005)[1].A1]
#high_pts_1d = min_evec.T * high_pts_2d
high_pts_1d = ut.norm(high_pts_2d)
idx1 = np.argmin(high_pts_1d)
pt1 = high_pts_2d[:, idx1]
idx2 = np.argmax(high_pts_1d)
pt2 = high_pts_2d[:, idx2]
if np.linalg.norm(pt1) < np.linalg.norm(pt2):
grasp_point = np.row_stack((pt1, np.matrix([max_height])))
else:
grasp_point = np.row_stack((pt2, np.matrix([max_height])))
vec = centroid_2d - grasp_point[0:2, 0]
gripper_angle = -math.atan2(-vec[0, 0], vec[1, 0])
grasp_vec = vec / np.linalg.norm(vec)
if display_list != None:
pt1 = np.row_stack((pt1, np.matrix([max_height])))
pt2 = np.row_stack((pt2, np.matrix([max_height])))
# display_list.insert(0,pu.CubeCloud(pt1,(0,0,200),size=(0.005,0.005,0.005)))
# display_list.insert(0,pu.CubeCloud(pt2,(200,0,200),size=(0.005,0.005,0.005)))
if display_list != None:
pts = gr.grid_to_points()
size = resolution
# size = resolution*2
# size[2,0] = size[2,0]*2
#display_list.insert(0,pu.PointCloud(pts,(200,0,0)))
display_list.append(pu.CubeCloud(pts, color=(200, 0, 0), size=size))
display_list.append(
pu.CubeCloud(grasp_point, (0, 200, 200),
size=(0.007, 0.007, 0.007)))
v = np.row_stack((grasp_vec, np.matrix([0.])))
min_end_pt1 = grasp_point + v * min_size / 2.
min_end_pt2 = grasp_point - v * min_size / 2.
max_evec = np.matrix((min_evec[1, 0], -min_evec[0, 0])).T
pts_1d = max_evec.T * pts_2d
max_size = pts_1d.max() - pts_1d.min()
display_list.append(
pu.Line(min_end_pt1, min_end_pt2, color=(0, 255, 0)))
return grasp_point, gripper_angle
def find_door_handle(grid,
pt,
list=None,
rotation_angle=math.radians(0.),
occupancy_threshold=None,
resolution=None):
grid.remove_vertical_plane()
pts = grid.grid_to_points()
rot_mat = tr.Rz(rotation_angle)
t_pt = rot_mat * pt
brf = t_pt + np.matrix([-0.2, -0.3, -0.2]).T
tlb = t_pt + np.matrix([0.2, 0.3, 0.2]).T
#resolution = np.matrix([0.02,0.0025,0.02]).T
grid = og3d.occupancy_grid_3d(brf,
tlb,
resolution,
rotation_z=rotation_angle)
if pts.shape[1] == 0:
return None
grid.fill_grid(tr.Rz(rotation_angle) * pts)
grid.to_binary(occupancy_threshold)
labeled_arr, n_labels = grid.find_objects()
if list == None:
object_points_list = []
else:
object_points_list = list
for l in range(n_labels):
pts = grid.labeled_array_to_points(labeled_arr, l + 1)
obj_height = np.max(pts[2, :]) - np.min(pts[2, :])
print 'object_height:', obj_height
if obj_height > 0.1:
#remove the big objects
grid.grid[np.where(labeled_arr == l + 1)] = 0
connect_structure = np.empty((3, 3, 3), dtype='int')
connect_structure[:, :, :] = 0
connect_structure[1, :, 1] = 1
# dilate away - actual width of the door handle is not affected
# because that I will get from the actual point cloud!
grid.grid = ni.binary_dilation(grid.grid, connect_structure, iterations=7)
labeled_arr, n_labels = grid.find_objects()
for l in range(n_labels):
pts = grid.labeled_array_to_points(labeled_arr, l + 1)
pts2d = pts[1:3, :] # only the y-z coordinates.
obj_width = (pts2d.max(1) - pts2d.min(1))[0, 0]
print 'processing_3d.find_door_handle: object width = ', obj_width
if obj_width < 0.05:
continue
pts2d_zeromean = pts2d - pts2d.mean(1)
e_vals, e_vecs = np.linalg.eig(pts2d_zeromean * pts2d_zeromean.T)
max_index = np.argmax(e_vals)
max_evec = e_vecs[:, max_index]
ang = math.atan2(max_evec[1, 0], max_evec[0, 0])
print 'processing_3d.find_door_handle: ang = ', math.degrees(ang)
if (ang>math.radians(45) and ang<math.radians(135)) or \
(ang>math.radians(-135) and ang<math.radians(-45)):
# assumption is that door handles are horizontal.
continue
object_points_list.append(pts)
print 'processing_3d.find_door_handle: found %d objects' % (
len(object_points_list))
closest_obj = find_closest_object(object_points_list, pt)
return closest_obj
