def fragmentation_fps_test(): output_dir = 'fragmentation_test_output' misc.ensure_dir(output_dir) datasets = ['hb', 'ycbv', 'tless', 'lmo', 'icbin', 'itodd', 'tudl'] for dataset in datasets: model_type = None if dataset == 'tless': model_type = 'reconst' elif dataset == 'itodd': model_type = 'dense' dp_model = dataset_params.get_model_params(config.BOP_PATH, dataset, model_type) for obj_id in dp_model['obj_ids']: print('Fragmenting object {} from dataset {}...'.format( obj_id, dataset)) model_fpath = dp_model['model_tpath'].format(obj_id=obj_id) model = inout.load_ply(model_fpath) # Fragmentation by the furthest point sampling. frag_centers, vertex_frag_ids = \ fragment.fragmentation_fps(model['pts'], num_frags=256) # Fragment colors. frag_colors = frag_centers - frag_centers.min() frag_colors = (255.0 * frag_colors / frag_colors.max()).astype( np.uint8) # Color the model points by the fragment colors. pts_colors = np.zeros((model['pts'].shape[0], 3), np.uint8) for frag_id in range(len(frag_centers)): pts_colors[vertex_frag_ids == frag_id] = frag_colors[frag_id] inout.save_ply( os.path.join( output_dir, '{}_obj_{:02d}_fragments.ply'.format(dataset, obj_id)), { 'pts': model['pts'], 'faces': model['faces'], 'colors': pts_colors })
def get_tgt_model(obj_id, renderer): """ Load the target object with given [obj_id] as Klampt PointCloud, numpy array, mesh and add it to renderer. """ path_ply = os.path.join(YCBV_PATH, f"models_eval/obj_{obj_id:06d}.ply") cloud = resource.get(path_ply).convert('PointCloud') cloud.transform(obj_roff[obj_id - 1].T.reshape(9).tolist(), obj_toff[obj_id - 1].reshape(3).tolist()) ply = inout.load_ply(path_ply) ply['pts'] = (obj_roff[obj_id - 1] @ ply['pts'].T + obj_toff[obj_id - 1].reshape(3, 1)).T mesh = trimesh.load(path_ply) T = np.eye(4) T[:3, :3] = obj_roff[obj_id - 1] T[:3, 3] = obj_toff[obj_id - 1] mesh = mesh.apply_transform(T) renderer.add_object(obj_id, os.path.join(YCBV_PATH, f"models/obj_{obj_id:06d}.ply"), offset=[obj_roff[obj_id - 1], obj_toff[obj_id - 1]]) return cloud, ply, mesh
split_type_str = ' - ' + split_type if split_type is not None else '' # Load dataset parameters. dp_split = dataset_params.get_split_params(p['datasets_path'], dataset, split, split_type) model_type = 'eval' dp_model = dataset_params.get_model_params(p['datasets_path'], dataset, model_type) # Load object models. models = {} if p['error_type'] in ['ad', 'add', 'adi', 'mssd', 'mspd', 'proj']: misc.log('Loading object models...') for obj_id in dp_model['obj_ids']: models[obj_id] = inout.load_ply( dp_model['model_tpath'].format(obj_id=obj_id)) # Load models info. models_info = None if p['error_type'] in ['ad', 'add', 'adi', 'vsd', 'mssd', 'mspd', 'cus']: models_info = inout.load_json(dp_model['models_info_path'], keys_to_int=True) # Get sets of symmetry transformations for the object models. models_sym = None if p['error_type'] in ['mssd', 'mspd']: models_sym = {} for obj_id in dp_model['obj_ids']: models_sym[obj_id] = misc.get_symmetry_transformations( models_info[obj_id], p['max_sym_disc_step'])
th_outlier = [th_outliers[m_id]] #provid a fixed outlier value print("Set outlier threshold to ", th_outlier[0]) recog_temp = recog.pix2pose(weight_fn, camK=cam_K, res_x=im_width, res_y=im_height, obj_param=obj_param, th_ransac=th_ransac, th_outlier=th_outlier, th_inlier=th_inlier, backbone=backbone) obj_pix2pose.append(recog_temp) obj_names.append(model_id) ply_fn = model_plys[m_id] if (gpu_rendering): obj_model = inout.load_ply(ply_fn) obj_model['pts'] = obj_model['pts'] * 0.001 #mm to m scale else: obj_model = Model3D() obj_model.load(ply_fn, scale=0.001) #mm to m scale obj_models.append(obj_model) scales = obj_param[:3] / 1000 obj_dia = np.sqrt(scales[0]**2 + scales[1]**2 + scales[2]**2) obj_diameter.append(obj_dia) test_target_fn = cfg['test_target'] target_list = bop_io.get_target_list( os.path.join(bop_dir, test_target_fn + ".json")) def fcn(diff_depth):
def add_object(self, obj_id, model_path, **kwargs): """See base class.""" # Color of the object model (the original color saved with the object model # will be used if None). surf_color = None if 'surf_color' in kwargs: surf_color = kwargs['surf_color'] # Load the object model. model = inout.load_ply(model_path) self.models[obj_id] = model # Calculate the 3D bounding box of the model (will be used to set the near # and far clipping plane). bb = misc.calc_3d_bbox(model['pts'][:, 0], model['pts'][:, 1], model['pts'][:, 2]) self.model_bbox_corners[obj_id] = np.array([ [bb[0], bb[1], bb[2]], [bb[0], bb[1], bb[2] + bb[5]], [bb[0], bb[1] + bb[4], bb[2]], [bb[0], bb[1] + bb[4], bb[2] + bb[5]], [bb[0] + bb[3], bb[1], bb[2]], [bb[0] + bb[3], bb[1], bb[2] + bb[5]], [bb[0] + bb[3], bb[1] + bb[4], bb[2]], [bb[0] + bb[3], bb[1] + bb[4], bb[2] + bb[5]], ]) # Set texture/color of vertices. self.model_textures[obj_id] = None # Use the specified uniform surface color. if surf_color is not None: colors = np.tile( list(surf_color) + [1.0], [model['pts'].shape[0], 1]) # Set UV texture coordinates to dummy values. texture_uv = np.zeros((model['pts'].shape[0], 2), np.float32) # Use the model texture. elif 'texture_file' in self.models[obj_id].keys(): model_texture_path = os.path.join( os.path.dirname(model_path), self.models[obj_id]['texture_file']) model_texture = inout.load_im(model_texture_path) # Normalize the texture image. if model_texture.max() > 1.0: model_texture = model_texture.astype(np.float32) / 255.0 model_texture = np.flipud(model_texture) self.model_textures[obj_id] = model_texture # UV texture coordinates. texture_uv = model['texture_uv'] # Set the per-vertex color to dummy values. colors = np.zeros((model['pts'].shape[0], 3), np.float32) # Use the original model color. elif 'colors' in model.keys(): assert (model['pts'].shape[0] == model['colors'].shape[0]) colors = model['colors'] if colors.max() > 1.0: colors /= 255.0 # Color values are expected in range [0, 1]. # Set UV texture coordinates to dummy values. texture_uv = np.zeros((model['pts'].shape[0], 2), np.float32) # Set the model color to gray. else: colors = np.ones((model['pts'].shape[0], 3), np.float32) * 0.5 # Set UV texture coordinates to dummy values. texture_uv = np.zeros((model['pts'].shape[0], 2), np.float32) # Set the vertex data. if self.mode == 'depth': vertices_type = [('a_position', np.float32, 3), ('a_color', np.float32, colors.shape[1])] vertices = np.array(list(zip(model['pts'], colors)), vertices_type) else: if self.shading == 'flat': vertices_type = [('a_position', np.float32, 3), ('a_color', np.float32, colors.shape[1]), ('a_texcoord', np.float32, 2)] vertices = np.array( list(zip(model['pts'], colors, texture_uv)), vertices_type) elif self.shading == 'phong': vertices_type = [('a_position', np.float32, 3), ('a_normal', np.float32, 3), ('a_color', np.float32, colors.shape[1]), ('a_texcoord', np.float32, 2)] vertices = np.array( list( zip(model['pts'], model['normals'], colors, texture_uv)), vertices_type) else: raise ValueError('Unknown shading type.') # Create vertex and index buffer for the loaded object model. self.vertex_buffers[obj_id] = vertices.view(gloo.VertexBuffer) self.index_buffers[obj_id] = \ model['faces'].flatten().astype(np.uint32).view(gloo.IndexBuffer) # Set shader for the selected shading. if self.shading == 'flat': rgb_fragment_code = _rgb_fragment_flat_code elif self.shading == 'phong': rgb_fragment_code = _rgb_fragment_phong_code else: raise ValueError('Unknown shading type.') # Prepare the RGB OpenGL program. rgb_program = gloo.Program(_rgb_vertex_code, rgb_fragment_code) rgb_program.bind(self.vertex_buffers[obj_id]) if self.model_textures[obj_id] is not None: rgb_program['u_use_texture'] = int(True) rgb_program['u_texture'] = self.model_textures[obj_id] else: rgb_program['u_use_texture'] = int(False) rgb_program['u_texture'] = np.zeros((1, 1, 4), np.float32) self.rgb_programs[obj_id] = rgb_program # Prepare the depth OpenGL program. depth_program = gloo.Program(_depth_vertex_code, _depth_fragment_code) depth_program.bind(self.vertex_buffers[obj_id]) self.depth_programs[obj_id] = depth_program
def __init__(self, cfg): self.cfg = cfg self.rgb_topic = cfg['rgb_topic'] self.depth_topic = cfg['depth_topic'] self.camK = np.array(cfg['cam_K']).reshape(3, 3) self.im_width = int(cfg['im_width']) self.im_height = int(cfg['im_height']) self.inlier_th = float(cfg['inlier_th']) self.ransac_th = float(cfg['ransac_th']) self.pub_before_icp = False self.graph = tf_backend.Graph() if (int(cfg['icp']) == 1): self.icp = True else: self.icp = False self.model_params = inout.load_json(cfg['norm_factor_fn']) self.detection_labels = cfg[ 'obj_labels'] #labels of corresponding detections n_objs = int(cfg['n_objs']) self.target_objs = cfg['target_obj_name'] self.colors = np.random.randint(0, 255, (n_objs, 3)) with self.graph.as_default(): if (detect_type == "rcnn"): #Load mask r_cnn ''' standard estimation parameter for Mask R-CNN (identical for all dataset) ''' self.config = BopInferenceConfig(dataset="ros", num_classes=n_objs + 1, im_width=self.im_width, im_height=self.im_height) self.config.DETECTION_MIN_CONFIDENCE = 0.3 self.config.DETECTION_MAX_INSTANCES = 30 self.config.DETECTION_NMS_THRESHOLD = 0.5 self.detection_model = modellib.MaskRCNN(mode="inference", config=self.config, model_dir="/") self.detection_model.load_weights( cfg['path_to_detection_weights'], by_name=True) self.obj_models = [] self.obj_bboxes = [] self.obj_pix2pose = [] pix2pose_dir = cfg['path_to_pix2pose_weights'] th_outlier = cfg['outlier_th'] self.model_scale = cfg['model_scale'] for t_id, target_obj in enumerate(self.target_objs): weight_fn = os.path.join( pix2pose_dir, "{:02d}/inference.hdf5".format(target_obj)) print("Load pix2pose weights from ", weight_fn) model_param = self.model_params['{}'.format(target_obj)] obj_param = bop_io.get_model_params(model_param) recog_temp = recog.pix2pose(weight_fn, camK=self.camK, res_x=self.im_width, res_y=self.im_height, obj_param=obj_param, th_ransac=self.ransac_th, th_outlier=th_outlier, th_inlier=self.inlier_th) self.obj_pix2pose.append(recog_temp) ply_fn = os.path.join(self.cfg['model_dir'], self.cfg['ply_files'][t_id]) if (self.icp): #for pyrender rendering obj_model = trimesh.load_mesh(ply_fn) obj_model.vertices = obj_model.vertices * self.model_scale mesh = pyrender.Mesh.from_trimesh(obj_model) self.obj_models.append(mesh) self.obj_bboxes.append( self.get_3d_box_points(obj_model.vertices)) else: obj_model = inout.load_ply(ply_fn) self.obj_bboxes.append( self.get_3d_box_points(obj_model['pts'])) rospy.init_node('pix2pose', anonymous=True) self.detect_pub = rospy.Publisher("/pix2pose/detected_object", ros_image) #self.pose_pub = rospy.Publisher("/pix2pose/object_pose", Pose) self.pose_pub = rospy.Publisher("/pix2pose/object_pose", ros_image) self.have_depth = False if (self.icp): self.sub_depth = rospy.Subscriber(self.depth_topic, ros_image, self.callback_depth, queue_size=1) if (self.pub_before_icp): self.pose_pub_noicp = rospy.Publisher( "/pix2pose/object_pose_noicp", ros_image) self.depth_img = np.zeros((self.im_height, self.im_width)) self.sub = rospy.Subscriber(self.rgb_topic, ros_image, self.callback, queue_size=1)
misc.ensure_dir(os.path.dirname(out_rgb_tpath.format( out_path=out_path, obj_id=obj_id, im_id=0))) misc.ensure_dir(os.path.dirname(out_depth_tpath.format( out_path=out_path, obj_id=obj_id, im_id=0))) misc.ensure_dir(os.path.dirname(out_uv_tpath.format( out_path=out_path, obj_id=obj_id, im_id=0))) misc.ensure_dir(os.path.dirname(out_mask_tpath.format( out_path=out_path, obj_id=obj_id, im_id=0))) misc.ensure_dir(os.path.dirname(out_scene_camera_tpath.format( out_path=out_path, obj_id=obj_id))) misc.ensure_dir(os.path.dirname(out_scene_gt_tpath.format( out_path=out_path, obj_id=obj_id))) # Load model. model_path = dp_model['model_tpath'].format(obj_id=obj_id) model = inout.load_ply(model_path) model_uv_path = dp_model['model_uv_tpath'].format(obj_id=obj_id) model_uv = inout.load_ply(model_uv_path) # Load model texture. if 'texture_file' in model: model_texture_path =\ os.path.join(os.path.dirname(model_path), model['texture_file']) model_texture = inout.load_im(model_texture_path) else: model_texture = None model_uv_texture = None scene_camera = {} scene_gt = {} im_id = 0
ren_depth.add_object(obj_id, dp_model['model_tpath'].format(obj_id=obj_id)) # Render training images for all object models. for obj_id in obj_ids[:2]: # for debugging # Prepare output folders. misc.ensure_dir( os.path.dirname( out_rgb_tpath.format(out_path=out_path, obj_id=obj_id, im_id=0))) misc.ensure_dir( os.path.dirname( out_depth_tpath.format(out_path=out_path, obj_id=obj_id, im_id=0))) # Load model. model_path = dp_model['model_tpath'].format(obj_id=obj_id) model = inout.load_ply(model_path) # Load model texture. if 'texture_file' in model: model_texture_path =\ os.path.join(os.path.dirname(model_path), model['texture_file']) model_texture = inout.load_im(model_texture_path) else: model_texture = None scene_camera = {} scene_gt = {} im_id = 0 for radius in radii: # Sample viewpoints. view_sampler_mode = 'hinterstoisser' # 'hinterstoisser' or 'fibonacci'.