def get_confidences(self, frame_id):
"""
Args:
frame_id (int): Frame id in question.
Returns:
confidences (np.ndarray): (N, 16)
Array of confidences
"""
N_JOINTS = Joint.get_num_joints()
skeleton = self._skeleton
confs = [] # type: List(List(float))
for actor_id in range(skeleton.n_actors):
frame_id2 = skeleton.unmod_frame_id(
frame_id=frame_id, actor_id=actor_id,
frames_mod=skeleton._frames_mod
)
if skeleton.has_pose(frame_id2):
_confs = [
skeleton.get_confidence(frame_id=frame_id2, joint=j)
for j in range(N_JOINTS)] # type: List(float)
actor_and_frame_ids = self._pose_ids[frame_id][len(confs)] # type: ActorAndFrameId
assert actor_and_frame_ids.frame_id == frame_id \
and actor_and_frame_ids.frame_id2 == frame_id2 \
and actor_and_frame_ids.actor_id == actor_id
confs.append(_confs)
# else:
# lg.warning("Warning, no pose for %d %d"
# % (frame_id, frame_id2))
return np.array(confs)
def extract_skeleton(scene,
frame_ids=None,
frame_multiplier=1.,
time_multiplier=1.):
joints = {
ob.name.split('.')[1]: ob
for ob in bpy.data.objects
if ob.name.startswith('Output') and ob.name.endswith('Sphere')
}
print("joints: %s" % joints)
assert len(joints) == Joint.get_num_joints(), \
"No: %s != %s" % (len(joints), Joint.get_num_joints())
if not frame_ids:
frame_ids = range(scene.frame_start, scene.frame_end + 1)
skeleton = Skeleton()
for frame_id in frame_ids:
o_frame_id = int(round(frame_id * frame_multiplier))
if skeleton.has_pose(o_frame_id):
print("skipping %s, because already set" % frame_id)
continue
print("frame_id: %s, o_frame_id: %s" % (frame_id, o_frame_id))
scene.frame_set(frame_id)
bpy.context.scene.update()
pose = np.zeros(shape=(Skeleton.DIM, len(joints)))
for joint, ob in joints.items():
pos = ob.matrix_world.col[3]
joint_id = Joint.from_string(joint)
pose[:, joint_id] = from_blender(pos)
assert not skeleton.has_pose(o_frame_id), "Already has %s" % frame_id
skeleton.set_pose(frame_id=o_frame_id,
pose=pose,
time=o_frame_id * time_multiplier)
# scenelet = Scenelet(skeleton=skeleton)
# scenelet.save(path=path_out, save_obj=False)
return skeleton
def add(self, entry, gt, occluded, frame_id, scale=1.):
assert entry.shape[0] <= 3 and gt.shape[0] <= 3, (entry, gt)
diff = np.linalg.norm(entry - gt, axis=0) * scale
sub = []
for j in range(Joint.get_num_joints()):
if j in (Joint.PELV, Joint.NECK):
continue
if occluded[j]:
sub.append(diff[j])
self._stats.add(occluded=OccludedType.OCCLUDED,
title=self._title,
name_method=self._name_method,
frame_id=frame_id,
diff=diff[j])
else:
self._stats.add(occluded=OccludedType.VISIBLE,
title=self._title,
name_method=self._name_method,
frame_id=frame_id,
diff=diff[j])
self._data.extend(sub)
def extract_annotated_scenelet(
scene,
prefix_obj='obb',
frame_ids=None,
frame_multiplier=1.,
time_multiplier=1.,
f_ob_is_joint=lambda ob: ob.name.startswith(
'Output') and ob.name.endswith('Sphere'),
f_joint_name_from_ob=lambda ob: ob.name.split('.')[1]):
"""
Args:
scene (bpy.types.Scene):
The current scene (e.g. bpy.context.scene).
prefix_obj (str):
Start of object names that we want to include in the scenelet
as oriented bounding boxes.
frame_ids (List[int]):
A subset of frame IDs to export.
frame_multiplier (float):
Scaling for frame IDs. The result will be rounded and truncated.
output.frame_id := int(round(frame_id * frame_multiplier))
time_multipler (float):
Scaling for times associated with frame_ids.
output.time := int(round(frame_id * frame_multiplier))
* time_multiplier.
f_ob_is_joint (Callable[[bpy.types.Object], bool]]):
Decides if a Blender object is a joint.
f_joint_name_from_ob (Callable[[bpy.types.Object], str]):
Gets the joint name from the Blender object name.
"""
# joints = {
# ob.name.split('.')[1]: ob
# for ob in bpy.data.objects
# if ob.name.startswith('Output') and ob.name.endswith('Sphere')}
joints = {
f_joint_name_from_ob(ob): ob
for ob in bpy.data.objects if f_ob_is_joint(ob)
}
print("joints: %s" % joints)
skeleton = Skeleton()
if len(joints):
assert len(joints) == 16, "No: %s" % len(joints)
if not frame_ids:
frame_ids = range(scene.frame_start, scene.frame_end + 1)
for frame_id in frame_ids:
o_frame_id = int(round(frame_id * frame_multiplier))
if skeleton.has_pose(o_frame_id):
print("skipping %s" % frame_id)
continue
print("frame_id: %s" % frame_id)
scene.frame_set(frame_id)
bpy.context.scene.update()
# bpy.ops.anim.change_frame(frame_id)
pose = np.zeros(shape=(3, len(joints)))
for joint, ob in joints.items():
pos = ob.matrix_world.col[3]
print("pos[%s]: %s" % (ob.name, pos))
joint_id = Joint.from_string(joint)
print("joint %s is %s" % (joint, Joint(joint_id)))
pose[:, joint_id] = from_blender(pos)
print("o_frame: %s from %s" % (o_frame_id, frame_id))
assert not skeleton.has_pose(o_frame_id), \
"Already has %s" % frame_id
skeleton.set_pose(frame_id=o_frame_id,
pose=pose,
time=o_frame_id * time_multiplier)
objs_bl = {}
for obj in bpy.data.objects:
if obj.name.startswith(prefix_obj) and not obj.hide:
obj_id = int(obj.name.split('_')[1])
try:
objs_bl[obj_id].append(obj)
except KeyError:
objs_bl[obj_id] = [obj]
print("objs: %s" % objs_bl)
scenelet = Scenelet(skeleton=skeleton)
print("scenelet: %s" % scenelet)
for obj_id, parts_bl in objs_bl.items():
name_category = None
scene_obj = None
for part_id, part_bl in enumerate(parts_bl):
transl, rot, scale = part_bl.matrix_world.decompose()
rot = rot.to_matrix()
if any(comp < 0. for comp in scale):
scale *= -1.
rot *= -1.
assert not any(comp < 0. for comp in scale), "No: %s" % scale
matrix_world = part_bl.matrix_world.copy()
# need to save full scale, not only half axes
for c in range(3):
for r in range(3):
matrix_world[r][c] *= 2.
name_parts = part_bl.name.split('_')
if name_category is None:
name_category = name_parts[2]
scene_obj = SceneObj(label=name_category)
else:
assert name_category == name_parts[2], \
"No: %s %s" % (name_category, name_parts[2])
name_part = name_parts[3]
print("part: %s" % name_part)
part = SceneObjPart(name_part)
part.obb = Obb(centroid=np.array(
from_blender([transl[0], transl[1], transl[2]])),
axes=np.array([[rot[0][0], rot[0][1], rot[0][2]],
[-rot[2][0], -rot[2][1], -rot[2][2]],
[rot[1][0], rot[1][1], rot[1][2]]]),
scales=np.array(
[scale[0] * 2., scale[1] * 2., scale[2] * 2.]))
# if 'table' in name_category:
# print(part.obb.axes)
# raise RuntimeError("stop")
print("obb: %s" % part.obb.to_json(0))
scene_obj.add_part(part_id, part)
scenelet.add_object(obj_id, scene_obj, clone=False)
return scenelet
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 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 show_multi(images, data, pose_ids, problem, p_dir,
thresh_log_conf=Conf.get().path.thresh_log_conf,
first_run=False, n_actors=1):
"""
Args:
images (Dict[int, np.ndarray]):
data (SkeletonPosesWrapper):
pose_ids (Dict[str, Dict[int, int]]):
problem:
p_dir (str):
thresh_log_conf:
first_run (bool):
Will output labeling_orig if True allowing the inspection of
pose_ids.
"""
_confs = []
# colors = {
# 0: (.8, .1, .1, 1.),
# 1: (.1, .8, .1, 1.),
# 2: (.8, .8, .1, 1.),
# 3: (.1, .8, .8, 1.),
# 4: (.8, .1, .8, 1.),
# 5: (.6, .4, .8, 1.),
# 6: (.6, .4, .8, 1.)
# }
color_norm = cmNormalize(vmin=0, vmax=n_actors+1)
scalar_map = cm.ScalarMappable(norm=color_norm, cmap='gist_earth')
colors = [tuple(c for c in scalar_map.to_rgba(i+1))
for i in range(n_actors)]
p_labeling = os.path.join(p_dir, 'debug',
'labeling' if not first_run else 'labeling_orig')
try:
os.makedirs(p_labeling)
except OSError:
pass
limits = (min(fid for fid in images), max(fid for fid in images)+1)
scale = None
for frame_id in range(limits[0], limits[1]):
frame_str = "color_%05d" % frame_id
# try:
# frame_id = int(frame_str.split('_')[1])
# except ValueError:
# print("skipping key %s" % frame_id)
# continue
# im = cv2.cvtColor(images[frame_id], cv2.COLOR_RGB2BGR)
im = images[frame_id].copy()
if im.shape[1] < 1900:
if scale is None:
scale = 1900 // im.shape[1] + 1
im = cv2.resize(im, dsize=None, fx=scale, fy=scale,
interpolation=cv2.INTER_CUBIC)
elif scale is None:
scale = 1.
# for frame_id in data.get_frames():
# frame_str = "color_%05d" % frame_id
# pose_in = np.asarray(data[frame_str][u'centered_3d'])
pose_in_2d = data.get_poses_2d(frame_id=frame_id)
# np.asarray(data[frame_str][u'pose_2d'])
# visible = np.asarray(data[frame_str][u'visible'])
# vis_f = np.asarray(data[frame_str][u'visible_float'])
vis_f = data.get_confidences(frame_id=frame_id)
# pose_id = pose_ids[frame_id]
for pose_id in range(pose_in_2d.shape[0]):
actor_id = next(
(actor_id_
for actor_id_, pose_id_ in pose_ids[frame_id].items()
if pose_id == pose_id_),
None)
if actor_id is None:
ccolor = (0.5, 0.5, 0.5, 1.)
else:
ccolor = colors[actor_id % len(colors)]
_confs.append(vis_f[pose_id:pose_id+1, :])
color = tuple(int(c_ * 255) for c_ in ccolor[:3])
# threshed = get_conf_thresholded(vis_f[pose_id:pose_id+1, :],
# thresh_log_conf=thresh_log_conf,
# dtype_np=np.float32)
# lg.debug("avg_vis: %s" % threshed)
# avg_vis = np.count_nonzero(threshed > 0.05, axis=1)
# if avg_vis > 0.4:
p2d_mean = np.mean(pose_in_2d[pose_id, :, 1]) * scale
# cv2.putText(im, "%.2f" % (avg_vis / threshed.shape[1]),
# (int(p2d_mean) - 20, 50), 1, 1, thickness=2,
# color=(200, 200, 200))
if actor_id is None:
actor_id = -1
cv2.putText(im, "a%d" % actor_id,
(int(p2d_mean) - 20, 30), fontFace=1, fontScale=2,
thickness=2, color=tuple(_c * 0.2 for _c in color))
for j in range(pose_in_2d.shape[1]):
p2d = [int(round(c * scale))
for c in pose_in_2d[pose_id, j, :]]
conf = get_conf_thresholded(conf=vis_f[pose_id, j],
thresh_log_conf=thresh_log_conf,
dtype_np=np.float32)
if conf > 0.5:
cv2.circle(
im, (p2d[0], p2d[1]), radius=3, color=color,
thickness=-1)
# jid_ours = JointDenis.to_ours_2d(j)
jid_ours = j
cv2.putText(im, Joint(jid_ours).get_name(),
(p2d[0], p2d[1]-5), 1, 1, color=color,
thickness=1)
center = (scale * np.mean(pose_in_2d[pose_id, :, :], axis=0)) \
.round().astype('i4').tolist()
# center = (scale * pose_in_2d[pose_id, 5, :])\
# .round().astype('i4').tolist()
cv2.putText(im, "p%da%d" % (pose_id, actor_id),
(center[0], center[1]), 1, 2,
[c_ * 1.2 for c_ in color], thickness=2)
# frame_id
cv2.putText(im, "#%d" % frame_id, (20, 30), 1, 2, (255, 255, 255),
thickness=2)
# cv2.imshow("im", im)
p_im = os.path.join(p_labeling, "im_%04d.jpg" % frame_id)
cv2.imwrite(p_im, im)