Ejemplo n.º 1
0
    def generate_images(self, salient_edge_set, n_samples=1):
        """Generate depth image, normal image, and binary edge mask tuples.

        Parameters
        ----------
        salient_edge_set : SalientEdgeSet
            A salient edge set to generate images of.
        n_samples : int
            The number of samples to generate.

        Returns
        -------
        depth_ims : (n,) list of perception.DepthImage
            Randomly-rendered depth images of object.
        normal_ims : (n,) list of perception.PointCloudImage
            Normals for the given image
        edge_masks : (n,) list of perception.BinaryImage
            Masks for pixels on the salient edges of the object.
        """
        # Compute stable poses of mesh
        mesh = salient_edge_set.mesh

        stp_pose_tfs, probs = mesh.compute_stable_poses()
        probs = probs / sum(probs)

        # Generate n renders
        depth_ims, normal_ims, edge_masks = [], [], []
        scene = Scene()
        so = SceneObject(mesh, RigidTransform(from_frame='obj', to_frame='world'))
        scene.add_object('object', so)

        for i in range(n_samples):
            # Sample random stable pose.
            tf_id = np.random.choice(np.arange(len(probs)), p=probs)
            tf = stp_pose_tfs[tf_id]
            T_obj_world = RigidTransform(tf[:3,:3], tf[:3,3], from_frame='obj', to_frame='world')
            so.T_obj_world = T_obj_world

            # Create the random uniform workspace sampler
            ws_cfg = self._config['worksurface_rv_config']
            uvs = UniformPlanarWorksurfaceImageRandomVariable('object', scene, [RenderMode.DEPTH], frame='camera', config=ws_cfg)

            # Sample and extract the depth image, camera intrinsics, and T_obj_camera
            sample = uvs.sample()
            depth_im = sample.renders[RenderMode.DEPTH]
            cs = sample.camera
            ci = CameraIntrinsics(frame='camera', fx=cs.focal, fy=cs.focal, cx=cs.cx, cy=cs.cy,
                                  skew=0.0, height=ws_cfg['im_height'], width=ws_cfg['im_width'])
            T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world)
            edge_mask = self._compute_edge_mask(salient_edge_set, depth_im, ci, T_obj_camera)
            point_cloud_im = ci.deproject_to_image(depth_im)
            normal_im =  point_cloud_im.normal_cloud_im()


            depth_ims.append(depth_im)
            normal_ims.append(normal_im)
            edge_masks.append(edge_mask)

        return depth_ims, normal_ims, edge_masks
Ejemplo n.º 2
0
def fine_grid_search(pc, indices, model, shadow, splits):
    length, width, height = shadow.extents
    split_size = max(length, width)
    pc_data, ind = get_pc_data(pc, indices)
    maxes = np.max(pc_data, axis=0)
    mins = np.min(pc_data, axis=0)
    bin_base = mins[2]
    plane_normal = model[0:3]
    #splits = 3
    step_size = split_size / splits
    
    plane_data = get_plane_data(pc, indices)
    plane_pc = PointCloud(plane_data.T, pc.frame)
    plane_pc = cp.inverse().apply(plane_pc)
    di = ci.project_to_image(plane_pc)
    bi = di.to_binary()
    bi = bi.inverse()

    scene = Scene()
    camera = VirtualCamera(ci, cp)
    scene.camera = camera
    shadow_obj = SceneObject(shadow)
    scene.add_object('shadow', shadow_obj)
    orig_tow = shadow_obj.T_obj_world

    numx = (int(np.round((maxes[0]-mins[0])/split_size)) - 1) * splits + 1
    numy = (int(np.round((maxes[1]-mins[1])/split_size)) - 1) * splits + 1
    scores = np.zeros((numx, numy))
    for i in range(numx):
        x = mins[0] + i*step_size
        for j in range(numy):
            y = mins[1] + j*step_size

            for tow in transforms(pc, pc_data, shadow, x, y, x+split_size, y+split_size, 8, orig_tow):
                shadow_obj.T_obj_world = tow
                scores[i][j] = under_shadow(scene, bi)
                shadow_obj.T_obj_world = orig_tow

    print("\nScores: \n" + str(scores))
    best = best_cell(scores)
    print("\nBest Cell: " + str(best) + ", with score = " + str(scores[best[0]][best[1]]))
    #-------
    # Visualize best placement
    vis3d.figure()
    x = mins[0] + best[0]*step_size
    y = mins[1] + best[1]*step_size
    cell_indices = np.where((x < pc_data[:,0]) & (pc_data[:,0] < x+split_size) & (y < pc_data[:,1]) & (pc_data[:,1] < y+split_size))[0]
    points = pc_data[cell_indices]
    rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
    vis3d.points(points, color=(0,1,1))
    vis3d.points(rest, color=(1,0,1))
    vis3d.show()
    #--------
    return best, scene
Ejemplo n.º 3
0
 def sample_scene_objs(self):
     model_pose = RigidTransform(
         rotation=np.eye(3),
         translation=np.array([0.0, 0.0, 0.0]),
         from_frame='obj',
         to_frame='world')  #model is already transformed
     for mesh, model_name in self.sample_models_set():
         model_material = self.sample_model_material()
         model_obj = SceneObject(mesh, model_pose, model_material)
         grip_obj = SceneObject(get_top_surface(mesh), model_pose,
                                model_material)
         yield model_obj, grip_obj, model_name
Ejemplo n.º 4
0
def do_stuff(pc, indices, model, rotated_shadow, img_file):
    scene = Scene()
    camera = VirtualCamera(ci, cp)
    scene.camera = camera

    # Works
    shadow_obj = SceneObject(rotated_shadow)
    scene.add_object('shadow', shadow_obj)
    wd = scene.wrapped_render([RenderMode.DEPTH])[0]
    wd_bi = wd.to_binary()
    vis2d.figure()
    vis2d.imshow(wd_bi)
    vis2d.show()

    # Doesn't work yet
    plane = pc.data.T[indices]
    plane_pc = PointCloud(plane.T, pc.frame)
    di = ci.project_to_image(plane_pc)
    bi = di.to_binary()
    vis2d.figure()
    vis2d.imshow(bi)
    vis2d.show()

    # Works
    both = bi.mask_binary(wd_bi)
    vis2d.figure()
    vis2d.imshow(both)
    vis2d.show()
Ejemplo n.º 5
0
 def sample_ground_obj(self):
     ground_mesh = trimesh.creation.box(self.GROUND_BOUND)
     ground_pose = RigidTransform(rotation=np.eye(3),
                                  translation=np.array([0.0, 0.0, 0.0]),
                                  from_frame='obj',
                                  to_frame='world')
     ground_material = self.sample_ground_material()
     ground_obj = SceneObject(ground_mesh, ground_pose, ground_material)
     return ground_obj
Ejemplo n.º 6
0
    def plot3d(points,
               color=(0.5, 0.5, 0.5),
               tube_radius=0.005,
               n_components=30,
               name=None):
        """Plot a 3d curve through a set of points using tubes.

        Parameters
        ----------
        points : (n,3) float
            A series of 3D points that define a curve in space.
        color : (3,) float
            The color of the tube.
        tube_radius : float
            Radius of tube representing curve.
        n_components : int
            The number of edges in each polygon representing the tube.
        name : str
            A name for the object to be added.
        """
        points = np.asanyarray(points)
        mp = MaterialProperties(color=np.array(color),
                                k_a=0.5,
                                k_d=0.3,
                                k_s=0.0,
                                alpha=10.0,
                                smooth=True)

        # Generate circular polygon
        vec = np.array([0, 1]) * tube_radius
        angle = np.pi * 2.0 / n_components
        rotmat = np.array([[np.cos(angle), -np.sin(angle)],
                           [np.sin(angle), np.cos(angle)]])
        perim = []
        for i in range(n_components):
            perim.append(vec)
            vec = np.dot(rotmat, vec)
        poly = Polygon(perim)

        # Sweep it out along the path
        mesh = trimesh.creation.sweep_polygon(poly, points)
        obj = SceneObject(mesh, material=mp)
        if name is None:
            name = str(uuid.uuid4())
        Visualizer3D._scene.add_object(name, obj)
Ejemplo n.º 7
0
def create_scene(camera, workspace_objects):

    # Start with an empty scene
    scene = Scene()

    # Create a VirtualCamera
    virt_cam = VirtualCamera(camera.intrinsics, camera.pose)

    # Add the camera to the scene
    scene.camera = virt_cam
    mp = MaterialProperties(
            color=np.array([0.3,0.3,0.3]),
            k_a=0.5, k_d=0.3, k_s=0.0, alpha=10.0
    )
    if camera.geometry is not None:
        so = SceneObject(camera.geometry, camera.pose.copy(), mp)
        scene.add_object(camera.name, so)

    return scene
Ejemplo n.º 8
0
def get_sim_point_cloud(scene, grasp_obj):
    
    # Remove old objects
    scene_objs = scene.objects.copy()
    if 'obj' in scene_objs:
        scene.remove_object('obj')
    
    # Get graspable object material properties and add to scene
    mp = hasattr(grasp_obj, 'material_properties')
    if not mp:
        mp = MaterialProperties(
            color=np.random.uniform(0.0, 1.0, size=3),
            k_a=0.5, k_d=0.3, k_s=0.0, alpha=10.0
        )
    so = SceneObject(grasp_obj.mesh, grasp_obj.T_obj_world.copy(), mp)
    scene.add_object(grasp_obj.key, so)

    # Create simulated pointcloud for ICP matching
    wrapped_depth = depth_scene.wrapped_render([RenderMode.DEPTH])
    sim_point_cloud = phoxi_tf*phoxi.intrinsics.deproject(wrapped_depth[0])
    sim_point_cloud_masked, _ = sim_point_cloud.box_mask(mask_box)
    return sim_point_cloud_masked
Ejemplo n.º 9
0
    def mesh(mesh,
             T_mesh_world=RigidTransform(from_frame='obj', to_frame='world'),
             style='surface',
             smooth=False,
             color=(0.5, 0.5, 0.5),
             name=None):
        """Visualize a 3D triangular mesh.

        Parameters
        ----------
        mesh : trimesh.Trimesh
            The mesh to visualize.
        T_mesh_world : autolab_core.RigidTransform
            The pose of the mesh, specified as a transformation from mesh frame to world frame.
        style : str
            Triangular mesh style, either 'surface' or 'wireframe'.
        smooth : bool
            If true, the mesh is smoothed before rendering.
        color : 3-tuple
            Color tuple.
        name : str
            A name for the object to be added.
        """
        if not isinstance(mesh, trimesh.Trimesh):
            raise ValueError('Must provide a trimesh.Trimesh object')

        mp = MaterialProperties(color=np.array(color),
                                k_a=0.5,
                                k_d=0.3,
                                k_s=0.1,
                                alpha=10.0,
                                smooth=smooth,
                                wireframe=(style == 'wireframe'))

        obj = SceneObject(mesh, T_mesh_world, mp)
        if name is None:
            name = str(uuid.uuid4())
        Visualizer3D._scene.add_object(name, obj)
Ejemplo n.º 10
0
def load_3d_model(model_path):
    # Start with an empty scene
    scene = Scene()
    # Add objects to the scene
    # Begin by loading meshes
    pawn_mesh = trimesh.load_mesh(model_path)
    # Set up object's pose in the world
    pawn_pose = RigidTransform(rotation=np.eye(3),
                               translation=np.array([0.0, 0.0, 0.0]),
                               from_frame='obj',
                               to_frame='world')
    # Set up each object's material properties
    pawn_material = MaterialProperties(color=np.array([1.0, 1.0, 1.0]),
                                       k_a=1.0,
                                       k_d=1.0,
                                       k_s=0.0,
                                       alpha=1.0,
                                       smooth=False,
                                       wireframe=False)
    # Create SceneObjects for each object
    pawn_obj = SceneObject(pawn_mesh, pawn_pose, pawn_material)
    # Add the SceneObjects to the scene
    scene.add_object('pawn', pawn_obj)
    return scene, pawn_mesh
Ejemplo n.º 11
0
                                   k_s=0.2,
                                   alpha=10.0,
                                   smooth=False,
                                   wireframe=False)
#bar_material = MaterialProperties(
#    color = 7.0*np.array([0.1, 0.1, 0.1]),
#    k_a = 0.5,
#    k_d = 0.3,
#    k_s = 0.1,
#    alpha = 10.0,
#    smooth=False
#)
bar_material = pawn_material

# Create SceneObjects for each object
pawn_obj = SceneObject(pawn_mesh, pawn_pose, pawn_material)
bar_obj = SceneObject(bar_mesh, bar_pose, bar_material)
pawn_inst_obj = InstancedSceneObject(pawn_mesh, [pawn_pose, bar_pose],
                                     colors=np.array([[0, 0, 1], [0, 1, 0]]),
                                     material=pawn_material)

# Add the SceneObjects to the scene
scene.add_object('pawn', pawn_inst_obj)
scene.add_object('bar', bar_obj)

#====================================
# Add lighting to the scene
#====================================

# Create an ambient light
ambient = AmbientLight(color=np.array([1.0, 1.0, 1.0]), strength=1.0)
Ejemplo n.º 12
0
 
 # read workspace bounds
 workspace_box = Box(np.array(workspace_config['min_pt']),
                     np.array(workspace_config['max_pt']),
                     frame='world')
 
 # read workspace objects
 workspace_objects = {}
 for obj_key, obj_config in workspace_config['objects'].iteritems():
     mesh_filename = obj_config['mesh_filename']
     pose_filename = obj_config['pose_filename']
     obj_mesh = trimesh.load_mesh(mesh_filename)
     obj_pose = RigidTransform.load(pose_filename)
     obj_mat_props = MaterialProperties(smooth=True,
                                        wireframe=False)
     scene_obj = SceneObject(obj_mesh, obj_pose, obj_mat_props)
     workspace_objects[obj_key] = scene_obj
     
 # setup each sensor
 datasets = {}
 sensors = {}
 sensor_poses = {}
 camera_intrs = {}
 workspace_ims = {}
 for sensor_name, sensor_config in sensor_configs.iteritems():
     # read params
     sensor_type = sensor_config['type']
     sensor_frame = sensor_config['frame']
     
     # read camera calib
     tf_filename = '%s_to_world.tf' %(sensor_frame)
Ejemplo n.º 13
0
    def generate_examples(self, salient_edge_set_filename, n_samples=1):
        """Generate RegistrationExamples for evaluating the algorithm.

        Parameters
        ----------
        salient_edge_set_filename : str
            A file containing the salient edge set to generate images of.
        n_samples : int
            The number of samples to generate.

        Returns
        -------
        list of RegistrationExample
            A list of RegistrationExamples.
        """
        # Compute stable poses of mesh
        salient_edge_set = SalientEdgeSet.load(salient_edge_set_filename)
        mesh = salient_edge_set.mesh

        stp_pose_tfs, probs = mesh.compute_stable_poses()
        probs = probs / sum(probs)

        # Generate n renders
        examples = []
        scene = Scene()
        so = SceneObject(mesh,
                         RigidTransform(from_frame='obj', to_frame='world'))
        scene.add_object('object', so)

        for i in range(n_samples):
            # Sample random stable pose.
            tf_id = np.random.choice(np.arange(len(probs)), p=probs)
            tf = stp_pose_tfs[tf_id]
            T_obj_world = RigidTransform(tf[:3, :3],
                                         tf[:3, 3],
                                         from_frame='obj',
                                         to_frame='world')
            so.T_obj_world = T_obj_world

            # Create the random uniform workspace sampler
            ws_cfg = self._config['worksurface_rv_config']
            uvs = UniformPlanarWorksurfaceImageRandomVariable(
                'object',
                scene, [RenderMode.DEPTH],
                frame='camera',
                config=ws_cfg)

            # Sample and extract the depth image, camera intrinsics, and T_obj_camera
            sample = uvs.sample()
            depth_im = sample.renders[RenderMode.DEPTH]
            cs = sample.camera
            ci = CameraIntrinsics(frame='camera',
                                  fx=cs.focal,
                                  fy=cs.focal,
                                  cx=cs.cx,
                                  cy=cs.cy,
                                  skew=0.0,
                                  height=ws_cfg['im_height'],
                                  width=ws_cfg['im_width'])
            T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world)
            examples.append(
                RegistrationExample(salient_edge_set_filename, depth_im, ci,
                                    T_obj_camera))

        return examples
Ejemplo n.º 14
0
        k_d = 1.0,
        k_s = 1.0,
        alpha = 10.0,
        smooth=False
    )
    sphere_material = MaterialProperties(
        color = np.array([0.1, 0.1, 0.5]),
        k_a = 0.3,
        k_d = 1.0,
        k_s = 1.0,
        alpha = 10.0,
        smooth=True
    )

    # Create SceneObjects for each object
    cube_obj = SceneObject(cube_mesh, cube_pose, cube_material)
    sphere_obj = SceneObject(sphere_mesh, sphere_pose, sphere_material)

    # Add the SceneObjects to the scene
    scene.add_object('cube', cube_obj)
    scene.add_object('sphere', sphere_obj)

    #====================================
    # Add lighting to the scene
    #====================================

    # Create an ambient light
    ambient = AmbientLight(
        color=np.array([1.0, 1.0, 1.0]),
        strength=1.0
    )
default_pose = RigidTransform(rotation=np.eye(3),
                              translation=np.array([0.0, 0.0, 0.0]),
                              from_frame='obj',
                              to_frame='world')

obj_material_properties = MaterialProperties(
    color=np.array([66, 134, 244]) / 255.,
    # color = 5.0*np.array([0.1, 0.1, 0.1]),
    k_a=0.3,
    k_d=0.5,
    k_s=0.2,
    alpha=10.0,
    smooth=False,
    wireframe=False)

obj = SceneObject(mesh, default_pose, obj_material_properties)
scene.add_object('to_render', obj)
print("ADDED OBJECT SUCCESSFULLY")

# table_obj_properties = MaterialProperties(
#                       color = np.array([0, 0, 0]),
#                       )

# wrap the table as a SceneObject
# table_mesh = trimesh.load(PLANE_MESH)
# T_table_world = RigidTransform.load(PLANE_POSE)
# table = SceneObject(table_mesh, T_table_world, table_obj_properties)
# scene.add_object('table', table)

# add light
ambient = AmbientLight(color=np.array([1.0, 1.0, 1.0]), strength=1.0)
Ejemplo n.º 16
0
def fast_grid_search(pc, indices, model, shadow):
    length, width, height = shadow.extents
    split_size = max(length, width)
    pc_data, ind = get_pc_data(pc, indices)
    maxes = np.max(pc_data, axis=0)
    mins = np.min(pc_data, axis=0)
    bin_base = mins[2]
    plane_normal = model[0:3]

    #di_temp = ci.project_to_image(pc)
    #vis2d.figure()
    #vis2d.imshow(di_temp)
    #vis2d.show()
    #plane_data = pc.data.T[indices]
    #plane_pc = PointCloud(plane_data.T, pc.frame)
    #di = ci.project_to_image(plane_pc)
    #bi = di.to_binary()

    plane_data = get_plane_data(pc, indices)
    plane_pc = PointCloud(plane_data.T, pc.frame)
    #vis3d.figure()
    #vis3d.points(plane_pc)
    #vis3d.show()
    plane_pc = cp.inverse().apply(plane_pc)
    di = ci.project_to_image(plane_pc)
    bi = di.to_binary()
    bi = bi.inverse()
    #vis2d.figure()
    #vis2d.imshow(bi)
    #vis2d.show()

    scene = Scene()
    camera = VirtualCamera(ci, cp)
    scene.camera = camera
    shadow_obj = SceneObject(shadow)
    scene.add_object('shadow', shadow_obj)
    orig_tow = shadow_obj.T_obj_world
    #tr = transforms(pc, pc_data, shadow, mins[0], mins[1], mins[0]+split_size, mins[1]+split_size, 8, orig_tow)
    #shadow_obj.T_obj_world = tr[0]
    wd = scene.wrapped_render([RenderMode.DEPTH])[0]
    wd_bi = wd.to_binary()
    #vis2d.figure()
    #vis2d.imshow(wd_bi)
    #vis2d.show()

    scores = np.zeros((int(np.round((maxes[0]-mins[0])/split_size)), int(np.round((maxes[1]-mins[1])/split_size))))
    for i in range(int(np.round((maxes[0]-mins[0])/split_size))):
        x = mins[0] + i*split_size
        for j in range(int(np.round((maxes[1]-mins[1])/split_size))):
            y = mins[1] + j*split_size

            for tow in transforms(pc, pc_data, shadow, x, y, x+split_size, y+split_size, 8, orig_tow):
                shadow_obj.T_obj_world = tow
                scores[i][j] = under_shadow(scene, bi)
                shadow_obj.T_obj_world = orig_tow

 
    print("\nScores: \n" + str(scores))
    best = best_cell(scores)
    print("\nBest Cell: " + str(best) + ", with score = " + str(scores[best[0]][best[1]]))
    #-------
    # Visualize best placement
    vis3d.figure()
    x = mins[0] + best[0]*split_size
    y = mins[1] + best[1]*split_size
    cell_indices = np.where((x < pc_data[:,0]) & (pc_data[:,0] < x+split_size) & (y < pc_data[:,1]) & (pc_data[:,1] < y+split_size))[0]
    points = pc_data[cell_indices]
    rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
    vis3d.points(points, color=(0,1,1))
    vis3d.points(rest, color=(1,0,1))
    vis3d.show()
Ejemplo n.º 17
0
def fast_grid_search(pc, indices, model, shadow, img_file):
    length, width, height = shadow.extents
    split_size = max(length, width)
    pc_data, ind = get_pc_data(pc, indices)
    maxes = np.max(pc_data, axis=0)
    mins = np.min(pc_data, axis=0)
    bin_base = mins[2]
    plane_normal = model[0:3]

    di_temp = ci.project_to_image(pc)
    vis2d.figure()
    vis2d.imshow(di_temp)
    vis2d.show()

    plane_data = pc.data.T[indices]
    #all_indices = np.where([(plane_data[::,2] > 0.795) & (plane_data[::,2] < 0.862)])
    #all_indices = np.where((plane_data[::,1] < 0.16) & (plane_data[::,1] > -0.24) & (plane_data[::,0] > -0.3) & (plane_data[::,0] < 0.24))[0]
    #plane_data = plane_data[all_indices]

    plane_pc = PointCloud(plane_data.T, pc.frame)
    di = ci.project_to_image(plane_pc)
    bi = di.to_binary()

    scene = Scene()
    camera = VirtualCamera(ci, cp)
    scene.camera = camera
    # Get shadow depth img.
    shadow_obj = SceneObject(shadow)
    scene.add_object('shadow', shadow_obj)

    orig_tow = shadow_obj.T_obj_world

    scores = np.zeros((int(np.round((maxes[0] - mins[0]) / split_size)),
                       int(np.round((maxes[1] - mins[1]) / split_size))))
    for i in range(int(np.round((maxes[0] - mins[0]) / split_size))):
        x = mins[0] + i * split_size
        for j in range(int(np.round((maxes[1] - mins[1]) / split_size))):
            y = mins[1] + j * split_size

            for tow in transforms(pc, pc_data, shadow, x, y, x + split_size,
                                  y + split_size, 8):
                shadow_obj.T_obj_world = tow
                scores[i][j] = under_shadow(pc, pc_data, indices, model,
                                            shadow, x, x + split_size, y,
                                            y + split_size, scene, bi)
                shadow_obj.T_obj_world = orig_tow

    print("\nScores: \n" + str(scores))
    best = best_cell(scores)
    print("\nBest Cell: " + str(best) + ", with score = " +
          str(scores[best[0]][best[1]]))
    #-------
    # Visualize best placement
    vis3d.figure()
    x = mins[0] + best[0] * split_size
    y = mins[1] + best[1] * split_size
    cell_indices = np.where((x < pc_data[:, 0])
                            & (pc_data[:, 0] < x + split_size)
                            & (y < pc_data[:, 1])
                            & (pc_data[:, 1] < y + split_size))[0]
    points = pc_data[cell_indices]
    rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
    vis3d.points(points, color=(0, 1, 1))
    vis3d.points(rest, color=(1, 0, 1))
    vis3d.show()