예제 #1
0
def get_nrmls(pcd, camera_location=(800, -200, 1800), toggledebug=False):
    pcd_o3d = o3d_helper.nparray2o3dpcd(pcd)
    pcd_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(
        radius=20, max_nn=1000))
    # for n in np.asarray(pcd.normals)[:10]:
    #     print(n)
    # print("----------------")
    o3d.geometry.PointCloud.orient_normals_towards_camera_location(
        pcd_o3d, camera_location=camera_location)
    # for n in np.asarray(pcd.normals)[:10]:
    #     print(n)
    pcd_nrmls = np.asarray(pcd_o3d.normals)
    pcd_nrmls = np.asarray(
        [-n if np.dot(n, np.asarray([0, 0, 1])) < 0 else n for n in pcd_nrmls])

    if toggledebug:
        for i, n in enumerate(pcd_nrmls):
            import random
            v = random.choice(range(0, 100))
            if v == 1:
                base.pggen.plotArrow(base.render,
                                     spos=pcd[i],
                                     epos=pcd[i] + 10 * n)
        base.run()
    return pcd_nrmls
예제 #2
0
def get_objpcd(objcm, objmat4=np.eye(4), sample_num=100000, toggledebug=False):
    objpcd = np.asarray(
        objcm.sample_surface(nsample=sample_num, toggle_option=None))
    objpcd = trans_pcd(objpcd, objmat4)

    if toggledebug:
        objpcd = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd))
        objpcd.paint_uniform_color([1, 0.706, 0])
        o3d.visualization.draw_geometries([objpcd])

    return objpcd
예제 #3
0
def get_objpcd_partial(objcm,
                       objmat4=np.eye(4),
                       sample_num=100000,
                       toggledebug=False):
    objpcd = np.asarray(ts.sample_surface(objcm.trimesh, count=sample_num))
    objpcd = trans_pcd(objpcd, objmat4)

    grid = {}
    for p in objpcd:
        x = round(p[0], 0)
        y = round(p[1], 0)
        if str((x, y)) in grid.keys():
            grid[str((x, y))].append(p)
        else:
            grid[str((x, y))] = [p]
    objpcd_new = []
    for k, v in grid.items():
        z_max = max(np.array(v)[:, 2])
        for p in v:
            objpcd_new.append([p[0], p[1], z_max])
    objpcd_new = np.array(objpcd_new)

    print("Length of org pcd", len(objpcd))
    print("Length of partial pcd", len(objpcd_new))

    if toggledebug:
        objpcd = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd))
        objpcd.paint_uniform_color([1, 0.706, 0])
        o3d.visualization.draw_geometries([objpcd])

        objpcd_partial = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd_new))
        objpcd_partial.paint_uniform_color([0, 0.706, 1])
        o3d.visualization.draw_geometries([objpcd_partial])

        # pcddnp = base.pg.genpointcloudnp(objpcd)
        # pcddnp.reparentTo(base.render)

    return objpcd_new
예제 #4
0
def get_plane(pcd, dist_threshold=0.2, toggledebug=False):
    pcd_o3d = o3d_helper.nparray2o3dpcd(pcd)
    plane, inliers = pcd_o3d.segment_plane(distance_threshold=dist_threshold,
                                           ransac_n=3,
                                           num_iterations=1000)
    plane_pcd = pcd[inliers]
    center = get_pcd_center(plane_pcd)
    if toggledebug:
        show_pcd(pcd[inliers], rgba=(1, 0, 0, 1))
        base.pggen.plotArrow(base.render,
                             spos=center,
                             epos=plane[:3] * plane[3],
                             rgba=(1, 0, 0, 1))

    return plane[:3], plane[3]
예제 #5
0
def get_objpcd_partial_bycampos(objcm,
                                objmat4=np.eye(4),
                                sample_num=100000,
                                cam_pos=np.array([860, 80, 1780]),
                                toggledebug=False):
    def __get_angle(x, y):
        lx = np.sqrt(x.dot(x))
        ly = np.sqrt(y.dot(y))
        cos_angle = x.dot(y) / (lx * ly)
        angle = np.arccos(cos_angle)
        return angle * 360 / 2 / np.pi
        # return cos_angle

    def sigmoid(angle):
        return 1 / (1 + np.exp((angle - 90) / 90)) - 0.5

    objpcd = np.asarray(ts.sample_surface(objcm.trimesh, count=sample_num))
    objpcd_center = get_pcd_center(objpcd)
    face_num = len(objcm.trimesh.face_normals)

    objpcd_new = []
    area_list = objcm.trimesh.area_faces
    area_sum = sum(area_list)

    for i, n in enumerate(objcm.trimesh.face_normals):
        n = np.dot(n, objmat4[:3, :3])
        angle = __get_angle(n, np.array(cam_pos - objpcd_center))

        if angle > 90:
            continue
        else:
            objcm_temp = copy.deepcopy(objcm)
            # print(i, angle, sigmoid(angle))
            mask_temp = [False] * face_num
            mask_temp[i] = True
            objcm_temp.trimesh.update_faces(mask_temp)
            objpcd_new.extend(
                np.asarray(
                    objcm_temp.sample_surface(
                        nsample=int(sample_num / area_sum * area_list[i] *
                                    sigmoid(angle) * 100),
                        toggle_option=None)))
    if len(objpcd_new) > sample_num:
        objpcd_new = random.sample(objpcd_new, sample_num)
    objpcd_new = np.array(objpcd_new)
    objpcd_new = trans_pcd(objpcd_new, objmat4)

    # print("Length of org pcd", len(objpcd))
    # print("Length of source pcd", len(objpcd_new))

    if toggledebug:
        objpcd = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd))
        objpcd.paint_uniform_color([1, 0.706, 0])
        o3d.visualization.draw_geometries([objpcd])

        objpcd_partial = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd_new))
        objpcd_partial.paint_uniform_color([0, 0.706, 1])
        o3d.visualization.draw_geometries([objpcd_partial])

        # pcddnp = base.pg.genpointcloudnp(objpcd)
        # pcddnp.reparentTo(base.render)

    return objpcd_new
예제 #6
0
                             spos=center,
                             epos=plane[:3] * plane[3],
                             rgba=(1, 0, 0, 1))

    return plane[:3], plane[3]


if __name__ == '__main__':
    '''
    set up env and param
    '''
    base, env = el.loadEnv_wrs()
    objcm = el.loadObj("pentip.stl")

    # source_pcd = np.asarray(ts.sample_surface(objcm.trimesh, count=10000))
    source = o3d_helper.nparray2o3dpcd(source_pcd[source_pcd[:, 2] > 5])
    # source.paint_uniform_color([0, 0.706, 1])
    # o3d.visualization.draw_geometries([source])

    # get_objpcd_partial(objcm, sample_num=10000, toggledebug=True)

    # inithomomat = pickle.load(
    #     open(el.root + "/graspplanner/graspmap/pentip_cover_objmat4_list.pkl", "rb"))[1070]
    #
    # get_normals(get_objpcd(objcm, sample_num=10000))
    # pcd, pcd_normals = get_objpcd_withnormals(objcm, sample_num=100000)
    # for i, p in enumerate(pcd):
    #     base.pggen.plotArrow(base.render, spos=p, epos=p + 10 * pcd_normals[i])
    # base.run()
    get_objpcd_partial_bycampos(objcm, sample_num=10000, toggledebug=True)