def pushback_edge(pts2d,pt):
''' push pt away from the edge defined by pts2d.
pt - 2x1, pts2d - 2xN
returns the pushed point.
'''
closest_idx = find_closest_pt_index(pts2d,pt)
n_push_points = min(min(5,pts2d.shape[1]-closest_idx-1),closest_idx)
if closest_idx<n_push_points or (pts2d.shape[1]-closest_idx-1)<n_push_points:
print 'processing_3d.pushback_edge: pt is too close to the ends of the pts2d array.'
return None
edge_to_pt = pt-pts2d[:,closest_idx-n_push_points:closest_idx+n_push_points]
edge_to_pt_r = ut.norm(edge_to_pt)
edge_to_pt_a = np.arctan2(edge_to_pt[1,:],edge_to_pt[0,:])
non_zero_idxs = np.where(edge_to_pt_r>0.005)
edge_to_pt_r = edge_to_pt_r[non_zero_idxs]
edge_to_pt_r[0,:] = 1
edge_to_pt_a = edge_to_pt_a[non_zero_idxs]
edge_to_pt_unit = ut.cart_of_pol(np.row_stack((edge_to_pt_r,edge_to_pt_a)))
push_vector = edge_to_pt_unit.mean(1)
push_vector = push_vector/np.linalg.norm(push_vector)
print 'push_vector:', push_vector.T
pt_pushed = pt + push_vector*0.05
return pt_pushed
def find_closest_pt(pts2d,pt,pt_closer=False):
''' returns closest point to edge (2x1 matrix)
can return None also
'''
dist_pt = np.linalg.norm(pt[0:2,0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1,:],pts2d[0,:])
if pt_closer == False:
k_idxs = np.where(pts2d_r<=dist_pt)
else:
k_idxs = np.where(pts2d_r>dist_pt)
pts2d_r = pts2d_r[k_idxs]
pts2d_a = pts2d_a[k_idxs]
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((pts2d_r,pts2d_a))))
if pt_closer == False:
edge_to_pt = pt[0:2,0]-pts2d
else:
edge_to_pt = pts2d-pt[0:2,0]
edge_to_pt_a = np.arctan2(edge_to_pt[1,:],edge_to_pt[0,:])
keep_idxs = np.where(np.abs(edge_to_pt_a)<math.radians(70))[1].A1
if keep_idxs.shape[0] == 0:
return None
pts2d = pts2d[:,keep_idxs]
# pt_to_edge_dists = ut.norm(pts2d-pt[0:2,0])
# closest_pt_index = np.argmin(pt_to_edge_dists)
closest_pt_index = find_closest_pt_index(pts2d,pt[0:2,0])
closest_pt = pts2d[:,closest_pt_index]
return closest_pt
def pushback_edge(pts2d, pt):
''' push pt away from the edge defined by pts2d.
pt - 2x1, pts2d - 2xN
returns the pushed point.
'''
closest_idx = find_closest_pt_index(pts2d, pt)
n_push_points = min(min(5, pts2d.shape[1] - closest_idx - 1), closest_idx)
if closest_idx < n_push_points or (pts2d.shape[1] - closest_idx -
1) < n_push_points:
print 'processing_3d.pushback_edge: pt is too close to the ends of the pts2d array.'
return None
edge_to_pt = pt - pts2d[:, closest_idx - n_push_points:closest_idx +
n_push_points]
edge_to_pt_r = ut.norm(edge_to_pt)
edge_to_pt_a = np.arctan2(edge_to_pt[1, :], edge_to_pt[0, :])
non_zero_idxs = np.where(edge_to_pt_r > 0.005)
edge_to_pt_r = edge_to_pt_r[non_zero_idxs]
edge_to_pt_r[0, :] = 1
edge_to_pt_a = edge_to_pt_a[non_zero_idxs]
edge_to_pt_unit = ut.cart_of_pol(np.row_stack(
(edge_to_pt_r, edge_to_pt_a)))
push_vector = edge_to_pt_unit.mean(1)
push_vector = push_vector / np.linalg.norm(push_vector)
print 'push_vector:', push_vector.T
pt_pushed = pt + push_vector * 0.05
return pt_pushed
def find_closest_pt(pts2d, pt, pt_closer=False):
''' returns closest point to edge (2x1 matrix)
can return None also
'''
dist_pt = np.linalg.norm(pt[0:2, 0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1, :], pts2d[0, :])
if pt_closer == False:
k_idxs = np.where(pts2d_r <= dist_pt)
else:
k_idxs = np.where(pts2d_r > dist_pt)
pts2d_r = pts2d_r[k_idxs]
pts2d_a = pts2d_a[k_idxs]
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((pts2d_r, pts2d_a))))
if pt_closer == False:
edge_to_pt = pt[0:2, 0] - pts2d
else:
edge_to_pt = pts2d - pt[0:2, 0]
edge_to_pt_a = np.arctan2(edge_to_pt[1, :], edge_to_pt[0, :])
keep_idxs = np.where(np.abs(edge_to_pt_a) < math.radians(70))[1].A1
if keep_idxs.shape[0] == 0:
return None
pts2d = pts2d[:, keep_idxs]
# pt_to_edge_dists = ut.norm(pts2d-pt[0:2,0])
# closest_pt_index = np.argmin(pt_to_edge_dists)
closest_pt_index = find_closest_pt_index(pts2d, pt[0:2, 0])
closest_pt = pts2d[:, closest_pt_index]
return closest_pt
def find_approach_direction(grid,pt,display_list=None):
z_plane,max_count = grid.argmax_z(search_up=True)
z_plane_meters = z_plane*grid.resolution[2,0]+grid.brf[2,0]
l = grid.find_plane_indices(assume_plane=True)
print '------------ min(l)',min(l)
z_plane_argmax,max_count = grid.argmax_z(search_up=False)
z_plane_below = max(0,z_plane_argmax-5)
print 'z_plane_argmax',z_plane_argmax
print 'z_plane_below',z_plane_below
print 'l:',l
# l = range(z_plane_below,z_plane)+l
copy_grid = copy.deepcopy(grid)
plane_slices = grid.grid[:,:,l]
copy_grid.grid[:,:,:] = 0
copy_grid.grid[:,:,l] = copy.copy(plane_slices)
#display_list.append(pu.PointCloud(copy_grid.grid_to_points(),color=(0,0,255)))
#plane_pts = copy_grid.grid_to_points()
grid_2d = np.max(grid.grid[:,:,l],2)
grid_2d = ni.binary_dilation(grid_2d,iterations=4) # I want 4-connectivity while filling holes.
grid_2d = ni.binary_fill_holes(grid_2d) # I want 4-connectivity while filling holes.
labeled_arr,n_labels = ni.label(grid_2d)
labels_list = range(1,n_labels+1)
count_objects = ni.sum(grid_2d,labeled_arr,labels_list)
if n_labels == 1:
count_objects = [count_objects]
max_label = np.argmax(np.matrix(count_objects))
grid_2d[np.where(labeled_arr!=max_label+1)] = 0
# connect_structure = np.empty((3,3),dtype='int')
# connect_structure[:,:] = 1
# eroded_2d = ni.binary_erosion(grid_2d,connect_structure,iterations=4)
# eroded_2d = ni.binary_erosion(grid_2d)
# grid_2d = grid_2d-eroded_2d
labeled_arr_3d = copy_grid.grid.swapaxes(2,0)
labeled_arr_3d = labeled_arr_3d.swapaxes(1,2)
labeled_arr_3d = labeled_arr_3d*grid_2d
labeled_arr_3d = labeled_arr_3d.swapaxes(2,0)
labeled_arr_3d = labeled_arr_3d.swapaxes(1,0)
copy_grid.grid = labeled_arr_3d
pts3d = copy_grid.grid_to_points()
pts2d = pts3d[0:2,:]
dist_pt = np.linalg.norm(pt[0:2,0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1,:],pts2d[0,:])
max_angle = np.max(pts2d_a)
min_angle = np.min(pts2d_a)
max_angle = min(max_angle,math.radians(50))
min_angle = max(min_angle,math.radians(-50))
ang_res = math.radians(1.)
n_bins = int((max_angle-min_angle)/ang_res)
print 'n_bins:', n_bins
n_bins = min(50,n_bins)
# n_bins=50
ang_res = (max_angle-min_angle)/n_bins
print 'n_bins:', n_bins
angle_list = []
range_list = []
for i in range(n_bins):
angle = min_angle+ang_res*i
idxs = np.where(np.multiply(pts2d_a<(angle+ang_res/2.),pts2d_a>(angle-ang_res/2.)))
r_mat = pts2d_r[idxs]
a_mat = pts2d_a[idxs]
if r_mat.shape[1] == 0:
continue
min_idx = np.argmin(r_mat.A1)
range_list.append(r_mat[0,min_idx])
angle_list.append(a_mat[0,min_idx])
if range_list == []:
print 'processing_3d.find_approach_direction: No edge points found'
return None,None
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((range_list,angle_list))))
closest_pt_1 = find_closest_pt(pts2d,pt,pt_closer=False)
if closest_pt_1 == None:
dist1 = np.Inf
else:
approach_vector_1 = pt[0:2,0] - closest_pt_1
dist1 = np.linalg.norm(approach_vector_1)
approach_vector_1 = approach_vector_1/dist1
closest_pt_2 = find_closest_pt(pts2d,pt,pt_closer=True)
if closest_pt_2 == None:
dist2 = np.Inf
else:
approach_vector_2 = closest_pt_2 - pt[0:2,0]
dist2 = np.linalg.norm(approach_vector_2)
approach_vector_2 = approach_vector_2/dist2
if dist1 == np.Inf and dist2 == np.Inf:
approach_vector_1 = np.matrix([1.,0.,0.]).T
approach_vector_2 = np.matrix([1.,0.,0.]).T
print 'VERY STRANGE: processing_3d.find_approach_direction: both distances are Inf'
if dist1<0.05 or dist2<0.05:
print 'dist1,dist2:',dist1,dist2
t_pt = copy.copy(pt)
if dist1<dist2 and dist1<0.02:
t_pt[0,0] += 0.05
elif dist2<0.02:
t_pt[0,0] -= 0.05
#pt_new = pushback_edge(pts2d,pt[0:2,0])
pt_new = pushback_edge(pts2d,t_pt[0:2,0])
if display_list != None:
pt_new_3d = np.row_stack((pt_new,np.matrix([z_plane_meters])))
display_list.append(pu.CubeCloud(pt_new_3d,color=(200,000,0),size=(0.009,0.009,0.009)))
closest_pt_1 = find_closest_pt(pts2d,pt_new,pt_closer=False)
if closest_pt_1 == None:
dist1 = np.Inf
else:
approach_vector_1 = pt_new - closest_pt_1
dist1 = np.linalg.norm(approach_vector_1)
approach_vector_1 = approach_vector_1/dist1
closest_pt_2 = find_closest_pt(pts2d,pt_new,pt_closer=True)
if closest_pt_2 == None:
dist2 = np.Inf
else:
approach_vector_2 = closest_pt_2 - pt_new
dist2 = np.linalg.norm(approach_vector_2)
approach_vector_2 = approach_vector_2/dist2
print '----------- dist1,dist2:',dist1,dist2
if dist2<dist1:
closest_pt = closest_pt_2
approach_vector = approach_vector_2
else:
closest_pt = closest_pt_1
approach_vector = approach_vector_1
print '----------- approach_vector:',approach_vector.T
closest_pt = np.row_stack((closest_pt,np.matrix([z_plane_meters])))
if display_list != None:
z = np.matrix(np.empty((1,pts2d.shape[1])))
z[:,:] = z_plane_meters
pts3d_front = np.row_stack((pts2d,z))
display_list.append(pu.CubeCloud(closest_pt,color=(255,255,0),size=(0.020,0.020,0.020)))
display_list.append(pu.CubeCloud(pts3d_front,color=(255,0,255),size=grid.resolution))
#display_list.append(pu.CubeCloud(pts3d,color=(0,255,0)))
return closest_pt,approach_vector
def find_approach_direction(grid, pt, display_list=None):
z_plane, max_count = grid.argmax_z(search_up=True)
z_plane_meters = z_plane * grid.resolution[2, 0] + grid.brf[2, 0]
l = grid.find_plane_indices(assume_plane=True)
print '------------ min(l)', min(l)
z_plane_argmax, max_count = grid.argmax_z(search_up=False)
z_plane_below = max(0, z_plane_argmax - 5)
print 'z_plane_argmax', z_plane_argmax
print 'z_plane_below', z_plane_below
print 'l:', l
# l = range(z_plane_below,z_plane)+l
copy_grid = copy.deepcopy(grid)
plane_slices = grid.grid[:, :, l]
copy_grid.grid[:, :, :] = 0
copy_grid.grid[:, :, l] = copy.copy(plane_slices)
#display_list.append(pu.PointCloud(copy_grid.grid_to_points(),color=(0,0,255)))
#plane_pts = copy_grid.grid_to_points()
grid_2d = np.max(grid.grid[:, :, l], 2)
grid_2d = ni.binary_dilation(
grid_2d, iterations=4) # I want 4-connectivity while filling holes.
grid_2d = ni.binary_fill_holes(
grid_2d) # I want 4-connectivity while filling holes.
labeled_arr, n_labels = ni.label(grid_2d)
labels_list = range(1, n_labels + 1)
count_objects = ni.sum(grid_2d, labeled_arr, labels_list)
if n_labels == 1:
count_objects = [count_objects]
max_label = np.argmax(np.matrix(count_objects))
grid_2d[np.where(labeled_arr != max_label + 1)] = 0
# connect_structure = np.empty((3,3),dtype='int')
# connect_structure[:,:] = 1
# eroded_2d = ni.binary_erosion(grid_2d,connect_structure,iterations=4)
# eroded_2d = ni.binary_erosion(grid_2d)
# grid_2d = grid_2d-eroded_2d
labeled_arr_3d = copy_grid.grid.swapaxes(2, 0)
labeled_arr_3d = labeled_arr_3d.swapaxes(1, 2)
labeled_arr_3d = labeled_arr_3d * grid_2d
labeled_arr_3d = labeled_arr_3d.swapaxes(2, 0)
labeled_arr_3d = labeled_arr_3d.swapaxes(1, 0)
copy_grid.grid = labeled_arr_3d
pts3d = copy_grid.grid_to_points()
pts2d = pts3d[0:2, :]
dist_pt = np.linalg.norm(pt[0:2, 0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1, :], pts2d[0, :])
max_angle = np.max(pts2d_a)
min_angle = np.min(pts2d_a)
max_angle = min(max_angle, math.radians(50))
min_angle = max(min_angle, math.radians(-50))
ang_res = math.radians(1.)
n_bins = int((max_angle - min_angle) / ang_res)
print 'n_bins:', n_bins
n_bins = min(50, n_bins)
# n_bins=50
ang_res = (max_angle - min_angle) / n_bins
print 'n_bins:', n_bins
angle_list = []
range_list = []
for i in range(n_bins):
angle = min_angle + ang_res * i
idxs = np.where(
np.multiply(pts2d_a < (angle + ang_res / 2.), pts2d_a >
(angle - ang_res / 2.)))
r_mat = pts2d_r[idxs]
a_mat = pts2d_a[idxs]
if r_mat.shape[1] == 0:
continue
min_idx = np.argmin(r_mat.A1)
range_list.append(r_mat[0, min_idx])
angle_list.append(a_mat[0, min_idx])
if range_list == []:
print 'processing_3d.find_approach_direction: No edge points found'
return None, None
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((range_list, angle_list))))
closest_pt_1 = find_closest_pt(pts2d, pt, pt_closer=False)
if closest_pt_1 == None:
dist1 = np.Inf
else:
approach_vector_1 = pt[0:2, 0] - closest_pt_1
dist1 = np.linalg.norm(approach_vector_1)
approach_vector_1 = approach_vector_1 / dist1
closest_pt_2 = find_closest_pt(pts2d, pt, pt_closer=True)
if closest_pt_2 == None:
dist2 = np.Inf
else:
approach_vector_2 = closest_pt_2 - pt[0:2, 0]
dist2 = np.linalg.norm(approach_vector_2)
approach_vector_2 = approach_vector_2 / dist2
if dist1 == np.Inf and dist2 == np.Inf:
approach_vector_1 = np.matrix([1., 0., 0.]).T
approach_vector_2 = np.matrix([1., 0., 0.]).T
print 'VERY STRANGE: processing_3d.find_approach_direction: both distances are Inf'
if dist1 < 0.05 or dist2 < 0.05:
print 'dist1,dist2:', dist1, dist2
t_pt = copy.copy(pt)
if dist1 < dist2 and dist1 < 0.02:
t_pt[0, 0] += 0.05
elif dist2 < 0.02:
t_pt[0, 0] -= 0.05
#pt_new = pushback_edge(pts2d,pt[0:2,0])
pt_new = pushback_edge(pts2d, t_pt[0:2, 0])
if display_list != None:
pt_new_3d = np.row_stack((pt_new, np.matrix([z_plane_meters])))
display_list.append(
pu.CubeCloud(pt_new_3d,
color=(200, 000, 0),
size=(0.009, 0.009, 0.009)))
closest_pt_1 = find_closest_pt(pts2d, pt_new, pt_closer=False)
if closest_pt_1 == None:
dist1 = np.Inf
else:
approach_vector_1 = pt_new - closest_pt_1
dist1 = np.linalg.norm(approach_vector_1)
approach_vector_1 = approach_vector_1 / dist1
closest_pt_2 = find_closest_pt(pts2d, pt_new, pt_closer=True)
if closest_pt_2 == None:
dist2 = np.Inf
else:
approach_vector_2 = closest_pt_2 - pt_new
dist2 = np.linalg.norm(approach_vector_2)
approach_vector_2 = approach_vector_2 / dist2
print '----------- dist1,dist2:', dist1, dist2
if dist2 < dist1:
closest_pt = closest_pt_2
approach_vector = approach_vector_2
else:
closest_pt = closest_pt_1
approach_vector = approach_vector_1
print '----------- approach_vector:', approach_vector.T
closest_pt = np.row_stack((closest_pt, np.matrix([z_plane_meters])))
if display_list != None:
z = np.matrix(np.empty((1, pts2d.shape[1])))
z[:, :] = z_plane_meters
pts3d_front = np.row_stack((pts2d, z))
display_list.append(
pu.CubeCloud(closest_pt,
color=(255, 255, 0),
size=(0.020, 0.020, 0.020)))
display_list.append(
pu.CubeCloud(pts3d_front,
color=(255, 0, 255),
size=grid.resolution))
#display_list.append(pu.CubeCloud(pts3d,color=(0,255,0)))
return closest_pt, approach_vector
