Python open3d.Vector3dVector方法代码示例

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def vis_pc(xyz, color_axis=-1, rgb=None):
    # TODO move to the other module and do import in the module
    import open3d
    pcd = open3d.PointCloud()
    pcd.points = open3d.Vector3dVector(xyz)

    if color_axis >= 0:
        if color_axis == 3:
            axis_vis = np.arange(0, xyz.shape[0], dtype=np.float32)
        else:
            axis_vis = xyz[:, color_axis]
        min_ = np.min(axis_vis)
        max_ = np.max(axis_vis)

        colors = cm.gist_rainbow((axis_vis - min_) / (max_ - min_))[:, 0:3]
        pcd.colors = open3d.Vector3dVector(colors)
    if rgb is not None:
        pcd.colors = open3d.Vector3dVector(rgb)

    open3d.draw_geometries([pcd]) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def do_icp(self, name1, name2, method, init=np.eye(4), radius=0.2, constrain=False):
        pc1 = o3.geometry.PointCloud()
        pc1.points = o3.Vector3dVector(self.points[name1].points[:, :3])
        pc2 = o3.geometry.PointCloud()
        pc2.points = o3.Vector3dVector(self.points[name2].points[:, :3])

        start = time.time()
        sys.__stdout__.write(method + '\n')
        if method == 'p2p':
            reg_result = self._do_icp_p2p(pc1, pc2, radius, init, constrain=constrain)
        elif method == 'global':
            reg_result = self._do_icp_global(pc1, pc2, constrain=constrain)
        elif method == 'goicp':
            reg_result = self._do_icp_goicp(self.points[name1].points[:, :3], self.points[name2].points[:, :3], constrain=constrain)
        elif method == 'fast_global':
            reg_result = self._do_icp_fast_global(pc1, pc2, constrain=constrain)

        print(f'{method} {name1}->{name2} icp registration took {time.time() - start:.3} sec.', reg_result.transformation[:3, 3])

        before = o3.evaluate_registration(pc1, pc2, 0.02, init)
        after = o3.evaluate_registration(pc1, pc2, 0.02, reg_result.transformation)
        return [reg_result, before, after] 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def downsample_point_clouds():
    cfg = parse_arguments()

    vox_size = cfg.downsample_voxel_size
    synth_set = cfg.synth_set

    inp_dir = os.path.join(cfg.inp_dir, synth_set)
    files = glob.glob('{}/*.mat'.format(inp_dir))

    out_dir = cfg.out_dir
    out_synthset = os.path.join(out_dir, cfg.synth_set)
    mkdir_if_missing(out_synthset)

    for k, model_file in enumerate(files):
        print("{}/{}".format(k, len(files)))

        file_name = os.path.basename(model_file)
        sample_name, _ = os.path.splitext(file_name)

        obj = scipy.io.loadmat(model_file)

        out_filename = "{}/{}.mat".format(out_synthset, sample_name)
        if os.path.isfile(out_filename):
            print("already exists:", sample_name)
            continue

        Vgt = obj["points"]

        pcd = open3d.PointCloud()
        pcd.points = open3d.Vector3dVector(Vgt)
        downpcd = open3d.voxel_down_sample(pcd, voxel_size=vox_size)
        down_xyz = np.asarray(downpcd.points)
        scipy.io.savemat(out_filename, {"points": down_xyz}) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def draw_registration_result(src, trgt):
    source = open3d.PointCloud()
    source.points = open3d.Vector3dVector(src)
    target = open3d.PointCloud()
    target.points = open3d.Vector3dVector(trgt)
    source.paint_uniform_color([1, 0.706, 0])
    target.paint_uniform_color([0, 0.651, 0.929])
    open3d.draw_geometries([source, target]) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def open3d_icp(src, trgt, init_rotation=np.eye(3, 3)):
    source = open3d.PointCloud()
    source.points = open3d.Vector3dVector(src)

    target = open3d.PointCloud()
    target.points = open3d.Vector3dVector(trgt)

    init_rotation_4x4 = np.eye(4, 4)
    init_rotation_4x4[0:3, 0:3] = init_rotation

    threshold = 0.2
    reg_p2p = open3d.registration_icp(source, target, threshold, init_rotation_4x4,
                                    open3d.TransformationEstimationPointToPoint())

    return reg_p2p 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def load_pountclouds(file_idx, cfg, return_numpy=False):
    ps1 = np.load(f'{cfg.data.basepath}/pointcloud1/{str(file_idx).zfill(8)}.npy')[:, :3]
    ps2 = np.load(f'{cfg.data.basepath}/pointcloud2/{str(file_idx).zfill(8)}.npy')[:, :3]
    pc1_centroid = ps1.mean(axis=0)
    if return_numpy:
        return ps1, ps2, pc1_centroid
    pc1 = o3.geometry.PointCloud()
    pc1.points = o3.Vector3dVector(ps1)
    pc2 = o3.geometry.PointCloud()
    pc2.points = o3.Vector3dVector(ps2)
    return pc1, pc2, pc1_centroid 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def main():
    bot = Robot("locobot")
    vis = open3d.Visualizer()
    vis.create_window("3D Map")
    pcd = open3d.PointCloud()
    coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
    vis.add_geometry(pcd)
    vis.add_geometry(coord)
    # We update the viewer every a few seconds
    update_time = 4
    while True:
        start_time = time.time()
        pts, colors = bot.base.base_state.vslam.get_3d_map()
        pcd.clear()
        # note that open3d.Vector3dVector is slow
        pcd.points = open3d.Vector3dVector(pts)
        pcd.colors = open3d.Vector3dVector(colors / 255.0)
        vis.update_geometry()
        vis.poll_events()
        vis.update_renderer()
        s_t = time.time()
        while time.time() - s_t < update_time:
            vis.poll_events()
            vis.update_renderer()
            time.sleep(0.1)
        end_time = time.time()
        process_time = end_time - start_time
        print("Updating every {0:.2f}s".format(process_time)) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def main():
    parser = argparse.ArgumentParser(description="Argument Parser")
    parser.add_argument(
        "--floor_height", type=float, default=0.03, help="the z coordinate of the floor"
    )
    args = parser.parse_args()
    np.set_printoptions(precision=4, suppress=True)
    bot = Robot("locobot")
    bot.camera.set_pan_tilt(0, 0.7, wait=True)
    bot.arm.go_home()
    bot.arm.set_joint_positions([1.96, 0.52, -0.51, 1.67, 0.01], plan=False)
    vis = open3d.Visualizer()
    vis.create_window("3D Map")
    pcd = open3d.PointCloud()
    coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
    vis.add_geometry(pcd)
    vis.add_geometry(coord)
    while True:
        pts, colors = bot.camera.get_current_pcd(in_cam=False)
        pts, colors = filter_points(pts, colors, z_lowest=args.floor_height)
        pcd.clear()
        # note that open3d.Vector3dVector is slow
        pcd.points = open3d.Vector3dVector(pts)
        pcd.colors = open3d.Vector3dVector(colors / 255.0)
        vis.update_geometry()
        vis.poll_events()
        vis.update_renderer()
        time.sleep(0.1) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def main():
    bot = Robot("kinect2")
    vis = open3d.Visualizer()
    vis.create_window("3D Map")
    pcd = open3d.PointCloud()
    coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
    vis.add_geometry(pcd)
    vis.add_geometry(coord)
    # We update the viewer every a few seconds
    update_time = 4
    while True:
        start_time = time.time()
        pts, colors = bot.camera.get_current_pcd()
        pcd.clear()
        # note that open3d.Vector3dVector is slow
        pcd.points = open3d.Vector3dVector(pts)
        pcd.colors = open3d.Vector3dVector(colors / 255.0)
        vis.update_geometry()
        vis.poll_events()
        vis.update_renderer()
        s_t = time.time()
        while time.time() - s_t < update_time:
            vis.poll_events()
            vis.update_renderer()
            time.sleep(0.1)
        end_time = time.time()
        process_time = end_time - start_time
        print("Updating every {0:.2f}s".format(process_time)) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def create_wire_box(edgelengths, color=[0.0, 0.0, 0.0]):
    lineset = o3.LineSet()
    x, y, z = edgelengths
    lineset.points = o3.Vector3dVector([[0, 0, 0], [x, 0, 0], [0, y, 0], 
        [x, y, 0], [0, 0, z], [x, 0, z], [0, y, z], [x, y, z]])
    lineset.lines = o3.Vector2iVector([[0, 1], [1, 3], [3, 2], [2, 0],
        [0, 4], [1, 5], [3, 7], [2, 6],
        [4, 5], [5, 7], [7, 6], [6, 4]])
    lineset.colors = o3.Vector3dVector(np.tile(color, [len(lineset.lines), 1]))
    return lineset 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def xyzi2pc(xyz, intensities=None):
    pc = o3.PointCloud()
    pc.points = o3.Vector3dVector(xyz)
    if intensities is not None:
        pc.colors = o3.Vector3dVector(intensity2color(intensities / 255.0))
    return pc 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def load_snapshot(sessionname):
    cloud = o3.PointCloud()
    trajectory = o3.LineSet()
    with np.load(os.path.join(resultdir, sessionname, snapshotfile)) as data:
        cloud.points = o3.Vector3dVector(data['points'])
        cloud.colors = o3.Vector3dVector(
            util.intensity2color(data['intensities'] / 255.0))
        
        trajectory.points = o3.Vector3dVector(data['trajectory'])
        lines = np.reshape(range(data['trajectory'].shape[0] - 1), [-1, 1]) \
                + [0, 1]
        trajectory.lines = o3.Vector2iVector(lines)
        trajectory.colors = o3.Vector3dVector(
            np.tile([0.0, 0.5, 0.0], [lines.shape[0], 1]))
    return cloud, trajectory 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def save_local_maps(seq):
    print(seq)
    sequence = dataset.sequence(seq)
    seqdir = os.path.join('kitti', '{:03d}'.format(seq))
    util.makedirs(seqdir)
    istart, imid, iend = get_map_indices(sequence)
    maps = []
    with progressbar.ProgressBar(max_value=len(iend)) as bar:
        for i in range(len(iend)):
            scans = []
            for idx, val in enumerate(range(istart[i], iend[i])):
                velo = sequence.get_velo(val)
                scan = o3.PointCloud()
                scan.points = o3.Vector3dVector(velo[:, :3])
                scan.colors = o3.Vector3dVector(
                    util.intensity2color(velo[:, 3]))
                scans.append(scan)

            T_m_w = T_m_mc.dot(T_mc_cam0).dot(
                util.invert_ht(sequence.poses[imid[i]]))
            T_w_velo = np.matmul(
                sequence.poses[istart[i]:iend[i]], sequence.calib.T_cam0_velo)
            T_m_velo = np.matmul(T_m_w, T_w_velo)
            occupancymap = mapping.occupancymap(
                scans, T_m_velo, mapshape, mapsize)
            poleparams = poles.detect_poles(occupancymap, mapsize)

            # accuscan = o3.PointCloud()
            # for j in range(len(scans)):
            #     scans[j].transform(T_w_velo[j])
            #     accuscan.points.extend(scans[j].points)
            # o3.draw_geometries([accuscan])

            # import ndshow
            # ndshow.matshow(occupancymap.transpose([2, 0, 1]))

            map = {'poleparams': poleparams, 
                'istart': istart[i], 'imid': imid[i], 'iend': iend[i]}
            maps.append(map)
            bar.update(i)
    np.savez(os.path.join(seqdir, localmapfile), maps=maps) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def view_local_maps(seq):
    sequence = dataset.sequence(seq)
    seqdir = os.path.join('kitti', '{:03d}'.format(seq))
    with np.load(os.path.join(seqdir, localmapfile)) as data:
        for i, map in enumerate(data['maps']):
            T_w_m = sequence.poses[map['imid']].dot(T_cam0_mc).dot(T_mc_m)
            mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0])
            mapboundsvis.transform(T_w_m)
        
            polemap = []
            for poleparams in map['poleparams']:
                x, y, zs, ze, a, _ = poleparams
                pole = util.create_wire_box(
                    [a, a, ze - zs], color=[1.0, 1.0, 0.0])
                T_m_p = np.identity(4)
                T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs]
                pole.transform(T_w_m.dot(T_m_p))
                polemap.append(pole)

            accucloud = o3.PointCloud()
            for j in range(map['istart'], map['iend']):
                velo = sequence.get_velo(j)
                cloud = o3.PointCloud()
                cloud.points = o3.Vector3dVector(velo[:, :3])
                cloud.colors = o3.Vector3dVector(
                    util.intensity2color(velo[:, 3]))
                cloud.transform(
                    sequence.poses[j].dot(sequence.calib.T_cam0_velo))
                accucloud.points.extend(cloud.points)
                accucloud.colors.extend(cloud.colors)
            o3.draw_geometries([accucloud, mapboundsvis] + polemap) 

# 需要导入模块: import open3d [as 别名]
# 或者: from open3d import Vector3dVector [as 别名]
def initializePointCoud(pts):

    pcd = o3d.PointCloud()
    pcd.points = o3d.Vector3dVector(pts)

    return pcd