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