def get_rectangle(poly, angle): """ :param poly: :param angle: :return: [cx, cy, theta, sx, sy] """ poly2 = saffinity.affine_transform( poly, [np.cos(angle), -np.sin(angle), np.sin(angle), np.cos(angle), 0, 0] # a, b, d, e, cx, cy ) params = [ poly.centroid.x, poly.centroid.y, angle, poly2.bounds[2] - poly2.bounds[0], poly2.bounds[3] - poly2.bounds[1] ] if False: fig = plt.figure() ax = fig.add_subplot(111, aspect='equal') ax.add_artist(PolygonPatch(poly, facecolor='b', alpha=0.9)) lg.debug("poly: %s,\nangle: %s" % (poly, np.rad2deg(angle))) lg.debug("bounds: %s" % repr(poly.bounds)) lg.debug("poly2.centroid: %s" % poly2.centroid) lg.debug("poly.centroid: %s" % poly.centroid) dc = (poly.centroid.x - poly2.centroid.x, poly.centroid.y - poly2.centroid.y) lg.debug("dc: %s" % repr(dc)) poly2 = saffinity.translate(poly2, dc[0], dc[1]) ax.add_artist(PolygonPatch(poly2, facecolor='r', alpha=0.8)) ax.set_xlim(min(poly.bounds[0], poly2.bounds[0]), max(poly.bounds[2], poly2.bounds[2])) ax.set_ylim(min(poly.bounds[1], poly2.bounds[1]), max(poly.bounds[3], poly2.bounds[3])) plt.show() return params
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 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 filter_wrong_poses( skel_ours_2d, skel_ours_3d, d_thresh=Conf.get().optimize_path.head_ank_dthresh, l_torso_thresh=Conf.get().optimize_path.torso_length_thresh, show=False): """Attempts to filter poses, where ankles are too close to the head. Remember, up is -y, so lower y coordinate means "higher" in space. TODO: fix transition between multiple actors """ if show: fig = plt.figure(figsize=(12, 4)) ax = fig.add_subplot(121) y0 = [] y1 = [] l_torsos = [] d = [] x = [] frames = np.array(skel_ours_2d.get_frames(), dtype='i8') frames.flags.writeable = False for frame_id in frames: pose = skel_ours_3d.get_pose(frame_id=frame_id) x.append(frame_id) top = np.min(pose[1, [Joint.HEAD, Joint.LSHO, Joint.RSHO]]) bottom = min(pose[1, Joint.LANK], pose[1, Joint.RANK]) y0.append(-top) y1.append(-bottom) l_torso = np.linalg.norm(pose[:, Joint.THRX] - pose[:, Joint.PELV]) l_torsos.append(l_torso) # d.append(-(top - bottom)) l_torsos = np.array(l_torsos) y0_f = medfilt(y0, 3) y1_f = medfilt(y1, 3) dt_b = np.logical_and( np.concatenate(([True], np.less(frames[1:] - frames[:-1], 2))), np.concatenate((np.less(frames[:-1] - frames[1:], 2), [True]))) y0 = np.where(dt_b, y0_f, y0) y1 = np.where(dt_b, y1_f, y1) d = (y0 - y1) x = np.array(x, dtype='i8') frames_to_remove = x[d < d_thresh] min_y = min(np.min(y0), np.min(y1)) y0 -= min_y y1 -= min_y if show: ax.plot(x, y0, 'g', label='max(head, *sho)') ax.plot(x, y1, 'b', label='min(*ank)') ax.plot(x, d, 'm', label='diff') for frame_id in frames_to_remove: ax.scatter(frame_id, skel_ours_3d.get_joint_3d(Joint.LANK, frame_id=frame_id)[1], c='r', marker='o') ax.legend(bbox_to_anchor=(1.01, 1)) ax.set_title("3D Local space joint positions over time") ax.set_xlabel("Discrete time") ax.set_ylabel("Height of joints") ax = fig.add_subplot(122) ax.plot(x, l_torsos) plt.subplots_adjust(right=0.7, wspace=0.5) frames_to_remove = list( set( np.append(frames_to_remove, x[l_torsos < l_torso_thresh]).tolist())) if show: for frame_id in frames_to_remove: ax.scatter(frame_id, skel_ours_3d.get_joint_3d(Joint.LANK, frame_id=frame_id)[1], c='r', marker='o') plt.show() # plt.pause(100) plt.close() # sys.exit() # for frame_id in frames_to_remove: # skel_ours_2d.remove_pose(frame_id) return frames_to_remove #, skel_ours_2d
def filter_outliers(skel_ours_2d, winsorize_limit=0.05, show=False): lg.debug('[filter_outliers] Filtering based on 2D displacement...') seq = skel_ours_2d.poses[:, :2, :] displ = seq[1:, :, :] - seq[:-1, :, :] frames = np.array(skel_ours_2d.get_frames_mod()) assert displ.shape[0] == seq.shape[0] - 1 \ and displ.shape[1] == seq.shape[1] \ and displ.shape[2] == seq.shape[2], \ "No: %s" % displ.shape displ = np.linalg.norm(displ, axis=1) assert displ.shape == (seq.shape[0] - 1, seq.shape[2]), \ "No: %s" % repr(displ.shape) if show: plt.figure() # Ensure we only filter consequtive frames dtime = frames[1:] - frames[:-1] # lg.debug("delta time: %s" % dtime) out = scipy.stats.mstats.winsorize(displ, limits=winsorize_limit) # diff = np.linalg.norm(out - displ, axis=1) diff = out - displ fraction_corrected = np.sum((diff < -1.).astype('i4'), axis=1) \ / float(diff.shape[1]) # print("diff: %s" % diff) # threshold changed from 5. to 0.6 on 19/1/2018 # threshold changed from 0.6 to 0.5 on 20/1/2018 lin_ids_to_remove = np.argwhere(fraction_corrected > 0.6) frame_ids_to_remove = \ [skel_ours_2d.get_frame_id_for_lin_id(lin_id) for lin_id in np.squeeze(lin_ids_to_remove, axis=1).tolist() if dtime[lin_id] == 1 and (lin_id+1 >= dtime.size or dtime[lin_id+1] == 1)] cpy = copy.deepcopy(skel_ours_2d) for frame_id in frame_ids_to_remove: lg.debug("Removing frame_id %d because it jumped in 2D." % frame_id) skel_ours_2d.remove_pose(frame_id) for frame_id_ in skel_ours_2d._frame_ids.keys(): if frame_id_ != frame_id: assert np.allclose(skel_ours_2d.get_pose(frame_id_), cpy.get_pose(frame_id_)), \ "No (frame_id: %d, lin_id: %d, old lin_id: %d)\nnew: %s\nold:\n%s" \ % (frame_id_, skel_ours_2d.get_lin_id_for_frame_id(frame_id_), cpy.get_lin_id_for_frame_id(frame_id_), skel_ours_2d.get_pose(frame_id_), cpy.get_pose(frame_id_) ) # mask = np.where(diff < 10., True, False) # if mask[-1]: # mask = np.append(mask, mask[-1]) # else: # mask = np.insert(mask, 0, mask[0]) # print(mask) # skel_ours_2d._poses = skel_ours_2d._poses[mask, :, :] # assert list(skel_ours_2d._frame_ids.values()) == sorted(list(skel_ours_2d._frame_ids.values())), \ # "Not sorted: %s" % list(skel_ours_2d._frame_ids.values()) # skel_ours_2d._frame_ids = dict((k, v) for i, (k, v) in enumerate(skel_ours_2d._frame_ids.items()) if mask[i]) # assert list(skel_ours_2d._frame_ids.values()) == sorted(list(skel_ours_2d._frame_ids.values())), \ # "Not sorted: %s" % list(skel_ours_2d._frame_ids.values()) if show: plt.plot(diff, 'k') plt.plot(displ[:, 6], 'g') plt.plot(out[:, 6], 'b') plt.show() return skel_ours_2d, frame_ids_to_remove