def read_charness_histograms(path): """ ['pigraph_norm_factor', 'pigraph_histogram_params', 'pigraph_histogram_charness', 'pigraph_pose_charness', 'pigraph_scenelet_names', 'categories'] """ hash_mat_file_current = hash_path_md5(path) hists = None path_pickle = "%s.pickle" % path if os.path.exists(path_pickle): with open(path_pickle, 'rb') as f: # hists, hash_mat_file = pickle_load(f) tmp = pickle_load(f) if tmp[-1] != hash_mat_file_current: hists = None lg.warning("Hashes don't match, reloading hists") else: if len(tmp) == 3: hists = tmp[1] # pose_charness, hists, hash else: hists = tmp[0] # hists, hash lg.info("Loaded hists from\n\t%s!!!" % split_path(path_pickle)) if hists is None: dmat = scipy.io.loadmat(path) hists = parse_charness_histograms(dmat) with open(path_pickle, 'wb') as f: pickle.dump((hists, hash_mat_file_current), f, -1) lg.info("Saved hists to %s" % path_pickle) # print(hists.keys()) # key = list(hists.keys())[0] # logging.info("key: %s, %s" % (key, hists[key].volume)) return hists
def find_gaps(skel, min_pad=3): """ :param skel: Input skeleton :param min_pad: Minimum number of existing frames to have. :return: list of frame_ids, [(start, end), ..] that start with min_pad number of existing frames, and end with min_pad number of existing frames, whilst having missing frames inbetween. """ frame_ids = skel.get_frames() indicator = np.zeros((frame_ids[-1] + 1), dtype=int) indicator[frame_ids] = 1 entries = rlencode(indicator) out = [] for i, (s, l, v) in enumerate(entries): if v == 1: continue assert entries[i - 1][2] == 1, "previous is also 0?" assert entries[i + 1][2] == 1, "next is also 0?" if entries[i - 1][1] < min_pad: lg.warning("Todo: extend beyond first neighbours: %s" % entries[i - 3:i]) continue if entries[i + 1][1] < min_pad: lg.warning("Todo: extend beyond first neighbours: %s" % entries[i:i + 4]) continue out.append((entries[i - 1][0] + entries[i - 1][1] - min_pad, entries[i + 1][0] + min_pad - 1)) # lg.debug("gap: %s" % repr((s, l, v))) out = sorted(out, key=lambda g: g[1] - g[0], reverse=True) return out
def intrinsics_matrix(scaled_height, shape_orig, camera_name): """ Estimate intrinsic matrix for a given camera :param shape_orig: height x width of original image (e.g. [1080, 1920] :param scaled_height: pose.config.INPUT_SIZE from Denis' config.py (368) """ K = np.eye(3, dtype='f4') width = np.float32(shape_orig[1] * scaled_height / shape_orig[0]) height = np.float32(scaled_height) K[0, 2] = np.float32(width / 2.) K[1, 2] = np.float32(height / 2.) if camera_name.lower() in {'s6', 'aron', 'galaxy s6'}: # calibration happened for 1280 x 720 K[0, 0] = np.float32((width / 1280.) * 1086.) K[1, 1] = np.float32((height / 720.) * 1085.) elif camera_name.lower() in {'g15', 'canon g15'}: # see above K[0, 0] = np.float32(width / 1920 * 1574.193548) K[1, 1] = np.float32(height / 1080 * 2098.924731) elif camera_name.lower() in {'t6', 'canon t6'}: # see above K[0, 0] = np.float32(width / 1920 * 1549.775784753) K[1, 1] = np.float32(height / 1080 * 1304.697986577) elif camera_name.lower() in {'a7r', 'sony a7r'}: # see above K[0, 0] = np.float32(width / 3840. * 3743.732590529) K[1, 1] = np.float32(height / 2160. * 3150.) elif camera_name.lower() in {'s8'}: lg.warning("TODO: calibrate S8") K[0, 0] = np.float32((width / 1280.) * 1086.) K[1, 1] = np.float32((height / 720.) * 1085.) else: raise RuntimeError("Unknown camera name: %s" % camera_name) return K
def create_intersection_jo_losses(pos_3d, obj_2d_polys, joints_active, pos_3d_transform_indices, obj_2d_poly_transform_indices, independent, um, d_padding, cat_ids_polys): """Joint-object intersection loss creator. Currently only applied to the average of the hip joints. If obj_2d_poly_transform_indices are None, no masking is applied, e.g. all points intersect all polygons. Args: pos_3d (tf.Variable): (N, 14, 3) Transformed poses in world space 3D, permuted! obj_2d_polys (tf.Variable): (M, 4, 3) Transformed top-view polygons in world space 3D. joints_active (list): A list of joint names for the last dimension of pos_3d. pos_3d_transform_indices (tf.Variable): (N, 1) Transformation ids of the poses, indicating which are moving together. Can be None, in which case there will be no check for scenelet-self intersection (poses in scenelets should not be penalized for intersecting the objects in the scenelets, we assume them to be correct). obj_2d_poly_transform_indices (tf.Variable): (M, 4) Transformation ids of the polygons, indicating which are moving together. Can be None, see above. independent (bool): Quads from dynamic scenelets don't intersect poses from dynamic scenelets. d_padding (float): Extra padding around objects in meters. Returns: loss_jo (tf.Variable): (N,) A loss per input pose, which is the sum of squared penetration distances. loss_jo_per_transform (tf.Variable): (n_transforms, ) A loss per transformation id. """ dtype_tf = pos_3d.dtype dtype_np = dtype_tf.as_numpy_dtype assert pos_3d.get_shape().as_list()[2] == 3, "Assumed xyz in last dim" assert len(joints_active) == pos_3d.get_shape().as_list()[1], \ "joints_active should describe the names of the joints in pos_3d:\n" \ "%s vs %s" % (joints_active, pos_3d.get_shape().as_list()) if Joint.LHIP not in joints_active or Joint.RHIP not in joints_active: raise RuntimeError("Need hips to compute intersection term.") # pelvis is hip average (no pelvis in features, so no pelvis in pos_3d) hips = tf.divide( tf.add(pos_3d[:, joints_active[Joint.LHIP], :], pos_3d[:, joints_active[Joint.RHIP], :]), dtype_np(2.)) # It's enough, if all lower limbs are outside, sits have pelvis inside joints_tup = ( hips, pos_3d[:, joints_active[Joint.LKNE], :], pos_3d[:, joints_active[Joint.RKNE]], # added 18/9/2018 pos_3d[:, joints_active[Joint.LANK]], pos_3d[:, joints_active[Joint.RANK]]) joints = tf.concat(joints_tup, axis=0, name="joint_object_intersection_joints") # joint-object distances d3 = PointPolyDistanceEstimator.point_poly_distance( p=joints, poly=obj_2d_polys, name='joint_object_distances_all') _n = hips.get_shape().as_list()[0] d3_2d = tf.reshape(d3, (len(joints_tup), _n, d3.get_shape().as_list()[1]), name='joint_object_distances_per_pose') # max over distances, where negative distance means inside # shape: (vertices, joints), e.g. (223, 3) # d = tf.reduce_max(d3_2d, axis=0, name='joint_object_distances_raw') d_all_outside = tf.reduce_min(d3_2d[1:, ...], axis=0, name='knees_ankles_to_object_distances_raw') # pdb.set_trace() d_for_tables = d3_2d[0, ...] d_for_other = tf.minimum(d3_2d[0, ...], d_all_outside, name='joint_object_distances_raw') CAT_TABLE = CATEGORIES['table'] is_table_tiled = tf.tile(tf.equal(cat_ids_polys, CAT_TABLE), multiples=(_n, 1), name='is_table_tiled') d = tf.where(condition=is_table_tiled, x=d_for_tables, y=d_for_other) # with tf.Session() as session: # session.run(tf.global_variables_initializer()) # print("_n: %s" % _n) # pdb.set_trace() # sys.exit(0) # od3_2d = d3_2d.eval() # o_d = d.eval() # od3 = d3.eval() # for j in range(3): # for i in range(_n): # lg.debug("checking %s" % repr((i,j))) # assert np.allclose(od3[j * _n + i, ...], od3_2d[j, i, ...]), \ # "No" # for i in range(_n): # lg.debug("\nvalues are\n%s, min is\n%s" # % (od3_2d[:, i, ...], o_d[i, ...])) if abs(d_padding) > 1e-3: d = tf.subtract(d, d_padding, name="joint_object_distances_padded") # mask inside mask_neg = tf.less(d, dtype_np(0.), 'jo_mask_neg') # lg.debug("mask: %s" % mask_neg) # mask same scenelet distances mask_same = tf.not_equal(tf.cast(pos_3d_transform_indices, tf.int64), tf.cast(obj_2d_poly_transform_indices[:, 0], tf.int64), name='jo_mask_same') if independent and um.has_static(): mask_static_pose = tf.greater_equal(pos_3d_transform_indices, um.tid_static, name='mask_static_pose') mask_static_quad = tf.greater_equal(obj_2d_poly_transform_indices[:, 0], um.tid_static, name='mask_static_quad') xor = tf.logical_xor(mask_static_pose, mask_static_quad, name="static_dynamic_xor") mask_same = tf.logical_and(mask_same, xor, name='jo_mask_same_xor') lg.warning("TODO: check for static objects") mask = tf.logical_and(mask_neg, mask_same, name='jo_mask') # else: # assert False # mask = mask_neg # distance masked: (N, M) d = tf.where(mask, d, tf.zeros_like(d), name='jo_distances') # # loss # # per pose and quad loss_jo_unnorm_2d = tf.square(d, name='loss_jo_unn_2d') # per pose loss_jo_unnorm = tf.reduce_sum(input_tensor=loss_jo_unnorm_2d, axis=1, name='loss_jo_unnormalized') # if um.has_static(): # with tf.Session() as session: # session.run(tf.global_variables_initializer()) # o = mask_same.eval() # o2 = xor.eval() # sys.exit(0) normalizer = tf.cast(x=joints.get_shape().as_list()[0], dtype=dtype_tf, name='loss_jo_normalizer') loss_jo = tf.divide( loss_jo_unnorm, # sqr. distances per joint normalizer, name='loss_jo_normalized') # # loss per transform # lg.warning("TODOOOO: add static poses to loss of quads' transforms.") pos_3d_transform_indices_1d = tf.squeeze(pos_3d_transform_indices, axis=-1) normalizer_per_transform = tf.segment_sum( data=tf.ones_like(pos_3d_transform_indices_1d, dtype=d.dtype), segment_ids=pos_3d_transform_indices_1d, name='normalizer_loss_jo_per_transform') loss_jo_per_transform = tf.Variable( initial_value=np.zeros(shape=(um.get_n_transforms), dtype=dtype_np)) loss_jo_per_transform = tf.scatter_add(ref=loss_jo_per_transform, indices=pos_3d_transform_indices_1d, updates=loss_jo_unnorm, name='loss_jo_per_transform') loss_jo_per_transform = tf.divide(x=loss_jo_per_transform, y=normalizer_per_transform, name='loss_jo_per_transform_normalized') # # static-dynamic interactions # # e.g. a scenelet candidate's object is intersecting the initial path # or an already placed scenelet's poses, then the cost of that should # go to the object's scenelet and not to the already placed scenelet # the poses belong to. (static pose - dynamic quad) if um.has_static() and independent: # # static poses' costs should be assigned to dynamic quads # mask_static_pose_dynamic_quad = tf.logical_and( x=xor, y=mask_static_pose, name='mask_static_pose_dynamic_quad') static_pose_costs_2d = tf.multiply( loss_jo_unnorm_2d, tf.cast(mask_static_pose_dynamic_quad, loss_jo_unnorm_2d.dtype), name='static_pose_costs_per_dyn_quad_2d') static_pose_costs = tf.reduce_sum( static_pose_costs_2d, axis=0, name='static_pose_costs_per_dyn_quad') static_pose_costs_per_transform_unn = tf.Variable( initial_value=np.zeros(shape=um.get_n_transforms), dtype=dtype_np) static_pose_costs_per_transform_unn = tf.scatter_add( ref=static_pose_costs_per_transform_unn, indices=obj_2d_poly_transform_indices[:, 0], updates=static_pose_costs, name='static_pose_costs_per_transform_unn') static_pose_normalizer_per_transform = tf.Variable( initial_value=np.zeros(shape=um.get_n_transforms), dtype=dtype_np) static_pose_normalizer_per_transform = tf.scatter_add( ref=static_pose_normalizer_per_transform, indices=obj_2d_poly_transform_indices, updates=tf.ones_like(obj_2d_poly_transform_indices, dtype=d.dtype), name='static_pose_costs_per_transform_normalizer') loss_jo_per_transform += tf.divide( static_pose_costs_per_transform_unn, static_pose_normalizer_per_transform, name='static_pose_costs_per_transform') # if um.has_static(): # with tf.Session() as session: # session.run(tf.global_variables_initializer()) # o_mspdq = mask_static_pose_dynamic_quad.eval() # o_static_pose_costs = static_pose_costs.eval() # o = static_pose_costs_per_transform_unn.eval() # o2 = loss_jo_per_transform.eval() # sys.exit(0) return loss_jo, loss_jo_per_transform, \ {'joints': joints, 'd': d}
def read_scenelets(scenelet_dir, filter_lambda=None, transform=None, skel_only=True): """Recursively reads a directory containing scenelets (json files). Args: scenelet_dir (str): Path to scan for scenelets. filter_lambda (Callable[[Scenelet], bool]): Takes a scenelet, and returns False for reject, True for keep. transform (np.ndarray): Transformation to apply to scenelets. skel_only (bool): Read only files starting with 'skel'. Returns: py_scenes (Dict[str, Dict[str, Scenelet]]): Scenelets keyed by their scene name and their recording name. """ py_scenes = {} for parent, dirs, files in os.walk(scenelet_dir): name_scene = None for f in [f for f in files if f.endswith('.json')]: if skel_only and not f.startswith('skel'): continue if '__' in f: name_parts = f.split('__') # lg.debug("name_parts: %s" % name_parts) name_scene = name_parts[0] if name_scene.startswith('skel_'): name_scene = name_scene[5:] name_sclt = "skel_%s" % os.path.splitext(name_parts[1])[0] else: if not name_scene: name_scene = os.path.basename(os.path.split(parent)[-1]) if not len(name_scene): name_scene = \ os.path.basename(os.path.split(parent[:-1])[-1]) py_scenes[name_scene] = {} name_sclt = os.path.splitext(f)[0] # lg.debug("name scene: %s, name_scenelet: %s" # % (name_scene, name_sclt)) j_path = os.path.join(parent, f) sclt = Scenelet.load(j_path) # save to output sclt.name_scene = name_scene sclt.name_scenelet = name_sclt if filter_lambda and not filter_lambda(sclt): # lg.debug("filtering %s" % sclt) continue if transform is not None: sclt.apply_transform(transform) try: while name_sclt in py_scenes[name_scene]: lg.warning("Modifying scenelet map key in order not to " "overwrite %s in %s, (keys; %s)" % (name_sclt, name_scene, sorted(list(py_scenes[name_scene].keys())))) name_sclt = "%s_" % name_sclt py_scenes[name_scene][name_sclt] = sclt except KeyError: py_scenes[name_scene] = {name_sclt: sclt} # lg.info("Added %s" % sclt) del name_sclt return py_scenes
def main(argv=None): np.set_printoptions(suppress=True) parser = argparse.ArgumentParser() parser.add_argument('d', help="Folder of scene") parser.add_argument('-resolution', help='Target resolution for occupancy map', default=0.1) parser.add_argument( '-thresh-area', help='Ratio of occupancy map cell area that has to be occupied ' 'for it to count as occupied', default=0.1) parser.add_argument('-postfix', type=str, help="Scene postfix for augmentation", default="") args = parser.parse_args(argv if argv is not None else sys.argv) res_target = args.resolution if args.postfix and len(args.postfix) and not args.postfix.startswith('_'): args.postfix = "_%s" % args.postfix path_parent, name_input = os.path.split(os.path.abspath(args.d)) lg.warning("name input: %s" % name_input) path_for_tf = os.path.abspath( os.path.join(path_parent, os.pardir, 'dataset')) # if 'video' not in path_parent else os.path.join(path_parent, 'dataset') if not os.path.exists(path_for_tf): os.makedirs(path_for_tf, mode=0o0775) lg.debug("Loading scenelet...") path_scenelet = os.path.join(args.d, "skel_%s.json" % name_input) scenelet = Scenelet.load(path_scenelet) lg.debug("Scenelet: %s" % scenelet) path_state_pickle = os.path.join(args.d, "state%s.pickle" % args.postfix) if not os.path.exists(path_state_pickle): lg.error("Does not exist: %s" % path_state_pickle) return False # assert os.path.exists(path_state_pickle), \ # "Does not exist: %s" % path_state_pickle lg.debug("Loading volume...") state = pickle_load(open(path_state_pickle, 'rb')) lg.debug("Loaded volume...") lg.debug("Creating scene from scenelet") if not no_vis: vis = Visualizer(win_size=(1024, 1024)) vis.add_coords() else: vis = None # scene = Scene(scenelet.name_scenelet) # colors = {0: (200., 0., 0.), 1: (0., 200., 0.), 2: (0., 0., 200.)} # unit_x = np.array((1., 0., 0.)) occup = State(room=state.room, tr_ground_inv=None, res_theta=state.resolution[3], resolution=[res_target, res_target, res_target]) occup.get_volume(labels_to_lin_ids_arg=state.get_labels_to_lin_ids()) occup_angle = np.ones(shape=(len( occup.volume), occup.volume[0].shape[0], occup.volume[0].shape[1], 1), dtype=np.float32) * -1. assert np.min(occup_angle) < 0. and np.max(occup_angle) < 0., "Not empty" grid_polys = get_grid_shapely(occup=occup, res_orig=state.resolution) occup.volume.flags.writeable = True volume_occp = occup.volume angles = sorted(state.get_angles()) labels_to_lin_ids = occup.get_labels_to_lin_ids() had_vtk_problem = no_vis plt.figure() rects = [] for oid, ob in scenelet.objects.items(): assert oid >= 0, "Need positive here" label = ob.label if label in TRANSLATIONS_CATEGORIES: label = TRANSLATIONS_CATEGORIES[label] if label not in labels_to_lin_ids: continue try: poly = get_poly([part.obb for part in ob.parts.values()]) except ValueError as e: print("\n===========\n\nShapely error: %s for %s\n\n" % (e, (label, oid, ob))) with open('error.log', 'a') as f: f.write("[%s] %d, %s, %s\n" % (args.d, oid, label, ob)) continue ob_angle = ob.get_angle(positive_only=True) assert 0. <= ob_angle <= 2 * np.pi, "No: %g" % ob_angle rect = get_rectangle(poly, ob_angle) rect.extend([oid, CATEGORIES[label]]) rects.append(rect) cat_id = labels_to_lin_ids[label] # cat_id in volume, not categories for gp in grid_polys: # skip, if not occupied enough if gp.poly.intersection(poly).area / gp.area < args.thresh_area: continue # save occupancy gp.occupancy = 1. id_angle_lower = None id_angle_upper = None if ob_angle > angles[-1]: id_angle_lower = len(angles) - 1 id_angle_upper = 0 else: for id_angle, angle in enumerate(angles): if ob_angle < angle: id_angle_upper = id_angle id_angle_lower = id_angle - 1 break assert id_angle_lower is not None \ and id_angle_upper is not None, \ "Wrong?" assert id_angle_upper != id_angle_lower, \ "? %s %s" % (id_angle_lower, id_angle_upper) # cache xy = gp.xy # zero means empty in occupancy, # so object ids are shifted with 1 # we need object ids to filter "untouched" objects # in tfrecords_create if volume_occp[cat_id, xy[0], xy[1], id_angle_lower] == 0 \ or label in CATEGORIES_DOMINANT: volume_occp[cat_id, xy[0], xy[1], id_angle_lower] = oid + 1 if volume_occp[cat_id, xy[0], xy[1], id_angle_upper] == 0 \ or label in CATEGORIES_DOMINANT: volume_occp[cat_id, xy[0], xy[1], id_angle_upper] = oid + 1 # angles are right now not per-category, but per-scene # hence, an object can only overwrite, if it's usually "above" # other objects, e.g. a table # this is a hack for a z-test if occup_angle[cat_id, xy[0], xy[1], 0] < 0. \ or label in CATEGORIES_DOMINANT: occup_angle[cat_id, xy[0], xy[1], 0] = ob_angle if not had_vtk_problem: color = COLORS_CATEGORIES[label] if label in COLORS_CATEGORIES \ else (200., 200., 200.) try: for id_part, part in ob.parts.items(): vis.add_mesh(MeshOBJ.from_obb(part.obb), name="ob_%02d_part_%02d" % (oid, id_part), color=color) except AttributeError: print("VTK problem...") had_vtk_problem = True #plt.savefig() plt.close() if not had_vtk_problem: vis.set_camera_pos(pos=(0., -1., 0.)) vis.camera().SetFocalPoint(0., 0., 0.) vis.camera().SetViewUp(-1., 0., 0.) vis.set_camera_type(is_ortho=True) vis.camera().SetParallelScale(3.) # vis.show() name_recording = "%s_%s" % (os.path.basename(args.d), args.postfix) \ if args.postfix else os.path.basename(args.d) lg.info("name_recording: %s" % name_recording) path_out_occp = os.path.join(os.path.dirname(args.d), os.pardir, 'occupancy', name_recording) if not os.path.exists(path_out_occp): os.makedirs(path_out_occp) # prepare www storage www_grid = {'evidence': {}, 'occ': {}} # normalize evidence maps vmax = 0. ims = {} for cat, cat_id in labels_to_lin_ids.items(): ims[cat] = np.squeeze( np.sum(state.volume[cat_id, :, :, :], axis=2, keepdims=True)) vmax = max(vmax, np.max(ims[cat])) # gather joined occupancy map im_sum = None # for each evidence category for cat, cat_id in labels_to_lin_ids.items(): im = ims[cat] / vmax * 255. path_out_im = os.path.join(path_out_occp, "e_%s.jpg" % cat) cv2.imwrite(path_out_im, im) # lg.debug("wrote to %s" % path_out_im) www_grid['evidence'][cat] = path_out_im im = np.squeeze(volume_occp[cat_id, :, :, 0]) path_out_im = os.path.join(path_out_occp, "o_%s.jpg" % cat) cv2.imwrite(path_out_im, im * 255.) # lg.debug("wrote to %s" % path_out_im) www_grid['occ'][cat] = path_out_im if im_sum is None: im_sum = im.copy() else: im_sum = np.maximum(im, im_sum) # # save dataset # name_input_old = name_input if args.postfix is not None and len(args.postfix): name_input = "%s_%s" % (name_input, args.postfix) # state path_state_dest = os.path.join(path_for_tf, "state_%s.pickle" % name_input) shutil.copyfile(path_state_pickle, path_state_dest) lg.info("Copied\n\t%s to\n\t%s" % (path_state_pickle, path_state_dest)) # occupancy path_occup_dest = os.path.join(path_for_tf, "occup_%s.pickle" % name_input) pickle.dump(occup, open(path_occup_dest, 'wb'), -1) lg.info("Wrote to %s" % path_occup_dest) # occupancy_angle path_occup_angle_dest = os.path.join(path_for_tf, "angle_%s.npy" % name_input) min_angle = np.min(occup_angle) assert min_angle < 0., "No empty cells??" lg.debug("min angle is %s" % min_angle) np.save(open(path_occup_angle_dest, 'wb'), occup_angle) lg.info("Wrote to %s" % path_occup_angle_dest) # skeleton path_copied = shutil.copy2(path_scenelet, path_for_tf) lg.info("Copied\n\t%s to \n\t%s" % (path_scenelet, path_copied)) # charness skeleton name_skeleton_charness = "skel_%s-charness.json" % name_input_old path_scenelet_charness = os.path.join(args.d, name_skeleton_charness) assert os.path.exists(path_scenelet_charness), \ "Does not exist: %s" % path_scenelet_charness shutil.copy2(path_scenelet_charness, path_for_tf) assert os.path.exists(os.path.join(path_for_tf, name_skeleton_charness)), \ "Does not exist: %s" % os.path.join(path_for_tf, name_skeleton_charness) # rectangles name_rectangles = "rectangles_%s.npy" % name_input_old path_rectangles = os.path.join(path_for_tf, name_rectangles) np.save(open(path_rectangles, 'wb'), rects) # # visualize # path_out_im = os.path.join(path_out_occp, '3d.png') if not had_vtk_problem: vis.save_png(path_out_im) www_grid['3d'] = path_out_im path_out_im = os.path.join(path_out_occp, 'o_sum.png') max_im_sum = np.max(im_sum) if max_im_sum > 0.: cv2.imwrite(path_out_im, im_sum / max_im_sum * 255.) else: cv2.imwrite(path_out_im, im_sum * 255.) www_grid['o_sum'] = path_out_im path_www = os.path.join(path_out_occp, os.pardir) with open(os.path.join(path_www, 'index.html'), 'a') as f: f.write("<style> img {image-rendering: pixelated; } </style>\n") f.write("<script>\n") f.write("</script>\n") f.write("<h3>%s</h3>" % os.path.basename(args.d)) f.write('<table>\n') f.write("<tr>\n") f.write("<th>3d</th>") f.write("<th>Occupancy sum</th>") for cat in www_grid['evidence']: f.write("\t<th>%s</th>\n" % cat) f.write("<th></th>\n") # titles f.write("</tr>\n") f.write("<tr>\n") # 3D f.write("\t<td rowspan=\"2\">\n") path_im = os.path.relpath(www_grid['3d'], path_www) f.write("\t<a href=\"%s\">\n" "\t\t<img src=\"%s\" height=\"400\" />\n" "\t</a>\n" % (path_im, path_im)) # Evidence sum f.write("\t<td rowspan=\"2\">\n") path_im = os.path.relpath(www_grid['o_sum'], path_www) f.write("\t<a href=\"%s\">\n" "\t\t<img src=\"%s\" height=\"400\" />\n" "\t</a>\n" % (path_im, path_im)) # Evidence for cat in www_grid['evidence']: f.write("<td style=\"padding-bottom: 2px\">\n") path_im = os.path.relpath(www_grid['evidence'][cat], path_www) f.write("\t<a href=\"%s\">\n" "\t\t<img src=\"%s\" height=\"200\" />\n" "\t</a>\n" % (path_im, path_im)) f.write("</td>\n") f.write("<td>Evidence</td>\n") f.write("\t</td>\n") f.write("</tr>\n") f.write("<tr>\n") for cat in www_grid['occ']: f.write("<td>\n") path_im = os.path.relpath(www_grid['occ'][cat], path_www) f.write("\t<a href=\"%s\">\n" "\t\t<img src=\"%s\" height=\"200\" />\n" "</a>\n" % (path_im, path_im)) f.write("</td>\n") f.write("<td>Occupancy map</td>\n") f.write("</tr>") f.write('</table>') return True
def export_scenelet(um, o_pos_3d, o_polys_3d, query_full_skeleton, scenes, joints_active, transform_id=None): """Extract a scenelet (poses and objects) from the data from the optimized problem. Args: um (stealth.pose.unk_manager.UnkManager): Data manager. o_pos_3d (np.ndarray): Output 3D poses. o_polys_3d (np.ndarray): (6K, 4, 3) 3D oriented bounding boxes stored stacked. query_full_skeleton (stealth.logic.skeleton.Skeleton): Initial path containing time information. joints_active (list): List of joint_ids that were optimized for. Usage: pose16[:, joints_active] = o_pos_3d[pid, :, :] transform_id (int): Export only a specific group. Everything is exported, if None. Returns: A scenelet extracted from the data provided. """ # cache function _guess_time_at = query_full_skeleton.guess_time_at # all poses or the ones that belong to a group/scenelet if transform_id is None: pids_sorted = sorted([(pid, pid2scene) for pid, pid2scene in um.pids_2_scenes.items()], key=lambda e: e[1].frame_id) else: # pids_sorted = sorted([(pid, pid2scene) # for pid, pid2scene in um.pids_2_scenes.items() # if pid2scene.transform_id == transform_id], # key=lambda e: e[1].frame_id) pids_2_scenes = um.pids_2_scenes pids_sorted = sorted([(pid, pids_2_scenes[pid]) for pid in um.get_pids_for(transform_id)], key=lambda e: e[1].frame_id) # create output scenelet o = Scenelet() charness = None # # Skeleton # # cache skeleton reference skeleton = o.skeleton # fill skeleton for pid, pid2scene in pids_sorted: if charness is None: scene = scenes[pid2scene.id_scene] charness = scene.charness o.add_aux_info('name_scenelet', scene.name_scenelet) o.charness = charness # get frame_id frame_id = int(pid2scene.frame_id) # check if already exists if skeleton.has_pose(frame_id): # TODO: fix overlapping frame_ids lg.warning("[export_scenelet] Overwriting output frame_id %d" % frame_id) # add with time guessed from input skeleton rate pose = np.zeros((3, Joint.get_num_joints())) pose[:, joints_active] = o_pos_3d[pid, :, :] pose[:, Joint.PELV] = (pose[:, Joint.LHIP] + pose[:, Joint.RHIP]) / 2. pose[:, Joint.NECK] = (pose[:, Joint.HEAD] + pose[:, Joint.THRX]) / 2. # for j, jid in joints_remap.items(): # pose[:, j] = o_pos_3d[pid, :, jid] assert not skeleton.has_pose(frame_id=frame_id), \ 'Already has pose: {}'.format(frame_id) skeleton.set_pose(frame_id=frame_id, pose=pose, time=_guess_time_at(frame_id)) # # Objects # scene_obj = None scene_obj_oid = 0 # unique identifier that groups parts to objects for polys2scene in um.polys2scene.values(): # Check, if we are restricted to a certain group if transform_id is not None \ and polys2scene.transform_id != transform_id: continue start = polys2scene.poly_id_start end = start + polys2scene.n_polys # 6 x 4 x 3 polys = o_polys_3d[start:end, ...] assert polys.shape[0] == 6, "Assumed cuboids here" if scene_obj is None or scene_obj_oid != polys2scene.object_id: category = next(cat for cat in CATEGORIES if CATEGORIES[cat] == polys2scene.cat_id) scene_obj = SceneObj(label=category) scene_obj_oid = polys2scene.object_id o.add_object(obj_id=-1, scene_obj=scene_obj, clone=False) part = scene_obj.add_part(part_id=-1, label_or_part=polys2scene.part_label) # TODO: average for numerical precision errors centroid = np.mean(polys, axis=(0, 1)) ax0 = polys[0, 1, :] - polys[0, 0, :] scale0 = np.linalg.norm(ax0) ax0 /= scale0 ax1 = polys[0, 3, :] - polys[0, 0, :] scale1 = np.linalg.norm(ax1) ax1 /= scale1 ax2 = polys[1, 0, :] - polys[0, 0, :] scale2 = np.linalg.norm(ax2) ax2 /= scale2 part.obb = Obb(centroid=centroid, axes=np.concatenate( (ax0[:, None], ax1[:, None], ax2[:, None]), axis=1), scales=[scale0, scale1, scale2]) # if scene_obj is not None: # o.add_object(obj_id=-1, scene_obj=scene_obj, clone=False) # else: # lg.warning("No objects in scenelet?") # scene_obj = SceneObj('couch') # for poly_id in range(0, o_polys_3d.shape[0], 6): # rects = o_polys_3d[poly_id : poly_id + 6, ...] # # lg.debug("rects:\n%s" % rects) # scene_obj.add_part(poly_id, 'seat') # # # fig = plt.figure() # # ax = fig.add_subplot(111, projection='3d') # # for rid, rect in enumerate(rects): # # wrapped = np.concatenate((rect, rect[0:1, :]), axis=0) # # ax.plot(wrapped[:, 0], wrapped[:, 2], wrapped[:, 1]) # # for ci in range(4): # # c = rect[ci, :] # # ax.text(c[0], c[2], c[1], s="%d, %d, %d" # # % (poly_id, rid, ci)) # # if rid >= 1: # # break # # # # plt.show() # part = scene_obj.get_part(poly_id) # centroid = np.mean(rects, axis=(0, 1)) # ax0 = rects[0, 1, :] - rects[0, 0, :] # scale0 = np.linalg.norm(ax0) # ax0 /= scale0 # ax1 = rects[0, 3, :] - rects[0, 0, :] # scale1 = np.linalg.norm(ax1) # ax1 /= scale1 # ax2 = rects[1, 0, :] - rects[0, 0, :] # scale2 = np.linalg.norm(ax2) # ax2 /= scale2 # part.obb = Obb(centroid=centroid, # axes=np.concatenate(( # ax0[:, None], ax1[:, None], ax2[:, None] # ), axis=1), # scales=[scale0, scale1, scale2]) # o.add_object(obj_id=99, scene_obj=scene_obj, # clone=False) return o
def main(argv): conf = Conf.get() parser = argparse.ArgumentParser("Denis pose converter") parser.add_argument('camera_name', help="Camera name ('G15', 'S6')", type=str) parser.add_argument( '-d', dest='dir', required=True, help="Path to the <scene folder>/denis containing skeletons.json") parser.add_argument( '-filter', dest='with_filtering', action="store_true", help="Should we do post-filtering (1-euro) on the pelvis positions") parser.add_argument('-huber', required=False, help="Should we do huber loss?", action='store_true') parser.add_argument('-smooth', type=float, default=0.005, help="Should we have a smoothness term (l2/huber)?") parser.add_argument( '--winsorize-limit', type=float, default=conf.optimize_path.winsorize_limit, help='Threshold for filtering too large jumps of the 2D centroid') parser.add_argument('--no-resample', action='store_true', help="add resampled frames") parser.add_argument('--n-actors', type=int, default=1, help="How many skeletons to track.") parser.add_argument('-n-actors', type=int, default=1, help="Max number of people in scene.") # parser.add_argument( # '-r', type=float, # help='Video rate. Default: 1, if avconv -r 5. ' # 'Original video sampling rate (no subsampling) should be ' # '24/5=4.8. avconv -r 10 leads to 24/10=2.4.', # required=True) parser.add_argument('--person_height', type=float, help='Assumed height of human(s) in video.', default=Conf.get().optimize_path.person_height) parser.add_argument( '--forwards-window-size', type=int, help='How many poses in time to look before AND after to ' 'average forward direction. 0 means no averaging. Default: 0.', default=0) parser.add_argument('--no-img', action='store_true', help='Read and write images (vis reproj error)') parser.add_argument('--postfix', type=str, help="output file postfix.", default='unannot') args = parser.parse_args(argv) show = False args.resample = not args.no_resample # assert not args.resample, "resample should be off" assert os.path.exists(args.dir), "Source does not exist: %s" % args.dir p_scene = os.path.normpath(os.path.join(args.dir, os.pardir)) # type: str p_video_params = os.path.join(p_scene, 'video_params.json') assert os.path.exists(p_video_params), "Need video_params.json for rate" if 'r' not in args or args.r is None: args.r = json.load(open(p_video_params, 'r'))['rate-avconv'] # manual parameters (depth initialization, number of actors) p_scene_params = os.path.join(args.dir, os.pardir, 'scene_params.json') if not os.path.exists(p_scene_params): scene_params = { 'depth_init': 10., 'actors': args.n_actors, 'ground_rot': [0., 0., 0.] } json.dump(scene_params, open(p_scene_params, 'w')) raise RuntimeError("Inited scene_params.json, please check: %s" % p_scene_params) else: scene_params = json.load(open(p_scene_params, 'r')) lg.warning("Will work with %d actors and init depth to %g" % (scene_params['actors'], scene_params['depth_init'])) assert '--n-actors' not in argv \ or args.n_actors == scene_params['actors'], \ "Actor count mismatch, remove %d from args, because " \ "scene_params.json says %d?" \ % (args.n_actors, scene_params['actors']) args.n_actors = scene_params['actors'] ground_rot = scene_params['ground_rot'] or [0., 0., 0.] # load images path_images = os.path.abspath(os.path.join(args.dir, os.pardir, 'origjpg')) images = {} shape_orig = None if not args.no_img: images, shape_orig = load_images(path_images) path_skeleton = \ max((f for f in os.listdir(os.path.join(args.dir)) if f.startswith('skeletons') and f.endswith('json')), key=lambda s: int(os.path.splitext(s)[0].split('_')[1])) print("path_skeleton: %s" % path_skeleton) data = json.load(open(os.path.join(args.dir, path_skeleton), 'r')) # data, pose_constraints, first_run = \ # cleanup(data, p_dir=os.path.join(args.dir, os.pardir)) # poses_2d = [] # plt.figure() # show_images(images, data) if False: # pose_ids = identify_actors_multi(data, n_actors=1) p_segm_pickle = os.path.join(args.dir, os.pardir, "label_skeletons.pickle") problem = None if False and os.path.exists(p_segm_pickle): lg.warning("Loading skeleton segmentation from pickle %s" % p_segm_pickle) pose_ids, problem = pickle_load(open(p_segm_pickle, 'rb')) if not problem or problem._n_actors != args.n_actors: pose_ids, problem, data = more_actors_gurobi( data, n_actors=args.n_actors, constraints=pose_constraints, first_run=first_run) if True or show: show_multi(images, data, pose_ids, problem, p_dir=os.path.join(args.dir, os.pardir), first_run=first_run, n_actors=args.n_actors) pickle.dump((pose_ids, problem), open(p_segm_pickle, 'wb'), -1) else: pose_ids = greedy_actors(data, n_actors=args.n_actors) data = DataPosesWrapper(data=data) visible_f = {a: {} for a in range(args.n_actors)} visible_f_max = 0. if show: plt.ion() fig = None axe = None scatters = dict() # how many images we have min_frame_id = min(f for f in pose_ids) frames_mod = max(f for f in pose_ids) - min_frame_id + 1 skel_ours = Skeleton(frames_mod=frames_mod, n_actors=args.n_actors, min_frame_id=min_frame_id) skel_ours_2d = Skeleton(frames_mod=frames_mod, n_actors=args.n_actors, min_frame_id=min_frame_id) # assert len(images) == 0 or max(f for f in images) + 1 == frames_mod, \ # "Assumed image count is %d, but max_frame_id is %d" \ # % (len(images), frames_mod-1) if isinstance(data, DataPosesWrapper): frames = data.get_frames() else: frames = [] for frame_str in sorted(data.get_frames()): try: frame_id = int(frame_str.split('_')[1]) except ValueError: print("skipping key %s" % frame_id) continue frames.append(frame_id) my_visibilities = [[], []] for frame_id in frames: frame_str = DataPosesWrapper._to_frame_str(frame_id) pose_in = data.get_poses_3d(frame_id=frame_id) # np.asarray(data[frame_str][u'centered_3d']) # pose_in_2d = np.asarray(data[frame_str][u'pose_2d']) pose_in_2d = data.get_poses_2d(frame_id=frame_id) # visible = np.asarray(data[frame_str][u'visible']) if False and len(pose_in.shape) > 2: pose_id = pose_ids[frame_id] if not args.no_img: im = cv2.cvtColor(images[frame_id], cv2.COLOR_RGB2BGR) for i in range(pose_in.shape[0]): c = (1., 0., 0., 1.) if i == pose_id: c = (0., 1., 0., 1.) color = tuple(int(c_ * 255) for c_ in c[:3]) for p2d in pose_in_2d[i, :, :]: # color = (c[0] * 255, c[1] * 255., c[2] * 255.) cv2.circle(im, (p2d[1], p2d[0]), radius=3, color=color, thickness=-1) center = np.mean(pose_in_2d[i, :, :], axis=0).round().astype('i4').tolist() cv2.putText(im, "%d" % i, (center[1], center[0]), 1, 1, color) if show: cv2.imshow("im", im) cv2.waitKey(100) # if sid not in scatters: # scatters[sid] = axe.scatter(pose_in_2d[i, :, 1], pose_in_2d[i, :, 0], c=c) # else: # scatters[sid].set_offsets(pose_in_2d[i, :, [1, 0]]) # scatters[sid].set_array(np.tile(np.array(c), pose_in_2d.shape[1])) # scatter.set_color(c) # plt.draw() # plt.pause(1.) pose_in = pose_in[pose_id, :, :] pose_in_2d = pose_in_2d[pose_id, :, :] visible = visible[pose_id] # else: # pose_id = 0 # pose_id = pose_ids[frame_id] for actor_id in range(args.n_actors): # if actor_id in (2, 3, 4, 5, 8, 9) # expanded frame_id frame_id2 = Skeleton.unmod_frame_id(frame_id=frame_id, actor_id=actor_id, frames_mod=frames_mod) assert (actor_id != 0) ^ (frame_id2 == frame_id), "no" frame_id_mod = skel_ours.mod_frame_id(frame_id=frame_id2) assert frame_id_mod == frame_id, \ "No: %d %d %d" % (frame_id, frame_id2, frame_id_mod) actor_id2 = skel_ours.get_actor_id(frame_id2) assert actor_id2 == actor_id, "no: %s %s" % (actor_id, actor_id2) # which pose explains this actor in this frame pose_id = pose_ids[frame_id][actor_id] # check, if actor found if pose_id < 0: continue # 3D pose pose = pose_in[pose_id, :, JointDenis.revmap].T # added by Aron on 4/4/2018 (Denis' pelvis is too high up) pose[:, Joint.PELV] = (pose[:, Joint.LHIP] + pose[:, Joint.RHIP]) \ / 2. skel_ours.set_pose(frame_id2, pose) # 2D pose pose_2d = pose_in_2d[pose_id, :, :] arr = np.array(JointDenis.pose_2d_to_ours(pose_2d), dtype=np.float32).T skel_ours_2d.set_pose(frame_id2, arr) # # visibility (binary) and confidence (float) # # np.asarray(data[frame_str][u'visible'][pose_id]) vis_i = data.get_visibilities(frame_id)[pose_id] # vis_f = np.asarray(data[frame_str][u'visible_float'][pose_id]) vis_f = data.get_confidences(frame_id)[pose_id] for jid, visible in enumerate(vis_i): # for each joint # binary visibility jid_ours = JointDenis.to_ours_2d(jid) skel_ours_2d.set_visible(frame_id2, jid_ours, visible) # confidence (fractional visibility) if np.isnan(vis_f[jid]): continue try: visible_f[actor_id][frame_id2][jid_ours] = vis_f[jid] except KeyError: visible_f[actor_id][frame_id2] = {jid_ours: vis_f[jid]} visible_f_max = max(visible_f_max, vis_f[jid]) conf_ = get_conf_thresholded(vis_f[jid], thresh_log_conf=None, dtype_np=np.float32) skel_ours_2d.set_confidence(frame_id=frame_id2, joint=jid_ours, confidence=conf_) my_visibilities[0].append(vis_f[jid]) my_visibilities[1].append(conf_) skel_ours_2d._confidence_normalized = True plt.figure() plt.plot(my_visibilities[0], my_visibilities[1], 'o') plt.savefig('confidences.pdf') assert skel_ours.n_actors == args.n_actors, "no" assert skel_ours_2d.n_actors == args.n_actors, "no" # align to room min_z = np.min(skel_ours.poses[:, 2, :]) print("min_max: %s, %s" % (min_z, np.max(skel_ours.poses[:, 2, :]))) skel_ours.poses[:, 2, :] += min_z skel_ours.poses /= 1000. # The output is scaled to 2m by Denis. # We change this to 1.8 * a scale in order to correct for # the skeletons being a bit too high still. skel_ours.poses *= \ args.person_height * conf.optimize_path.height_correction / 2. skel_ours.poses[:, 2, :] *= -1. skel_ours.poses = skel_ours.poses[:, [0, 2, 1], :] # refine name_video = args.dir.split(os.sep)[-2] out_path = os.path.join(args.dir, os.pardir, "skel_%s_%s.json" % (name_video, args.postfix)) out_path_orig = os.path.join(args.dir, os.pardir, "skel_%s_lfd_orig.json" % name_video) sclt_orig = Scenelet(skeleton=copy.deepcopy(skel_ours)) sclt_orig.save(out_path_orig) skel_ours_2d_all = copy.deepcopy(skel_ours_2d) assert len(skel_ours_2d_all.get_frames()), skel_ours_2d_all.get_frames() # # Optimize # # frames_ignore = [(282, 372), (516, 1000)] skel_ours, skel_ours_2d, intrinsics, \ frame_ids_filled_in = prepare( args.camera_name, winsorize_limit=args.winsorize_limit, shape_orig=shape_orig, path_scene=p_scene, skel_ours_2d=skel_ours_2d, skel_ours=skel_ours, resample=args.resample, path_skel=path_skeleton) frames_ignore = [] tr_ground = np.eye(4, dtype=np.float32) skel_opt, out_images, K = \ optimize_path( skel_ours, skel_ours_2d, images, intrinsics=intrinsics, path_skel=out_path, shape_orig=shape_orig, use_huber=args.huber, weight_smooth=args.smooth, frames_ignore=frames_ignore, resample=args.resample, depth_init=scene_params['depth_init'], ground_rot=ground_rot) for frame_id in skel_opt.get_frames(): skel_opt.set_time(frame_id=frame_id, time=float(frame_id) / args.r) skel_opt_raw = copy.deepcopy(skel_opt) skel_opt_resampled = Skeleton.resample(skel_opt) # Filter pelvis if args.with_filtering: out_filter_path = os.path.join(args.dir, os.pardir, "vis_filtering") skel_opt = filter_(skel_opt_resampled, out_filter_path=out_filter_path, skel_orig=skel_opt, weight_smooth=args.smooth, forwards_window_size=args.forwards_window_size) else: skel_opt.estimate_forwards(k=args.forwards_window_size) skel_opt_resampled.estimate_forwards(k=args.forwards_window_size) # if len(images): # skel_opt.fill_with_closest(images.keys()[0], images.keys()[-1]) min_y, max_y = skel_opt.get_min_y(tr_ground) print("min_y: %s, max_y: %s" % (min_y, max_y)) # # save # frame_ids_old = set(skel_opt.get_frames()) if args.resample: skel_opt = skel_opt_resampled frame_ids_filled_in.update( set(skel_opt.get_frames()).difference(frame_ids_old)) lg.warning("Saving resampled scenelet!") scenelet = Scenelet(skel_opt) del skel_opt # skel_dict = skel_opt.to_json() tr_ground[1, 3] = min_y scenelet.aux_info['ground'] = tr_ground.tolist() assert isinstance(ground_rot, list) and len(ground_rot) == 3 scenelet.add_aux_info('ground_rot', ground_rot) scenelet.add_aux_info( 'path_opt_params', { 'rate': args.r, 'w-smooth': args.smooth, 'winsorize-limit': args.winsorize_limit, 'camera': args.camera_name, 'huber': args.huber, 'height_correction': conf.optimize_path.height_correction, 'focal_correction': conf.optimize_path.focal_correction }) scenelet.add_aux_info('frame_ids_filled_in', list(frame_ids_filled_in)) # To MATLAB # _skeleton.get_min_y(_tr_ground) # with skel_opt as skeleton: # skeleton = skel_opt # skeleton_name = os.path.split(args.dir)[0] # skeleton_name = skeleton_name[skeleton_name.rfind('/')+1:] # mdict = skeleton.to_mdict(skeleton_name) # mdict['room_transform'] = tr_ground # mdict['room_transform'][1, 3] *= -1. # print(mdict) # print("scene_name?: %s" % os.path.split(args.dir)[0]) # skeleton.save_matlab( # os.path.join(os.path.dirname(args.dir), "skeleton_opt.mat"), # mdict=mdict) assert scenelet.skeleton.has_forwards(), "No forwards??" scenelet.save(out_path) if show: # save path plot out_path_path = os.path.join(args.dir, os.pardir, "%s_path.jpg" % name_video) path_fig = plot_path(scenelet.skeleton) legend = ["smooth %g" % args.smooth] # hack debug # path_skel2 = os.path.join(args.dir, os.pardir, 'skel_lobby7_nosmooth.json') # if os.path.exists(path_skel2): # skel2 = Skeleton.load(path_skel2) # path_fig = plot_path(skel2, path_fig) # legend.append('no smooth') if show: plt.legend(legend) path_fig.savefig(out_path_path) # backup args path_args = os.path.join(args.dir, os.pardir, 'args_denis.txt') with open(path_args, 'a') as f_args: f_args.write("%s %s\n" % (os.path.basename(sys.executable), " ".join(argv))) # save 2D detections to file if args.postfix == 'unannot': path_skel_ours_2d = os.path.join( args.dir, os.pardir, "skel_%s_2d_%02d.json" % (name_video, 0)) sclt_2d = Scenelet(skel_ours_2d_all) print('Saving {} to {}'.format(len(skel_ours_2d_all.get_frames()), path_skel_ours_2d)) sclt_2d.skeleton.aux_info = {} sclt_2d.save(path_skel_ours_2d) else: print(args.postfix) logging.info("Saving images...") if len(images) and len(out_images): path_out_images = os.path.join(args.dir, os.pardir, 'color') try: os.makedirs(path_out_images) except OSError: pass visible_f_max_log = np.log(visible_f_max) frames = list(out_images.keys()) for frame_id in range(frames[0], frames[-1] + 1): im = out_images[frame_id] if frame_id in out_images \ else cv2.cvtColor(images[frame_id], cv2.COLOR_BGR2RGB) for actor_id in range(args.n_actors): if frame_id in visible_f[actor_id]: frame_id2 = skel_ours_2d_all.unmod_frame_id( frame_id=frame_id, actor_id=actor_id, frames_mod=skel_ours_2d_all.frames_mod) for joint, is_vis in visible_f[actor_id][frame_id].items(): p2d = skel_ours_2d_all.get_joint_3d(joint, frame_id=frame_id2) # radius = np.log(is_vis) / visible_f_max_log # lg.debug("r0: %g" % radius) # radius = np.exp(np.log(is_vis) / visible_f_max_log) # lg.debug("radius is %g" % radius) vis_bool = True if skel_ours_2d_all.has_visible(frame_id=frame_id2, joint_id=joint): vis_bool &= skel_ours_2d_all.is_visible( frame_id2, joint) radius = abs(np.log(is_vis / 0.1 + 1e-6)) if not np.isnan(radius): p2d = (int(round(p2d[0])), int(round(p2d[1]))) cv2.circle(im, center=p2d, radius=int(round(radius)), color=(1., 1., 1., 0.5), thickness=1) conf = get_conf_thresholded(conf=is_vis, thresh_log_conf=None, dtype_np=np.float32) if conf > 0.5: cv2.putText(img=im, text=Joint(joint).get_name(), org=p2d, fontFace=1, fontScale=1, color=(10., 150., 10., 100.)) # lg.debug("set visibility to %g, radius %g" % (is_vis, radius)) # if frame_id in out_images: scale = (shape_orig[1] / float(im.shape[1]), shape_orig[0] / float(im.shape[0])) cv2.imwrite( os.path.join(path_out_images, "color_%05d.jpg" % frame_id), cv2.resize(im, (0, 0), fx=scale[0], fy=scale[1], interpolation=cv2.INTER_CUBIC)) # else: # fname = "color_%05d.jpg" % frame_id # shutil.copyfile( # os.path.join(path_images, fname), # os.path.join(path_out_images, fname)) lg.info("Wrote images to %s/" % path_out_images)
def prepare(camera_name, winsorize_limit, shape_orig, path_scene, skel_ours_2d, skel_ours, resample, path_skel): """ Args: camera_name (str): Name of camera for intrinsics calculation. winsorize_limit (float): Outlier detection threshold. shape_orig (Tuple[int, int]): Original video resolution. path_scene (str): Root path to scene. skel_ours_2d (np.ndarray): (N, 2, 16) 2D skeletons from LFD in our format. skel_ours (np.ndarray): (N, 3, 16) Local space 3D skeletons in iMapper coordinate frame (y-down, z-front). resample (bool): If needs densification using Blender's IK engine. Returns: skel_ours (Skeleton): skel_ours_2d (Skeleton): intrinsics (np.ndarray): """ assert camera_name is not None and isinstance(camera_name, str), \ "Need a camera name" if shape_orig is None: shape_orig = (np.float32(1080.), np.float32(1920.)) np.set_printoptions(linewidth=200, suppress=True) if False: plt.figure() for i, frame_id in enumerate(skel_ours.get_frames()): plot_2d(skel_ours_2d.get_pose(frame_id), images[frame_id]) plt.show() path_intrinsics = os.path.join(path_scene, "intrinsics.json") if os.path.exists(path_intrinsics): lg.warning("Loading existing intrinsics matrix!") K = np.array(json.load(open(path_intrinsics, 'r')), dtype=np.float32) scale = (shape_orig[1] / int(round(shape_orig[1] * float(INPUT_SIZE) / shape_orig[0])), shape_orig[0] / float(INPUT_SIZE)) K[0, 0] /= scale[0] K[0, 2] /= scale[0] K[1, 1] /= scale[1] K[1, 2] /= scale[1] else: K = intrinsics_matrix(INPUT_SIZE, shape_orig, camera_name) focal_correction = Conf.get().optimize_path.focal_correction if abs(focal_correction - 1.) > 1.e-3: lg.warning("Warning, scaling intrinsics matrix by %f" % focal_correction) K[0, 0] *= focal_correction K[1, 1] *= focal_correction #print("K:\n%s,\nintr:\n%s" % (K, intr)) # sys.exit(0) # # Prune poses # skel_ours_2d, frame_ids_removed = filter_outliers( skel_ours_2d, winsorize_limit=winsorize_limit, show=False) frames_to_remove_3d = filter_wrong_poses(skel_ours_2d, skel_ours) frames_to_ignore_list = set() # if frames_ignore is not None: # for start_end in frames_ignore: # if isinstance(start_end, tuple): # l_ = list(range( # start_end[0], # min(start_end[1], skel_ours_2d.get_frames()[-1]))) # frames_to_remove_3d.extend(l_) # frames_to_ignore_list.update(l_) # else: # assert isinstance(start_end, int), \ # "Not int? %s" % repr(start_end) # frames_to_remove_3d.append(start_end) # frames_to_ignore_list.add(start_end) for frame_id in skel_ours.get_frames(): if frame_id in frames_to_remove_3d: skel_ours.remove_pose(frame_id) # resample skeleton to fill in missing frames skel_ours_old = skel_ours frame_ids_filled_in = set(skel_ours_2d.get_frames()).difference( set(skel_ours_old.get_frames())) if resample: lg.warning("Resampling BEFORE optimization") # frames_to_resample = sorted(set(skel_ours_2d.get_frames()).difference( # frames_to_ignore_list)) # skel_ours = Skeleton.resample(skel_ours_old, # frame_ids=frames_to_resample) # Aron on 6/4/2018 sclt_ours = Scenelet(skeleton=skel_ours) stem = os.path.splitext(path_skel)[0] path_filtered = "%s_filtered.json" % stem path_ipoled = "%s_ikipol.json" % os.path.splitext(path_filtered)[0] if not os.path.exists(path_ipoled): sclt_ours.save(path_filtered) script_filepath = \ os.path.normpath(os.path.join( os.path.dirname(os.path.abspath(__file__)), os.pardir, 'blender', 'ipol_ik.py')) assert os.path.exists(script_filepath), "No: %s" % script_filepath blender_path = os.environ.get('BLENDER') if not os.path.isfile(blender_path): raise RuntimeError( "Need \"BLENDER\" environment variable to be set " "to the blender executable") cmd_params = [ blender_path, '-noaudio', '-b', '-P', script_filepath, '--', path_filtered ] print("calling %s" % " ".join(cmd_params)) ret = check_call(cmd_params) print("ret: %s" % ret) else: lg.warning("\n\n\tNOT recomputing IK interpolation, " "file found at %s!\n" % path_ipoled) skel_ours = Scenelet.load(path_ipoled, no_obj=True).skeleton # remove extra frames at ends and beginnings of actors spans = skel_ours_old.get_actor_empty_frames() old_frames = skel_ours_old.get_frames() frames_to_remove = [] for frame_id in skel_ours.get_frames(): if frame_id not in old_frames: in_spans = next( (True for span in spans if span[0] < frame_id < span[1]), None) if in_spans: frames_to_remove.append(frame_id) # lg.debug("diff: %s (a%s, f%s)" # % ( # frame_id, # skel_ours_old.get_actor_id(frame_id), # skel_ours_old.mod_frame_id(frame_id) # )) for frame_id in frames_to_remove: skel_ours.remove_pose(frame_id) for frame_id in skel_ours_2d.get_frames(): if not skel_ours.has_pose(frame_id): skel_ours_2d.remove_pose(frame_id) for frame_id in skel_ours.get_frames(): if not skel_ours_2d.has_pose(frame_id): skel_ours.remove_pose(frame_id) frames_set_ours = set(skel_ours.get_frames()) frames_set_2d = set(skel_ours_2d.get_frames()) if frames_set_ours != frames_set_2d: print("Frame mismatch: %s" % frames_set_ours.difference(frames_set_2d)) lg.warning("Removing pelvis and neck from 2D input") for frame_id in skel_ours_2d.get_frames(): skel_ours_2d.set_visible(frame_id, Joint.PELV, 0) skel_ours_2d.set_visible(frame_id, Joint.NECK, 0) return skel_ours, skel_ours_2d, K, frame_ids_filled_in
def main(argv): pjoin = os.path.join # cache long name parser = argparse.ArgumentParser( "Find characteristic scene times", description="Scans a directory of short scenelets (exported from " "Matlab), and looks up their full version in the original " "scenes. It exports a new scenelet containing all poses " "between the start and end times of the input short " "scenelets. Matching is done by name." "Scenelets below time length limit and not enough objects " "are thrown away." "It also saves the scenelet characteristicness into the " "output scenelet files.") parser.add_argument( 'd', type=argparse_check_exists, help="Folder containing PiGraphs scenelets. E.g. " "/mnt/thorin_data/stealth/shared/" "pigraph_scenelets__linterval_squarehist_large_radiusx2") parser.add_argument('s', type=argparse_check_exists, help="Folder containing PiGraphs full scenes. E.g. " "/mnt/thorin_data/stealth/shared/scenes_pigraphs") parser.add_argument('-l', '--limit-len', type=int, help="Minimum length for a scenelet", default=10) # changed from `5` on 15/1/2018 parser.add_argument( '--dist-thresh', type=float, help='Distance threshold for object pruning. Typically: 0.2 or 0.5.', default=.5) # parse arguments args = parser.parse_args(argv) parts_to_remove = ['sidetable'] lg.warning("Will remove all parts named %s" % parts_to_remove) # read scenes and scenelets p_pickle = pjoin(args.d, 'scenes_and_scenelets.pickle') if os.path.exists(p_pickle): lg.info("reading from %s" % p_pickle) scenes, scenelets = pickle_load(open(p_pickle, 'rb')) lg.info("read from %s" % p_pickle) else: scenelets = read_scenelets(args.d) scenes = read_scenelets(args.s) scenes = {scene.name_scene: scene for scene in scenes} pickle.dump((scenes, scenelets), open(p_pickle, 'wb'), protocol=-1) lg.info("wrote to %s" % p_pickle) # Read characteristicnesses (to put them into the scenelet). p_charness = pjoin(args.d, "charness__gaussian.mat") pose_charness, scenelet_names = read_charness(p_charness, return_hists=False, return_names=True) # output folder d_scenelets_parent = os.path.dirname(args.d) d_dest = pjoin(d_scenelets_parent, 'deb', "%s_full_sampling" % args.d.split(os.sep)[-1]) # makedirs_backed if os.path.exists(d_dest): i = 0 while i < 100: try: os.rename(d_dest, "%s.bak.%02d" % (d_dest, i)) break except OSError: i += 1 os.makedirs(d_dest) # _is_close = is_close # cache namespace lookup # processing for sclt in scenelets: # cache skeleton skeleton = sclt.skeleton if 'scene09' in sclt.name_scenelet or 'scene10' in sclt.name_scenelet: lg.debug("here") else: continue # prune objects per_cat = {} cnt = 0 for oid, scene_obj in sclt.objects.items(): close_, dist = is_close(scene_obj, skeleton, args.dist_thresh, return_dist=True) label = scene_obj.label if 'chair' in label or 'couch' in label or 'stool' in label: label = 'sittable' try: per_cat[label].append((dist, oid)) except KeyError: per_cat[label] = [(dist, oid)] if scene_obj.label != 'floor': cnt += 1 per_cat = {k: sorted(v) for k, v in per_cat.items()} name_scene = sclt.name_scene.split('__')[0] if '-no-coffeetable' in name_scene: name_scene = name_scene[:name_scene.find('-no-coffeetable')] scene = scenes[name_scene] if 'shelf' not in per_cat: for oid, ob in scene.objects.items(): if ob.label == 'shelf': close_, dist = is_close(ob, skeleton, args.dist_thresh, return_dist=True) oid_ = oid while oid_ in sclt.objects: oid_ += 1 sclt.add_object(oid_, ob) cnt += 1 try: per_cat['shelf'].append((dist, oid_)) except KeyError: per_cat['shelf'] = [(dist, oid_)] if 'shelf' in per_cat: assert len(per_cat['shelf']) == 1, "TODO: keep all shelves" oids_to_keep = [ v[0][1] for v in per_cat.values() if v[0][0] < args.dist_thresh ] if not len(oids_to_keep): # there is always a floor lg.warning("Skipping %s, not enough objects: %s" % (sclt.name_scenelet, per_cat)) continue # if 'gates392_mati3_2014-04-30-21-13-46__scenelet_25' \ # == sclt.name_scenelet: # lg.debug("here") # else: # continue # copy skeleton with dense sampling in time mn, mx = skeleton.get_frames_min_max() # assert mn == 0, "This usually starts indexing from 0, " \ # "no explicit problem, just flagging the change." time_mn = floor(skeleton.get_time(mn)) time_mx = ceil(skeleton.get_time(mx)) # artificially prolong mocap scenes if 'scene' in name_scene and (time_mx - time_mn < 60): d = (time_mx - time_mn) // 2 + 1 time_mn -= d time_mx += d # lookup original scene name # mn_frame_id_scene, mx_frame_id_scene = \ # scene.skeleton.get_frames_min_max() frame_ids_old = skeleton.get_frames() times_old = [skeleton.get_time(fid) for fid in frame_ids_old] for frame_id in frame_ids_old: skeleton.remove_pose(frame_id) for frame_id in range(time_mn, time_mx + 1): if not scene.skeleton.has_pose(frame_id): continue pose = scene.skeleton.get_pose(frame_id=frame_id) # scale mocap skeletons fw = scene.skeleton.get_forward(frame_id=frame_id, estimate_ok=False) sclt.set_pose(frame_id=frame_id, angles=None, pose=pose, forward=fw, clone_forward=True) if 'scene0' in name_scene or 'scene10' in name_scene: mx_old = np.max(sclt.skeleton.poses[:, 1, :]) sclt.skeleton.poses *= 0.8 mx_new = np.max(sclt.skeleton.poses[:, 1, :]) sclt.skeleton.poses[1, :] += mx_new - mx_old + 0.05 _frames = sclt.skeleton.get_frames() # check length if len(_frames) < args.limit_len: lg.warning("Skipping %s, because not enough frames: %s" % (sclt.name_scene, _frames)) continue # save charness try: id_charness = next(i for i in range(len(scenelet_names)) if scenelet_names[i] == sclt.name_scenelet) sclt.charness = pose_charness[id_charness] except StopIteration: lg.error("Something is wrong, can't find %s, %s in charness db " "containing names such as %s." % (sclt.name_scene, sclt.name_scenelet, scenelet_names[0])) sclt.charness = 0.4111111 _mn, _mx = (_frames[0], _frames[-1]) assert _mn >= time_mn, "not inside? %s < %s" % (_mn, time_mn) assert _mx <= time_mx, "not inside? %s < %s" % (_mx, time_mx) if len(_frames) < len(frame_ids_old): lg.warning("Not more frames than interpolated " "scenelet?\n%s\n%s\n%s" % (_frames, frame_ids_old, times_old)) oids = list(sclt.objects.keys()) for oid in oids: if oid not in oids_to_keep: lg.debug("removed %s" % sclt.objects[oid]) sclt.objects.pop(oid) else: obj = sclt.objects[oid] part_ids_to_remove = [ part_id for part_id, part in obj.parts.items() if part.label in parts_to_remove ] if len(part_ids_to_remove) == len(obj.parts): sclt.objects.pop(oid) else: for part_id in part_ids_to_remove: lg.debug("removed %s" % sclt.objects[obj].parts[part_id]) obj.parts.pop(part_id) if len(sclt.objects) < 2 and next(iter( sclt.objects.values())).label == 'floor': lg.debug("finally removing scenelet: %s" % sclt.objects) continue # save in the scene folder d_dest_scene = pjoin(d_dest, name_scene) if not os.path.exists(d_dest_scene): os.makedirs(d_dest_scene) sclt.save(pjoin(d_dest_scene, "skel_%s" % sclt.name_scenelet))
def show_folder(argv): # python3 stealth/pose/fit_full_video.py --show /home/amonszpa/workspace/stealth/data/video_recordings/scenelets/lobby15 opt1 # python3 stealth/pose/visualization/show_charness_scores.py --show /media/data/amonszpa/stealth/shared/video_recordings/library1 -o opt1 pjoin = os.path.join parser = argparse.ArgumentParser("Fit full video") parser.add_argument('--show', action='store_true') parser.add_argument("video", type=argparse_check_exists, help="Input path") parser.add_argument( '-o', '--opt-folder', help="Which optimization output to process. Default: opt1", default='opt1') parser.add_argument("--window-size", type=int, help="Window size in frames.", default=20) args = parser.parse_args(argv) d = os.path.join(args.video, args.opt_folder) assert os.path.exists(d), "does not exist: %s" % d # parse video path if args.video.endswith(os.sep): args.video = args.video[:-1] name_query = os.path.split(args.video)[-1] print("split: %s" % repr(os.path.split(args.video))) p_query = pjoin(args.video, "skel_%s_unannot.json" % name_query) \ if os.path.isdir(args.video) else args.video assert p_query.endswith('.json'), "Need a skeleton file" # load initial video path (local poses) query = Scenelet.load(p_query, no_obj=True) frame_ids = query.skeleton.get_frames() centroids = Skeleton.get_resampled_centroids(start=frame_ids[0], end=frame_ids[-1], old_frame_ids=frame_ids, poses=query.skeleton.poses) depths_times_charnesses = [] skeleton = Skeleton() depths = [] skeleton.charness_poses = {} # this is in Scenelet incorrectly... skeleton.score_fit = {} # inventing this now skeleton.score_reproj = {} # inventing this now for p in sorted(os.listdir(d)): d_time = pjoin(d, p) if not os.path.isdir(d_time): continue p_skel = next( f for f in os.listdir(d_time) if os.path.isfile(pjoin(d_time, f)) and f.startswith('skel') and f.endswith('json') and '_00' in f) sclt = Scenelet.load(pjoin(d_time, p_skel)) mn, mx = sclt.skeleton.get_frames_min_max() frame_id = mn + (mx - mn) // 2 if query.skeleton.has_pose(frame_id): pos_3d = query.skeleton.get_centroid_3d(frame_id) else: lin_id = frame_id - frame_ids[0] pos_3d = centroids[lin_id, :] # put centroid for each joint skeleton.set_pose(frame_id=frame_id, pose=np.tile(pos_3d[:, None], (1, 16))) with open(pjoin(d_time, 'avg_charness.json')) as fch: data = json.load(fch) set_or_max(skeleton.charness_poses, frame_id, data['avg_charness']) # if frame_id in skeleton.charness_poses: # lg.warning("Maxing charness at frame %d" % frame_id) # skeleton.charness_poses[frame_id] = max( # skeleton.charness_poses[frame_id], data['avg_charness']) # else: # skeleton.charness_poses[frame_id] = data['avg_charness'] # fit scores if 'score_fit' in sclt.aux_info: set_or_max(skeleton.score_fit, frame_id, sclt.aux_info['score_fit']) else: set_or_max(skeleton.score_fit, frame_id, 0.) if 'score_reproj' in sclt.aux_info: set_or_max(skeleton.score_reproj, frame_id, sclt.aux_info['score_reproj']) else: set_or_max(skeleton.score_reproj, frame_id, 0.) fig = plt.figure(figsize=(16, 12), dpi=100) ax = fig.add_subplot(121, aspect='equal') X = [] # skeleton x Z = [] # skeleton z (depth) C = [] # charness F = [] # score_fit R = [] # score_reproj T = [] # times for frame_id in skeleton.get_frames(): c = skeleton.get_joint_3d(6, frame_id=frame_id) X.append(c[0]) Z.append(c[2]) C.append(skeleton.charness_poses[frame_id]) F.append(skeleton.score_fit[frame_id]) R.append(skeleton.score_reproj[frame_id]) T.append(frame_id) ax.plot(X, Z, 'k--') for frame_id in skeleton.get_frames(): if frame_id % 5: continue c = skeleton.get_joint_3d(6, frame_id=frame_id) ax.annotate("%d" % frame_id, xy=(c[0], c[2]), zorder=5) cax = ax.scatter(X, Z, c=C, cmap='jet', zorder=5) fig.colorbar(cax) z_lim = (min(Z), max(Z)) z_span = (z_lim[1] - z_lim[0]) // 2 x_lim = min(X), max(X) x_span = (x_lim[1] - x_lim[0]) // 2 pad = .5 dspan = z_span - x_span if dspan > 0: ax.set_xlim(x_lim[0] - dspan - pad, x_lim[1] + dspan + pad) ax.set_ylim(z_lim[0] - pad, z_lim[1] + pad) else: ax.set_xlim(x_lim[0] - pad, x_lim[1] + pad) ax.set_ylim(z_lim[0] + dspan - pad, z_lim[1] - dspan + pad) ax.set_title('Fit score weighted characteristicness\ndisplayed at ' 'interpolated initial path position') ax = fig.add_subplot(122) ax.plot(T, C, 'x--', label='max charness') charness_threshes = [0.4, 0.35, 0.3] mn_thr_charness = min(charness_threshes) mx_thr_charness = max(charness_threshes) for ct in charness_threshes: ax.plot([T[0], T[-1]], [ct, ct], 'r') ax.annotate("charness %g" % ct, xy=(T[0], ct + 0.005)) charness_sorted = sorted([(fid, c) for fid, c in skeleton.charness_poses.items()], key=lambda e: e[1]) to_show = [] # Fitness divisor = 5. F_ = -np.log10(F) / divisor print(F_) ax.plot(T, F_, 'x--', label="-log_10(score) / %.0f" % divisor) mx_F_ = np.percentile(F_, 90) # np.max(F_) for i, (t, f) in enumerate(zip(T, F_)): if f > mx_F_ or any(C[i] > ct for ct in charness_threshes): to_show.append(i) # ax.annotate("%.4f" % (F[i]), xy=(t, f), xytext=(t+4, f-0.02), # arrowprops=dict(facecolor='none', shrink=0.03)) # charness # ax.annotate("%.3f\n#%d" % (C[i], t), xy=(t, C[i]), # xytext=(t-10, C[i]-0.02), # arrowprops=dict(facecolor='none', shrink=0.03)) windows = [] # [(t_start, t_max, t_end), ...] crossings = {} # Reproj R_ = -np.log10(R) / divisor # ax.plot(T, R_, 'x--', label="-log_10(score reproj) / %.0f" % divisor) mx_R_ = np.max(R_) is_above = [False for _ in charness_threshes] mx_above = [] for i, (t, r) in enumerate(zip(T, R_)): # if i in to_show: # ax.annotate("%.4f" % (R[i]), xy=(t, r), xytext=(t-10, r+0.02), # arrowprops=dict(facecolor='none', shrink=0.03)) # ax.annotate("%d" % t, xy=(t, r - 0.01)) if (i + 1 < len(C)) and (C[i] > C[i + 1]) and (C[i] > mn_thr_charness): mx_above.append((C[i], t)) for thr_i, thr in enumerate(charness_threshes): if (C[i] > thr) != is_above[thr_i] \ or (C[i] > mx_thr_charness and not is_above[thr_i]): step = 15 * (len(charness_threshes) - thr_i) \ if is_above[thr_i] \ else -15 * thr_i if is_above[thr_i]: if 'down' not in crossings: crossings['down'] = (C[i], t) # else: # assert crossings['down'][0] > C[i], (crossings['down'][0], C[i]) else: if 'up' not in crossings: crossings['up'] = (C[i - 1], t) elif crossings['up'][0] < C[i - 1]: crossings['up'] = (C[i - 1], t) # ax.annotate("%.3f\n#%d" % (C[i], t), xy=(t, C[i]), # xytext=(t + step, C[i]-0.1), # arrowprops=dict(facecolor='none', shrink=0.03)) if C[i] < mn_thr_charness and is_above[thr_i]: try: c, t = max((e for e in mx_above), key=lambda e: e[0]) ax.annotate("%.3f\n#%d" % (c, t), xy=(t, c), xytext=(t + step, c + 0.1), arrowprops=dict(facecolor='none', shrink=0.03)) mx_above = [] windows.append( (crossings['up'][1], t, crossings['down'][1])) except (KeyError, ValueError): lg.warning("Can't find gap: %s, %s" % (crossings, mx_above)) crossings = {} is_above[thr_i] = C[i] > thr break for crossing in windows: for i, t in enumerate(crossing): c = skeleton.charness_poses[t] step = -15 + i * 10 ax.annotate("%.3f\n#%d" % (c, t), xy=(t, c), xytext=(t + step, c - 0.1), arrowprops=dict(facecolor='none', shrink=0.03)) # extract_gaps([args.video]) # labels ax.set_title("Scores and charness w.r.t time: max charness: #%d %g" % (charness_sorted[-1][0], charness_sorted[-1][1])) ax.set_xlabel('integer time') ax.legend(loc='lower right') ax.grid(True) ax.yaxis.grid(which='both') ax.xaxis.set_ticks(np.arange(T[0] - 1, T[-1] + 1, 5)) ax.set_yticks([]) ax.set_ylim(0., 1.) ax.set_ylabel('higher is better') plt.suptitle("%s" % name_query) with open(os.path.join(d, 'charness_rank.csv'), 'w') as fout: fout.write("frame_id,charness\n") for fid_charness in reversed(charness_sorted): fout.write("{:d},{:g}\n".format(*fid_charness)) print(fid_charness) # plt.show() p_out = os.path.join(d, 'charnesses.svg') plt.savefig(p_out) lg.debug("saved to %s" % p_out)
def optimize_path(skel_ours, skel_ours_2d, images, intrinsics, path_skel, ground_rot, shape_orig=None, use_huber=False, weight_smooth=0.01, show=False, frames_ignore=None, resample=True, depth_init=10., p_constraints=None, smooth_mode=SmoothMode.ACCEL): """Optimize 3D path so that it matches the 2D corresponding observations. Args: skel_ours (Skeleton): 3D skeleton from LFD. skel_ours_2d (Skeleton): 2D feature points from LFD. images (dict): Color images for debug, keyed by frame_ids. camera_name (str): Initialize intrinsics matrix based on name of camera. path_skel (str): Path of input file from LFD on disk, used to create paths for intermediate result. shape_orig (tuple): Height and width of original images before LFD scaled them. use_huber (bool): Deprecated. weight_smooth (float): Smoothness term weight. winsorize_limit (float): Outlier detection parameter. show (bool): Show debug visualizations. frames_ignore (set): Deprecated. resample (bool): Fill in missing poses by interpolating using Blender's IK. depth_init (float): Initial depth for LFD poses. p_constraints (str): Path to 3D constraints scenelet file. smooth_mode (SmoothMode): Smooth velocity or acceleration. """ # scale 2D detections to canonical camera coordinates np_poses_2d = \ skel_ours_2d.poses[:, :2, :] \ - np.expand_dims(intrinsics[:2, 2], axis=1) np_poses_2d[:, 0, :] /= intrinsics[0, 0] np_poses_2d[:, 1, :] /= intrinsics[1, 1] n_frames = skel_ours.poses.shape[0] np_translation = np.zeros(shape=(n_frames, 3), dtype=np.float32) np_translation[:, 1] = -1. np_translation[:, 2] = \ np.random.uniform(-depth_init * 0.25, depth_init * 0.25, np_translation.shape[0]) \ + depth_init np_rotation = np.zeros(shape=(n_frames, 3), dtype=np.float32) frame_ids = np.array(skel_ours.get_frames(), dtype=np.float32) np_visibility = skel_ours_2d.get_confidence_matrix(frame_ids=frame_ids, dtype='f4') if p_constraints is not None: sclt_cnstr = Scenelet.load(p_constraints) np_cnstr_mask = np.zeros(shape=(len(frame_ids), Joint.get_num_joints()), dtype=np.float32) np_cnstr = np.zeros(shape=(len(frame_ids), 3, Joint.get_num_joints()), dtype=np.float32) for frame_id, confs in sclt_cnstr.confidence.items(): lin_id = None for j, conf in confs.items(): if conf > 0.5: if lin_id is None: lin_id = next( lin_id_ for lin_id_, frame_id_ in enumerate(frame_ids) if frame_id_ == frame_id) np_cnstr_mask[lin_id, j] = conf np_cnstr[lin_id, :, j] = \ sclt_cnstr.skeleton.get_joint_3d( joint_id=j, frame_id=frame_id) else: np_cnstr_mask = None np_cnstr = None spans = skel_ours.get_actor_empty_frames() dt = frame_ids[1:].astype(np.float32) \ - frame_ids[:-1].astype(np.float32) dt_pos_inv = np.reciprocal(dt, dtype=np.float32) dt_vel_inv = np.divide(np.float32(2.), dt[1:] + dt[:-1]) # ensure smoothness weight multipliers are not affected by # actor-transitions if skel_ours.n_actors > 1 and len(spans): for lin_id in range(len(dt)): frame_id0 = frame_ids[lin_id] frame_id1 = frame_ids[lin_id + 1] span = next((span_ for span_ in spans if span_[0] == frame_id0), None) if span is not None: assert frame_id1 == span[1], "No" dt[lin_id] = 0. dt_pos_inv[lin_id] = 0. dt_vel_inv[lin_id] = 0. dt_vel_inv[lin_id - 1] = 1. / dt[lin_id - 1] forwards = np.array([ skel_ours.get_forward(frame_id, estimate_ok=True, k=0) for frame_id in skel_ours.get_frames() ]) # from alignment import get_angle # xs = np.hstack(( # np.ones(shape=(len(forwards), 1)), # np.zeros(shape=(len(forwards), 2)) # )) # print(xs.shape) print(forwards.shape) unit_x = np.array((1., 0., 0.)) np_angles = [-np.arctan2(forward[2], forward[0]) for forward in forwards] print(forwards, np_angles) # ank_diff = \ # np.exp( # -2. * np.max( # [ # np.linalg.norm( # (skel_ours.poses[1:, :, joint] # - skel_ours.poses[:-1, :, joint]).T # * dt_pos_inv, axis=0 # ).astype(np.float32) # for joint in {Joint.LANK, Joint.RANK} # ], # axis=0 # ) # ) # assert ank_diff.shape == (skel_ours.poses.shape[0]-1,), \ # "Wrong shape: %s" % repr(ank_diff.shape) # cam_angle = [np.deg2rad(-8.)] assert np.isclose(ground_rot[1], 0.) and np.isclose(ground_rot[2], 0.), \ "Assumed only x rotation" # assert ground_rot[0] <= 0, "Negative means looking down, why looknig up?" cam_angle = [np.deg2rad(ground_rot[0])] # assert False, "Fixed angle!" device_name = '/gpu:0' if tf.test.is_gpu_available() else '/cpu:0' devices = {device_name} for device in devices: with Timer(device, verbose=True): graph = tf.Graph() with graph.as_default(), tf.device(device): tf_visibility = tf.Variable(np.tile(np_visibility, (1, 2, 1)), name='visibility', trainable=False, dtype=tf.float32) tf_dt_pos_inv = \ tf.Variable(np.tile(dt_pos_inv, (1, 3)).reshape(-1, 3), name='dt_pos_inv', trainable=False, dtype=tf.float32) tf_dt_vel_inv = \ tf.constant(np.tile(dt_vel_inv, (1, 3)).reshape(-1, 3), name='dt_vel_inv', dtype=tf.float32) # input data pos_3d_in = tf.Variable(skel_ours.poses.astype(np.float32), trainable=False, name='pos_3d_in', dtype=tf.float32) pos_2d_in = tf.Variable(np_poses_2d.astype(np.float32), trainable=False, name='pos_2d_in', dtype=tf.float32) params_camera = tf.Variable(initial_value=cam_angle, dtype=tf.float32, trainable=True) cam_sn = tf.sin(params_camera) cam_cs = tf.cos(params_camera) transform_camera = tf.reshape(tf.stack([ 1., 0., 0., 0., 0., cam_cs[0], cam_sn[0], 0., 0., -cam_sn[0], cam_cs[0], 0., 0., 0., 0., 1. ], axis=0), shape=(4, 4)) # 3D translation translation = tf.Variable(np_translation, name='translation') # 3D rotation (Euler XYZ) rotation = tf.Variable(np_rotation, name='rotation') fw_angles = tf.Variable(np_angles, name='angles') # rotation around y my_zeros = tf.zeros((n_frames, 1)) my_ones = tf.ones((n_frames, 1)) c = tf.cos(tf.slice(rotation, [0, 1], [n_frames, 1])) s = tf.sin(tf.slice(rotation, [0, 1], [n_frames, 1])) t0 = tf.concat([c, my_zeros, -s, my_zeros], axis=1) t1 = tf.concat([my_zeros, my_ones, my_zeros, my_zeros], axis=1) t2 = tf.concat([s, my_zeros, c, my_zeros], axis=1) t3 = tf.concat([my_zeros, my_zeros, my_zeros, my_ones], axis=1) transform = tf.stack([t0, t1, t2, t3], axis=2, name="transform") transform = tf.einsum('ij,ajk->aik', transform_camera, transform)[:, :3, :3] # transform to 3d pos_3d = tf.matmul(transform, pos_3d_in) \ + tf.tile(tf.expand_dims(translation, 2), [1, 1, int(pos_3d_in.shape[2])]) # constraints loss_cnstr = None if np_cnstr is not None: constraints = tf.Variable(np_cnstr, trainable=False, name='constraints', dtype=tf.float32) constraints_mask = tf.Variable(np_cnstr_mask, trainable=False, name='constraints_mask', dtype=tf.float32) cnstr_diff = tf.reduce_sum(tf.squared_difference( pos_3d, constraints), axis=1, name='constraints_difference') cnstr_diff_masked = tf.multiply( constraints_mask, cnstr_diff, name='constraints_difference_masked') loss_cnstr = tf.reduce_sum(cnstr_diff_masked, name='constraints_loss') # perspective divide pos_2d = tf.divide( tf.slice(pos_3d, [0, 0, 0], [n_frames, 2, -1]), tf.slice(pos_3d, [0, 2, 0], [n_frames, 1, -1])) if use_huber: diff = huber_loss(pos_2d_in, pos_2d, 1.) masked = diff * tf_visibility loss_reproj = tf.nn.l2_loss(masked) lg.info("Doing huber on reprojection, NOT translation") else: # re-projection loss diff = pos_2d - pos_2d_in # mask loss by 2d key-point visibility masked = diff * tf_visibility loss_reproj = tf.nn.l2_loss(masked) lg.info("NOT doing huber") sys.stderr.write( "TODO: Move huber to translation, not reconstruction\n") # translation smoothness dx = tf.multiply( x=0.5, y=tf.add( pos_3d[1:, :, Joint.LHIP] - pos_3d[:-1, :, Joint.LHIP], pos_3d[1:, :, Joint.RHIP] - pos_3d[:-1, :, Joint.RHIP], ), name="average_hip_displacement_3d") tf_velocity = tf.multiply(dx, tf_dt_pos_inv) tf_acceleration_z = tf.multiply(x=dx[1:, 2:3] - dx[:-1, 2:3], y=tf_dt_vel_inv[:, 2:3], name="acceleration_z") if smooth_mode == SmoothMode.VELOCITY: # if GT, use full smoothness to fix 2-frame flicker if np_cnstr is not None: print('Smoothing all velocity!') loss_transl_smooth = \ weight_smooth * tf.nn.l2_loss(tf_velocity) else: # Normal mode, don't oversmooth screen-space loss_transl_smooth = \ weight_smooth * tf.nn.l2_loss(tf_velocity[:, 2:3]) elif smooth_mode == SmoothMode.ACCEL: loss_transl_smooth = \ weight_smooth * tf.nn.l2_loss(tf_acceleration_z) else: raise RuntimeError( 'Unknown smooth mode: {}'.format(smooth_mode)) if show: sqr_accel_z = weight_smooth * tf.square(tf_acceleration_z) if weight_smooth > 0.: lg.info("Smoothing in time!") loss = loss_reproj + loss_transl_smooth else: lg.warning("Not smoothing!") loss = loss_reproj if loss_cnstr is not None: loss += 1000 * loss_cnstr # hip0 = tf.nn.l2_normalize(pos_3d[:-1, :, Joint.RHIP] - pos_3d[:-1, :, Joint.LHIP]) # hip1 = tf.nn.l2_normalize(pos_3d[1:, :, Joint.RHIP] - pos_3d[1:, :, Joint.RHIP]) # dots = tf.reduce_sum(tf.multiply(hip0, hip1), axis=1) # print(dots) # loss_dot = tf.nn.l2_loss(1. - dots) # loss_ang = fw_angles + rotation[:, 1] # print(loss_ang) # loss_ang = tf.square(loss_ang[1:] - loss_ang[:-1]) # print(loss_ang) # two_pi_sqr = tf.constant((2. * 3.14159)**2., dtype=tf.float32) # print(two_pi_sqr) # loss_ang = tf.reduce_mean(tf.where(loss_ang > two_pi_sqr, loss_ang - two_pi_sqr, loss_ang)) # print(loss_ang) # loss += loss_ang # # optimize # optimizer = ScipyOptimizerInterface( loss, var_list=[translation, rotation], options={'gtol': 1e-12}, var_to_bounds={rotation: (-np.pi / 2., np.pi / 2.)}) with tf.Session(graph=graph) as session: session.run(tf.global_variables_initializer()) optimizer.minimize(session) np_pos_3d_out, np_pos_2d_out, np_transl_out, np_masked, \ np_acceleration, np_loss_transl_smooth, np_dt_vel = \ session.run([pos_3d, pos_2d, translation, masked, tf_acceleration_z, loss_transl_smooth, tf_dt_vel_inv]) if show: o_sqr_accel_z = session.run(sqr_accel_z) o_vel = session.run(tf_velocity) o_dx = session.run(dx) o_rot = session.run(rotation) # o_dx, o_dx2 = session.run([accel_bak, acceleration2]) # assert np.allclose(o_dx, o_dx2), "no" o_cam = session.run(fetches=[params_camera]) print("camera angle: %s" % np.rad2deg(o_cam[0])) # o_losses = session.run([loss_reproj, loss_transl_smooth, loss_dot, loss_ang]) o_losses = session.run([loss_reproj, loss_transl_smooth]) print('losses: {}'.format(o_losses)) # o_dots = session.run(dots) # with open('tmp/dots.txt', 'w') as fout: # fout.write('\n'.join((str(e) for e in o_dots.tolist()))) fixed_frames = [] # for lin_frame_id in range(np_transl_out.shape[0]): # if np_transl_out[lin_frame_id, 2] < 0.: # print("Correcting frame_id %d: %s" # % (skel_ours.get_lin_id_for_frame_id(lin_frame_id), # np_transl_out[lin_frame_id, :])) # if lin_frame_id > 0: # np_transl_out[lin_frame_id, :] = np_transl_out[lin_frame_id-1, :] # else: # np_transl_out[lin_frame_id, :] = np_transl_out[lin_frame_id+1, :] # fixed_frames.append(lin_frame_id) # debug_forwards(skel_ours.poses, np_pos_3d_out, o_rot, forwards, np_angles) # z_jumps = np_pos_3d_out[1:, 2, Joint.PELV] - np_pos_3d_out[:-1, 2, Joint.PELV] # out = scipy.stats.mstats.winsorize(z_jumps, limits=1.) # plt.figure() # plt.plot(pos_3d[:, 2, Joint.PELV]) # plt.show() # sys.exit(0) # diff = np.linalg.norm(out - displ, axis=1) if len(fixed_frames): print("Re-optimizing...") with tf.Session(graph=graph) as session: np_pos_3d_out, np_pos_2d_out, np_transl_out = \ session.run(fetches=[pos_3d, pos_2d, translation], feed_dict={transform: np_transl_out}) if show: lim_fr = [105, 115, 135] fig = plt.figure() accel_thr = 0. # np.percentile(o_sqr_accel_z, 25) ax = plt.subplot2grid((2, 2), (0, 0), colspan=2) # print("np_masked:%s" % np_masked) # plt.plot(np_masked[:, ) ax.plot(np.linalg.norm(np_acceleration[lim_fr[0]:lim_fr[1]], axis=1), '--o', label='accel') ax.add_artist(Line2D([0, len(o_sqr_accel_z)], [accel_thr, accel_thr])) # plt.plot(np_dt_vel[:, 0], label='dt velocity') # plt.plot(np.linalg.norm(np_f_accel, axis=1), '--x', label='f_accel') # plt.plot(ank_diff, label='ank_diff') ax.plot(o_sqr_accel_z[lim_fr[0]:lim_fr[1] + 1], '--x', label='loss accel_z') ax.legend() ax2 = plt.subplot2grid((2, 2), (1, 0), aspect='equal') ax2.plot(np_pos_3d_out[lim_fr[0]:lim_fr[1] + 1, 0, Joint.PELV], np_pos_3d_out[lim_fr[0]:lim_fr[1] + 1, 2, Joint.PELV], '--x') for i, vel in enumerate(o_vel): if not (lim_fr[0] <= i <= lim_fr[1]): continue p0 = np_pos_3d_out[i + 1, [0, 2], Joint.PELV] p1 = np_pos_3d_out[i, [0, 2], Joint.PELV] ax2.annotate( "%f = ((%g - %g) + (%g - %g)) * %g = %g" % (vel[2], np_pos_3d_out[i + 1, 2, Joint.LHIP], np_pos_3d_out[i, 2, Joint.LHIP], np_pos_3d_out[i + 1, 2, Joint.RHIP], np_pos_3d_out[i, 2, Joint.RHIP], np_dt_vel[i, 2], o_dx[i, 2]), xy=((p0[0] + p1[0]) / 2., (p0[1] + p1[1]) / 2.)) ax2.set_title('velocities') ax1 = plt.subplot2grid((2, 2), (1, 1), aspect='equal') ax1.plot(np_pos_3d_out[lim_fr[0]:lim_fr[1] + 1, 0, Joint.PELV], np_pos_3d_out[lim_fr[0]:lim_fr[1] + 1, 2, Joint.PELV], '--x') for i, lacc in enumerate(o_sqr_accel_z): if not (lim_fr[0] <= i <= lim_fr[1]): continue if lacc > accel_thr: p0 = np_pos_3d_out[i + 1, [0, 2], Joint.PELV] ax1.annotate("%.3f" % np_acceleration[i], xy=(p0[0], p0[1])) ax.annotate("%.3f" % np.log10(lacc), xy=(i - lim_fr[0], abs(np_acceleration[i]))) ax1.set_title('accelerations') plt.show() np.set_printoptions(linewidth=200) np_pos_2d_out[:, 0, :] *= intrinsics[0, 0] np_pos_2d_out[:, 1, :] *= intrinsics[1, 1] np_pos_2d_out[:, 0, :] += intrinsics[0, 2] np_pos_2d_out[:, 1, :] += intrinsics[1, 2] np_poses_2d[:, 0, :] *= intrinsics[0, 0] np_poses_2d[:, 1, :] *= intrinsics[1, 1] np_poses_2d[:, 0, :] += intrinsics[0, 2] np_poses_2d[:, 1, :] += intrinsics[1, 2] out_images = {} if shape_orig is not None: frames_2d = skel_ours_2d.get_frames() for frame_id2 in frames_2d: try: lin_frame_id = skel_ours_2d.get_lin_id_for_frame_id(frame_id2) except KeyError: lin_frame_id = None frame_id = skel_ours_2d.mod_frame_id(frame_id=frame_id2) im = None if frame_id in out_images: im = out_images[frame_id] elif len(images): if frame_id not in images: lg.warning("Not enough images, the video was probably cut " "after LiftingFromTheDeep was run.") continue im = copy.deepcopy(images[frame_id]) im = cv2.cvtColor(im, cv2.COLOR_RGB2BGR) else: im = np.zeros( (shape_orig[0].astype(int), shape_orig[1].astype(int), 3), dtype='i1') if lin_frame_id is not None: for jid in range(np_pos_2d_out.shape[2]): if skel_ours_2d.is_visible(frame_id2, jid): p2d = tuple(np_pos_2d_out[lin_frame_id, :, jid].astype(int).tolist()) p2d_det = tuple(np_poses_2d[lin_frame_id, :, jid].astype(int).tolist()) cv2.line(im, p2d, p2d_det, color=(100, 100, 100), thickness=3) cv2.circle(im, p2d, radius=3, color=(0, 0, 200), thickness=-1) cv2.circle(im, p2d_det, radius=3, color=(0, 200, 0), thickness=-1) out_images[frame_id] = im # cv2.imshow("Out", im) # cv2.waitKey(50) if False: # visualize fig = plt.figure() ax = fig.gca(projection='3d') for frame_id in range(0, np_pos_3d_out.shape[0], 1): j = Joint.PELV ax.scatter(np_pos_3d_out[frame_id, 0, j], np_pos_3d_out[frame_id, 2, j], -np_pos_3d_out[frame_id, 1, j], marker='o') # smallest = np_pos_3d_out.min() # largest = np_pos_3d_out.max() ax.set_xlim3d(-5., 5.) ax.set_xlabel('x') ax.set_ylim3d(-5., 5.) ax.set_ylabel('y') ax.set_zlim3d(-5., 5.) ax.set_zlabel('z') if False: # visualize fig = plt.figure() ax = fig.gca(projection='3d') for frame_id in range(0, np_pos_3d_out.shape[0], 1): for j in range(np_pos_3d_out.shape[2]): ax.scatter(np_pos_3d_out[frame_id, 0, j], np_pos_3d_out[frame_id, 2, j], -np_pos_3d_out[frame_id, 1, j], marker='o') # smallest = np_pos_3d_out.min() # largest = np_pos_3d_out.max() ax.set_xlim3d(-5., 5.) ax.set_xlabel('x') ax.set_ylim3d(-5., 5.) ax.set_ylabel('y') ax.set_zlim3d(-5., 5.) ax.set_zlabel('z') plt.show() assert all(a == b for a, b in zip(skel_ours.poses.shape, np_pos_3d_out.shape)), \ "no" skel_ours.poses = np_pos_3d_out return skel_ours, out_images, intrinsics
def is_confidence_normalized(self): lg.warning("Assuming unnormalized confidence data.") return False
def create_objects_2d_mgrids(self, transforms, um): assert self.is_objects_2d_created, "Call create_objects_2d first" dtype_tf = transforms.dtype dtype_np = dtype_tf.as_numpy_dtype mgrid_vxs = tf.Variable(initial_value=um.obj_2d_mgrid_vxs, trainable=False, name='obj_2d_mgrid_vertices') mgrid_indices = tf.constant( value=um.obj_2d_mgrid_transform_indices[:, None], name='mgrid_indices', dtype=tf.int32) mgrid_transforms_tiled = tf.gather_nd(transforms, indices=mgrid_indices, name='mgrid_transforms_tiled') self._obj_2d_mgrid_vertices_transformed = tf.add( x=tf.squeeze(tf.matmul(a=mgrid_transforms_tiled[:, :, :3], b=mgrid_vxs[:, :, None]), axis=-1), y=mgrid_transforms_tiled[:, :, 3], name="obj_2d_mgrid_vertices_transformed") self.oo_mask_same = tf.not_equal( tf.cast(mgrid_indices, dtype=tf.int64), tf.cast(tf.transpose(self._obj_2d_poly_transform_indices[:, 0:1]), dtype=tf.int64), name='oo_mask_same') lg.warning("Use numpy based mask for less memory") # TODO: use this instead oo_mask_same_np = np.not_equal( um.obj_2d_mgrid_transform_indices[:, None], um.obj_2d_transform_indices[None, ::4]).astype('b1') # self.oo_mask_same_2 = tf.constant( # value=oo_mask_same_np, # dtype=tf.bool, # shape=(mgrid_indices.get_shape().as_list()[0], # self._obj_2d_poly_transform_indices.get_shape().as_list()[0]), # name='oo_mask_2', # verify_shape=True) # n_grid_points x 1 lg.warning("Use numpy based cat mask for less memory") cat_ids_mgrids = tf.constant(value=um.obj_2d_mgrid_cat_ids[:, None], name='oo_cat_ids_mgrids', dtype=tf.int32) # n_grid_points x n_polys self.oo_mask_cat = tf.logical_and( tf.not_equal(tf.cast(cat_ids_mgrids, tf.int64), tf.cast(self.cat_ids_polys, tf.int64), name='oo_mask_cat_same'), tf.constant( um.obj_2d_cat_ids_per_poly[None, :] != CATEGORIES['table'], dtype=tf.bool, name='oo_mask_cat_is_table'), name='oo_mask_cat') oo_mask_cat_np = np.not_equal( um.obj_2d_mgrid_cat_ids[:, None], um.obj_2d_cat_ids_per_poly[None, :]).astype('b1') self.oo_mask_cat_2 = tf.constant( oo_mask_cat_np, shape=(cat_ids_mgrids.get_shape().as_list()[0], self.cat_ids_polys.get_shape().as_list()[1]), verify_shape=True, name='oo_mask_cat_2') # TODO: use this instead self.oo_mask_interacting_2 = np.logical_and( oo_mask_same_np, oo_mask_cat_np).astype('b1') self._oo_mask_interacting = tf.logical_and(self.oo_mask_cat, self.oo_mask_same, name='oo_mask_interacting') # self._oo_mask_interacting_sum_inv = tf.reciprocal( # tf.cast(tf.reduce_sum(tf.cast(self._oo_mask_interacting, # dtype=tf.int32)), # dtype=dtype_tf)) # TODO: use this instead self._np_oo_sum = np.sum(self.oo_mask_interacting_2.astype('i4')) \ .astype(dtype_np) self._oo_mask_interacting_sum_inv = tf.constant( value=np.reciprocal(self._np_oo_sum if self._np_oo_sum > 0 else 1), dtype=dtype_tf, name='loss_oo_normalizer')