Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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