def load_images(path_images, n=50):
images = {}
shape_orig = None
logging.info("Reading images...")
path_images_pickle = os.path.join(path_images, 'images_0.pickle')
if os.path.exists(path_images_pickle):
logging.warning("Reading PICKLE from %s" % path_images_pickle)
_gen = (os.path.join(path_images, path_file)
for path_file in os.listdir(path_images)
if path_file.endswith('pickle')
and path_file.startswith('images_'))
pool = multiprocessing.Pool(4)
processes = []
for path_file in _gen:
processes.append(
pool.apply_async(func=_load_chunk, args=[path_file]))
pool.close()
pool.join()
for process in processes:
pickled = process.get()
if isinstance(pickled, tuple) and len(pickled) == 2:
images.update(pickled[0])
shape_orig = pickled[1]
else:
images.update(pickled)
assert shape_orig is not None, \
"Shape orig must be in one of the files"
else:
# load images
_gen = (f for f in os.listdir(path_images) if f.endswith('jpg'))
for f in _gen:
frame_id = int(os.path.splitext(f)[0].split('_')[1])
images[frame_id], shape_orig = \
load_image(os.path.join(path_images, f))
del frame_id, f, _gen
#
# save pickles
#
# sorted list of frame_ids
keys = sorted(list(images.keys()))
for id_pickle, i in enumerate(range(0, len(keys), n)):
range_end = min(i + n, len(keys))
_keys = keys[i:range_end]
_images = {frame_id: images[frame_id] for frame_id in _keys}
path_file = os.path.join(path_images,
"images_%d.pickle" % id_pickle)
with open(path_file, 'wb') as file_out:
to_dump = (_images, shape_orig) \
if not i \
else _images
pickle.dump(to_dump, file_out, protocol=-1)
lg.debug("Saved to %s" % path_file)
# pickle.dump((images, shape_orig), open(path_images_pickle, 'wb'))
# logging.info("Saved PICKLE to %s" % path_images_pickle)
logging.info("Finished loading images...")
return images, shape_orig
def get_grid_shapely(occup, res_orig):
assert res_orig.shape == (4, ), "Expected xyz-theta resolution"
room = occup.room
assert room.shape == (3, 2), "Assumed 3D room shape"
res_target = (occup.resolution[0], occup.resolution[2])
start_x = room[0, 0] - res_orig[0] / 2.
end_x = room[0, 1] + res_orig[0] / 2.
span_x = (end_x - start_x)
diff_x = span_x - int(span_x / res_target[0]) * res_target[0]
start_x += diff_x / 2.
end_x -= res_target[0] # last square ends at end_x + res_target
start_y = room[2, 0] - res_orig[2] / 2.
end_y = room[2, 1] + res_orig[2] / 2.
span_y = (end_y - start_y)
diff_y = span_y - int(span_y / res_target[1]) * res_target[1]
start_y += diff_y / 2.
end_y -= res_target[1] # last square ends at end_y + res_target
grid_size = occup.get_grid_size()
polys = []
lg.debug("start_x: %s, end_x: %s, res_target: %s" %
(start_x, end_x, res_target))
for ix, x in enumerate(np.arange(start_x, end_x, res_target[0])):
if ix >= grid_size[0]:
continue
for iy, y in enumerate(np.arange(start_y, end_y, res_target[1])):
if iy >= grid_size[2]:
continue
box = geom.box(x, y, x + res_target[0], y + res_target[1])
polys.append(GridPoly(box, area=box.area, xy=(ix, iy)))
#assert iy == grid_size[2] - 1, "No: %s %s" % (iy, grid_size)
#assert ix == grid_size[0] - 1, "No: %s %s %s" % (ix, x, grid_size)
return polys
def add_frame(self, frame_id, n_vars):
for pose_id in range(n_vars):
try:
self._constr[frame_id].append(len(self._vars))
except KeyError:
self._constr[frame_id] = [len(self._vars)]
self._vars[(frame_id, pose_id)] = len(self._vars)
lg.debug("vars: %s" % self._vars)
def plot_charness(scenes, d_dir, ch_thresh=0.5):
fig = plt.figure()
ax = fig.add_subplot(111)
charnesses = [s.charness for s in scenes if s.charness > ch_thresh]
ax.hist(charnesses, bins=30, cumulative=True)
plt.title("Cumulative charness above %g" % ch_thresh)
p = os.path.join(d_dir, 'charnesses.png')
plt.savefig(p, dpi=200)
lg.debug("Saved charnesses to %s" % p)
plt.close()
def select_gaps(argv):
pjoin = os.path.join
parser = argparse.ArgumentParser("Select gaps")
parser = parser_video(parser)
parser.add_argument('--select-gaps', action='store_true')
parser.add_argument("-s",
"--d-scenelets",
dest='s',
type=argparse_check_exists,
help="Folder containing original PiGraphs scenelets")
args = parser.parse_args(argv)
d = os.path.join(args.video, args.d)
assert os.path.exists(d), "does not exist: %s" % d
if os.path.isdir(args.video):
if args.video.endswith(os.sep):
args.video = args.video[:-1]
name_query = args.video.split(os.sep)[-1]
assert len(name_query), args.video.split(os.sep)
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 = []
for p in sorted(os.listdir(d)):
ch = float(p.split('charness')[1][1:])
d_time = pjoin(d, p)
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, :]
depth = np.linalg.norm(pos_3d)
depths.append(depth)
depths_times_charnesses.append(
DepthTimeCharness(depth=depth, frame_id=frame_id, charness=ch))
hist, bin_edges = np.histogram(depths, bins=5)
lg.debug("hist: %s" % hist)
lg.debug("edges: %s" % bin_edges)
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 test(py_p, py_poly):
np.set_printoptions(suppress=True)
with open('/tmp/test_p_poly_dist.json', 'r') as fin:
jdata = json.load(fin)
wrapped = []
for row in py_poly:
wrapped.append(np.concatenate((row, row[0:1, :])))
wrapped = np.array(wrapped, dtype=np.float64)
lg.debug("wrapped:\n%s" % wrapped)
graph = tf.Graph()
with graph.as_default(), tf.device('/gpu:0'):
p = tf.Variable(initial_value=py_p,
dtype=tf.float64,
trainable=False)
poly = tf.Variable(initial_value=wrapped,
dtype=tf.float64,
trainable=False)
# d, g, sel = PointPolyDistanceEstimator.point_poly_distance(p, poly)
d = PointPolyDistanceEstimator.point_poly_distance(p, poly)
with tf.Session(graph=graph) as session:
# init
session.run(tf.global_variables_initializer())
o_d = session.run(d)
# o_d, o_g, o_sel = session.run([d, g, sel])
# lg.debug("gradients: %s" % o_g)
# lg.debug("sel: %s" % o_sel)
lg.debug("o_d (%s):\n%s" % (repr(o_d.shape), o_d))
assert np.allclose(o_d,
np.squeeze(np.array(jdata['d']), axis=-1), atol=0.001), \
"no:\n%s\n%s\n%s\n%s" \
% ('o_d: ', o_d,
'gt d: ', np.squeeze(np.array(jdata['d']), axis=-1))
def main(argv):
np.set_printoptions(suppress=True, linewidth=200)
pjoin = os.path.join
parser = argparse.ArgumentParser("matcher")
parser.add_argument("d_scenelets",
type=argparse_check_exists,
help="Folder containing scenelets")
parser.add_argument("video", type=argparse_check_exists, help="Input path")
parser.add_argument("--gap-size-limit",
type=int,
help="Smallest gap size to still explain")
args = parser.parse_args(argv)
d_query = os.path.dirname(args.video)
# 2d keypoint rescale
p_im = pjoin(d_query, 'origjpg', 'color_00100.jpg')
im_ = cv2.imread(p_im)
shape_orig = im_.shape
scale_2d = shape_orig[0] / float(INPUT_SIZE)
query = Scenelet.load(args.video, no_obj=True)
tr_ground = np.array(query.aux_info['ground'], dtype=np.float32)
print("tr: %s" % tr_ground)
name_query = os.path.basename(args.video).split('_')[1]
query_2d = Scenelet.load(pjoin(d_query,
"skel_%s_2d_00.json" % name_query)).skeleton
p_intr = pjoin(d_query, 'intrinsics.json')
intr = np.array(json.load(open(p_intr, 'r')), dtype=np.float32)
lg.debug("intr: %s" % intr)
gaps = find_gaps(query.skeleton, min_pad=1)
p_scenelets_pickle = pjoin(args.d_scenelets, 'match_gap_scenelets.pickle')
if os.path.exists(p_scenelets_pickle):
scenelets = pickle_load(open(p_scenelets_pickle, 'rb'))
else:
scenelets = read_scenelets(args.d_scenelets, limit=0)
pickle.dump(scenelets, open(p_scenelets_pickle, 'wb'))
p_out_sclts = pjoin(d_query, 'fill')
if os.path.exists(p_out_sclts):
shutil.rmtree(p_out_sclts)
os.makedirs(p_out_sclts)
times = []
for gap_id, gap in enumerate(gaps):
lg.debug("gap is %s" % repr(gap))
if gap[1] - gap[0] < args.gap_size_limit:
continue
with Timer("gap %d" % gap_id) as timer:
chosen = []
for sc_id, sclt in enumerate(scenelets):
lg.info("scenelet %d / %d" % (sc_id, len(scenelets)))
sclt = scenelets[sc_id]
ground_obj = next(ob for ob in sclt.objects.values()
if ob.label == 'floor')
ground_part = ground_obj.get_part_by_name('floor')
lg.debug("floor: %s" % ground_part)
ground_transform = ground_part.obb.as_transform()
lg.debug("floor: %s" % ground_transform)
# sys.exit(0)
# chosen.extend(
out_sclts = match(query,
d_query,
query_2d,
sclt,
intr,
gap,
tr_ground,
scale=scale_2d)
if not len(out_sclts):
continue
# pick best from scene
chosen.append(
[out_sclts[0][i]
for i in range(len(out_sclts[0]))] + [sc_id])
# break
# os.system("rm %s/skel_%s_fill_%03d_%03d__*.json"
# % (p_out_sclts, name_query, gap[0], gap[1]))
chosen = sorted(chosen, key=lambda score_sclt: score_sclt[0])
for sid, (score, out_sclt, sc_id) in enumerate(chosen):
p_out = pjoin(
p_out_sclts, "skel_%s_fill_%03d_%03d__%02d.json" %
(name_query, gap[0], gap[1], sid))
out_sclt.save(p_out)
if sid > 5:
break
times.append(timer.get_elapsed_ms())
lg.info("mean time per gap: %s" % np.mean(times))
def match(query_full,
d_query,
query_2d_full,
scene,
intr,
gap,
tr_ground,
scale,
thresh_log_conf=7.5,
w_3d=0.01,
fps=3,
step_samples=100):
with_y = False # optimize for y as well
np.set_printoptions(suppress=True, linewidth=220)
pjoin = os.path.join
len_gap = gap[1] - gap[0] + 1
query, q_v = get_partial_scenelet(query_full,
start=gap[0],
end=gap[1] + 1,
fps=1)
q_v_sum = np.sum(q_v)
q_v_sum_inv = np.float32(1. / q_v_sum)
# lg.debug("q_v_sum: %s/%s" % (q_v_sum, q_v.size))
# scene_min_y = scene.skeleton.get_min_y(tr_ground)
# lg.debug("scene_min_y: %s" % repr(scene_min_y))
mid_frames = range(len_gap * fps,
scene.skeleton.poses.shape[0] - len_gap * fps,
step_samples)
if not len(mid_frames):
return []
scenelets, sc_v = (np.array(e) for e in zip(*[
get_partial_scenelet(
scene, mid_frame_id=mid_frame_id, n_frames=len_gap, fps=fps)
for mid_frame_id in mid_frames
]))
# for i, (scenelet, sc_v_) in enumerate(zip(scenelets, sc_v)):
# mn = np.min(scenelet[sc_v_.astype('b1'), 1, :])
# scenelets[i, :, 1, :] -= mn
# mn = np.min(scenelets[i, sc_v_.astype('b1'), 1, :])
# scenelets = np.array(scenelets, dtype=np.float32)
# sc_v = np.array(sc_v, dtype=np.int32)
# print("sc_v: %s" % sc_v)
# print("q_v: %s" % q_v)
lg.debug("have %d/%d 3D poses in scenelet, and %d/%d in query" %
(np.sum(sc_v), sc_v.shape[0], np.sum(q_v), q_v.shape[0]))
query_2d = np.zeros((len_gap, 2, 16), dtype=np.float32)
conf_2d = np.zeros((len_gap, 1, 16), dtype=np.float32)
for lin_id, frame_id in enumerate(range(gap[0], gap[1] + 1)):
if query_2d_full.has_pose(frame_id):
query_2d[lin_id, :, :] = query_2d_full.get_pose(frame_id)[:2, :]
# else:
# lg.warning("Query2d_full does not have pose at %d?" % frame_id)
# im = im_.copy()
if query_2d_full.has_confidence(frame_id):
# print("showing %s" % frame_id)
for joint, conf in query_2d_full._confidence[frame_id].items():
log_conf = abs(np.log(conf)) if conf >= 0. else 0.
# print("conf: %g, log_conf: %g" % (conf, log_conf))
# if log_conf <= thresh_log_conf:
# p2d = scale * query_2d_full.get_joint_3d(joint,
# frame_id=frame_id)
# p2d = (int(round(p2d[0])), int(round(p2d[1])))
# cv2.circle(im, center=p2d,
# radius=int(round(3)),
# color=(1., 1., 1., 0.5), thickness=1)
conf_2d[lin_id, 0, joint] = max(
0., (thresh_log_conf - log_conf) / thresh_log_conf)
# cv2.imshow('im', im)
# cv2.waitKey(100)
# while cv2.waitKey() != 27: pass
conf_2d /= np.max(conf_2d)
# scale from Denis' scale to current image size
query_2d *= scale
# move to normalized camera coordinates
query_2d -= intr[:2, 2:3]
query_2d[:, 0, :] /= intr[0, 0]
query_2d[:, 1, :] /= intr[1, 1]
#
# initialize translation
#
# centroid of query poses
c3d = np.mean(query[q_v.astype('b1'), :, :], axis=(0, 2))
# estimate scenelet centroids
sclt_means = np.array([
np.mean(scenelets[i, sc_v[i, ...].astype('b1'), ...], axis=(0, 2))
for i in range(scenelets.shape[0])
],
dtype=np.float32)
# don't change height
sclt_means[:, 1] = 0
scenelets -= sclt_means[:, None, :, None]
lg.debug("means: %s" % repr(sclt_means.shape))
if with_y:
np_translation = np.array([c3d for i in range(scenelets.shape[0])],
dtype=np.float32)
else:
np_translation = np.array(
[c3d[[0, 2]] for i in range(scenelets.shape[0])], dtype=np.float32)
np_rotation = np.array(
[np.pi * (i % 2) for i in range(scenelets.shape[0])],
dtype=np.float32)[:, None]
n_cands = np_translation.shape[0]
graph = tf.Graph()
with graph.as_default(), tf.device('/gpu:0'):
# 3D translation
translation_ = tf.Variable(initial_value=np_translation,
name='translation',
dtype=tf.float32)
t_y = tf.fill(dims=(n_cands, ),
value=(tr_ground[1, 3]).astype(np.float32))
# t_y = tf.fill(dims=(n_cands,), value=np.float32(0.))
lg.debug("t_y: %s" % t_y)
if with_y:
translation = translation_
else:
translation = tf.concat(
(translation_[:, 0:1], t_y[:, None], translation_[:, 1:2]),
axis=1)
lg.debug("translation: %s" % translation)
# 3D rotation (Euler XYZ)
rotation = tf.Variable(np_rotation, name='rotation', dtype=tf.float32)
# lg.debug("rotation: %s" % rotation)
w = tf.Variable(conf_2d, trainable=False, name='w', dtype=tf.float32)
pos_3d_in = tf.Variable(query,
trainable=False,
name='pos_3d_in',
dtype=tf.float32)
# pos_3d_in = tf.constant(query, name='pos_3d_in', dtype=tf.float32)
pos_2d_in = tf.Variable(query_2d,
trainable=False,
name='pos_2d_in',
dtype=tf.float32)
# pos_2d_in = tf.constant(query_2d, name='pos_2d_in',
# dtype=tf.float32)
pos_3d_sclt = tf.Variable(scenelets,
trainable=False,
name='pos_3d_sclt',
dtype=tf.float32)
# print("pos_3d_sclt: %s" % pos_3d_sclt)
# rotation around y
my_zeros = tf.zeros((n_cands, 1), dtype=tf.float32, name='my_zeros')
# tf.add_to_collection('to_init', my_zeros)
my_ones = tf.ones((n_cands, 1))
# tf.add_to_collection('to_init', my_ones)
c = tf.cos(rotation, 'cos')
# tf.add_to_collection('to_init', c)
s = tf.sin(rotation, 'sin')
# t0 = tf.concat([c, my_zeros, -s], axis=1)
# t1 = tf.concat([my_zeros, my_ones, my_zeros], axis=1)
# t2 = tf.concat([s, my_zeros, c], axis=1)
# transform = tf.stack([t0, t1, t2], axis=2, name="transform")
# print("t: %s" % transform)
transform = tf.concat(
[c, my_zeros, -s, my_zeros, my_ones, my_zeros, s, my_zeros, c],
axis=1)
transform = tf.reshape(transform, ((-1, 3, 3)), name='transform')
print("t2: %s" % transform)
# lg.debug("transform: %s" % transform)
# transform to 3d
# pos_3d = tf.matmul(transform, pos_3d_sclt) \
# + tf.tile(tf.expand_dims(translation, 2),
# [1, 1, int(pos_3d_in.shape[2])])
# pos_3d = tf.einsum("bjk,bcjd->bcjd", transform, pos_3d_sclt)
shp = pos_3d_sclt.get_shape().as_list()
transform_tiled = tf.tile(transform[:, None, :, :, None],
(1, shp[1], 1, 1, shp[3]))
# print("transform_tiled: %s" % transform_tiled)
pos_3d = tf.einsum("abijd,abjd->abid", transform_tiled, pos_3d_sclt)
# print("pos_3d: %s" % pos_3d)
pos_3d += translation[:, None, :, None]
#pos_3d = pos_3d_sclt
# print("pos_3d: %s" % pos_3d)
# perspective divide
# pos_2d = tf.divide(
# tf.slice(pos_3d, [0, 0, 0], [n_cands, 2, -1]),
# tf.slice(pos_3d, [0, 2, 0], [n_cands, 1, -1]))
pos_2d = tf.divide(pos_3d[:, :, :2, :], pos_3d[:, :, 2:3, :])
# print("pos_2d: %s" % pos_2d)
diff = pos_2d - pos_2d_in
# mask loss by 2d key-point visibility
# print("w: %s" % w)
# w_sum = tf.reduce_sum()
masked = tf.multiply(diff, w)
# print(masked)
# loss_reproj = tf.nn.l2_loss(masked)
# loss_reproj = tf.reduce_sum(tf.square(masked[:, :, 0, :])
# + tf.square(masked[:, :, 1, :]),
# axis=[1, 2])
masked_sqr = tf.square(masked[:, :, 0, :]) \
+ tf.square(masked[:, :, 1, :])
loss_reproj = tf.reduce_sum(masked_sqr, axis=[1, 2])
# lg.debug("loss_reproj: %s" % loss_reproj)
# distance from existing 3D skeletons
d_3d = q_v_sum_inv * tf.multiply(pos_3d - query[None, ...],
q_v[None, :, None, None],
name='diff_3d')
# print(d_3d)
loss_3d = w_3d * tf.reduce_sum(tf.square(d_3d[:, :, 0, :]) + tf.square(
d_3d[:, :, 1, :]) + tf.square(d_3d[:, :, 2, :]),
axis=[1, 2],
name='loss_3d_each')
# print(loss_3d)
loss = tf.reduce_sum(loss_reproj) + tf.reduce_sum(loss_3d)
# optimize
optimizer = ScipyOptimizerInterface(loss,
var_list=[translation_, rotation],
options={'gtol': 1e-12})
with Timer('solve', verbose=True) as t:
with tf.Session(graph=graph) as session:
session.run(tf.global_variables_initializer())
optimizer.minimize(session)
o_pos_3d, o_pos_2d, o_masked, o_t, o_r, o_w, o_d_3d, \
o_loss_reproj, o_loss_3d, o_transform, o_translation = \
session.run([
pos_3d, pos_2d, masked, translation, rotation, w,
d_3d, loss_reproj, loss_3d, transform, translation])
o_masked_sqr = session.run(masked_sqr)
# o_t, o_r = session.run([translation, rotation])
# print("pos_3d: %s" % o_pos_3d)
# print("pos_2d: %s" % o_pos_2d)
# print("o_loss_reproj: %s, o_loss_3d: %s" % (o_loss_reproj, o_loss_3d))
# print("t: %s" % o_t)
# print("r: %s" % o_r)
chosen = sorted((i for i in range(o_loss_reproj.shape[0])),
key=lambda i2: o_loss_reproj[i2] + o_loss_3d[i2])
lg.info("Best candidate is %d with error %g + %g" %
(chosen[0], o_loss_reproj[chosen[0]], o_loss_3d[chosen[0]]))
# print("masked: %s" % o_masked)
# opp = np.zeros_like(o_pos_3d)
# for i in range(o_pos_3d.shape[0]):
# for j in range(o_pos_3d.shape[1]):
# for k in range(16):
# opp[i, j, :2, k] = o_pos_3d[i, j, :2, k] / o_pos_3d[i, j, 2:3, k]
# # opp[i, j, 0, k] *= intr[0, 0]
# # opp[i, j, 1, k] *= intr[1, 1]
# # opp[i, j, :2, k] *= intr[1, 1]
# a = o_pos_2d[i, j, :, k]
# b = opp[i, j, :2, k]
# if not np.allclose(a, b):
# print("diff: %s, %s" % (a, b))
o_pos_2d[:, :, 0, :] *= intr[0, 0]
o_pos_2d[:, :, 1, :] *= intr[1, 1]
o_pos_2d += intr[:2, 2:3]
# for cand_id in range(o_pos_2d.shape[0]):
if False:
# return
# print("w: %s" % o_w)
# print("conf_2d: %s" % conf_2d)
# lg.debug("query_2d[0, 0, ...]: %s" % query_2d[0, 0, ...])
query_2d[:, 0, :] *= intr[0, 0]
query_2d[:, 1, :] *= intr[1, 1]
# lg.debug("query_2d[0, 0, ...]: %s" % query_2d[0, 0, ...])
query_2d += intr[:2, 2:3]
# lg.debug("query_2d[0, 0, ...]: %s" % query_2d[0, 0, ...])
ims = {}
for cand_id in chosen[:5]:
lg.debug("starting %s" % cand_id)
pos_ = o_pos_2d[cand_id, ...]
for lin_id in range(pos_.shape[0]):
frame_id = gap[0] + lin_id
try:
im = ims[frame_id].copy()
except KeyError:
p_im = pjoin(d_query, 'origjpg',
"color_%05d.jpg" % frame_id)
ims[frame_id] = cv2.imread(p_im)
im = ims[frame_id].copy()
# im = im_.copy()
for jid in range(pos_.shape[-1]):
xy2 = int(round(query_2d[lin_id, 0, jid])), \
int(round(query_2d[lin_id, 1, jid]))
# print("printing %s" % repr(xy))
cv2.circle(im,
center=xy2,
radius=5,
color=(10., 200., 10.),
thickness=-1)
if o_masked[cand_id, lin_id, 0, jid] > 0 \
or o_w[lin_id, 0, jid] > 0:
xy = int(round(pos_[lin_id, 0, jid])), \
int(round(pos_[lin_id, 1, jid]))
# print("printing %s" % repr(xy))
cv2.circle(im,
center=xy,
radius=3,
color=(200., 10., 10.),
thickness=-1)
cv2.putText(im,
"d2d: %g" %
o_masked_sqr[cand_id, lin_id, jid],
org=((xy2[0] - xy[0]) // 2 + xy[0],
(xy2[1] - xy[1]) // 2 + xy[1]),
fontFace=1,
fontScale=1,
color=(0., 0., 0.))
cv2.line(im, xy, xy2, color=(0., 0., 0.))
d3d = o_d_3d[cand_id, lin_id, :, jid]
d3d_norm = np.linalg.norm(d3d)
if d3d_norm > 0.:
cv2.putText(
im,
"%g" % d3d_norm,
org=((xy2[0] - xy[0]) // 2 + xy[0] + 10,
(xy2[1] - xy[1]) // 2 + xy[1]),
fontFace=1,
fontScale=1,
color=(0., 0., 255.))
cv2.putText(im,
text="%d::%02d" % (cand_id, lin_id),
org=(40, 80),
fontFace=1,
fontScale=2,
color=(255., 255., 255.))
# pos_2d_ = np.matmul(intr, pos_[lin_id, :2, :] / pos_[lin_id, 2:3, :])
# for p2d in pos_2d_
cv2.imshow('im', im)
cv2.waitKey()
break
while cv2.waitKey() != 27:
pass
out_scenelets = []
for cand_id in chosen[:1]:
lg.debug("score of %d is %g + %g = %g" %
(cand_id, o_loss_reproj[cand_id], o_loss_3d[cand_id],
o_loss_reproj[cand_id] + o_loss_3d[cand_id]))
scenelet = Scenelet()
rate = query_full.skeleton.get_rate()
prev_time = None
for lin_id, frame_id in enumerate(range(gap[0], gap[1] + 1)):
time_ = query_full.get_time(frame_id)
if lin_id and rate is None:
rate = time_ - prev_time
if time_ == frame_id:
time_ = prev_time + rate
scenelet.skeleton.set_pose(frame_id=frame_id,
pose=o_pos_3d[cand_id, lin_id, :, :],
time=time_)
prev_time = time_
tr = np.concatenate((np.concatenate(
(o_transform[cand_id, ...], o_translation[cand_id, None, :].T),
axis=1), [[0., 0., 0., 1.]]),
axis=0)
tr_m = np.concatenate(
(np.concatenate((np.identity(3), -sclt_means[cand_id, None, :].T),
axis=1), [[0., 0., 0., 1.]]),
axis=0)
tr = np.matmul(tr, tr_m)
for oid, ob in scene.objects.items():
if ob.label in ('wall', 'floor'):
continue
ob2 = copy.deepcopy(ob)
ob2.apply_transform(tr)
scenelet.add_object(obj_id=oid, scene_obj=ob2, clone=False)
scenelet.name_scene = scene.name_scene
out_scenelets.append((o_loss_reproj[cand_id], scenelet))
return out_scenelets
def show_images(images, data, thresh_log_conf=Conf.get().path.thresh_log_conf):
_confs = []
for frame_str in sorted(data):
try:
frame_id = int(frame_str.split('_')[1])
except ValueError:
print("skipping key %s" % frame_id)
continue
pose_in = np.asarray(data[frame_str][u'centered_3d'])
pose_in_2d = 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'])
# pose_id = pose_ids[frame_id]
im = cv2.cvtColor(images[frame_id], cv2.COLOR_RGB2BGR)
for i in range(pose_in.shape[0]):
c = (.7, .2, .7, 1.)
# if i == pose_id:
# c = (0., 1., 0., 1.)
_confs.append(vis_f[i:i+1, :])
color = tuple(int(c_ * 255) for c_ in c[:3])
threshed = get_conf_thresholded(vis_f[i:i+1, :], dtype_np=np.float32,
thresh_log_conf=thresh_log_conf)
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[i, :, 1])
cv2.putText(im, "%.2f" % (avg_vis / threshed.shape[1]),
(int(p2d_mean) - 20, 40), 1, 2, thickness=2,
color=(200, 200, 200))
for j in range(pose_in_2d.shape[1]):
p2d = pose_in_2d[i, j, :]
conf = get_conf_thresholded(conf=vis_f[i, j],
thresh_log_conf=thresh_log_conf,
dtype_np=np.float32)
if conf > 0.5:
cv2.circle(
im, (p2d[1], p2d[0]), radius=3, color=color, thickness=-1)
cv2.putText(im, ("%.2f" % conf)[1:],
(p2d[1], p2d[0]), 1, 1, color=color,
thickness=2)
# if conf > 0.:
# cv2.putText(im, "%.2f" % avg_vis,
# (p2d[1], p2d[0]), 1, 2, color=color)
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)
cv2.imshow("im", im)
cv2.imwrite("/tmp/im_%04d.jpg" % frame_id, im)
cv2.waitKey(100)
# _confs = np.array(_confs).flatten()
# _confs[_confs < 0.] = 0.
# _confs = np.sort(_confs)
# tr = 1. / (1. + np.exp(-5000. * _confs + 5))
# print("here: %g" % np.exp(0.001))
# plt.figure()
# # plt.hist(_confs, bins=100)
# sel = tr > 0.5
# tr0 = tr[sel]
# plt.scatter(_confs[sel], tr0, facecolor=None, edgecolor='g')
# sel1 = tr < 0.5
# plt.scatter(_confs[sel1], tr[sel1], facecolor=None, edgecolor='r')
# plt.plot([0., 0.003], [0.5, 0.5], 'k')
# plt.plot([0.001, 0.001], [0., 1.], 'k')
# # plt.plot([0, len(_confs)], [np.log10(np.exp(-7.5)),
# # np.log10(np.exp(-7.5))])
# # plt.plot([0, len(_confs)], [np.exp(0.001) - 1., np.exp(0.001) - 1.])
# # tr = 1. / (1 + np.log10(_confs))
# # print("showing %s" % _confs)
# plt.title('Transformed confidence: 1/(1 + exp(-5000x + 5))')
# plt.xlim(-0.0001, 0.003)
# plt.show()
# plt.close()
sys.exit(0)
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 show_output(tf_vars, deb_oo, deb_jo, d_query, session, smooth_pairs,
d_postfix, f_postfix, um):
colors = stealth_colors
p_deb = os.path.join(d_query, 'debug_isec' + d_postfix)
if not os.path.exists(p_deb):
os.makedirs(p_deb)
# os.system("rm %s/*.png" % p_deb)
# os.system("rm %s/*.svg" % p_deb)
# else:
# assert np.allclose(tf_vars.oo_mask_same.eval(),
# tf_vars.oo_mask_same_2.eval())
# assert np.allclose(tf_vars.oo_mask_cat.eval(),
# tf_vars.oo_mask_cat_2.eval())
# assert np.allclose(tf_vars.oo_mask_interacting_2,
# tf_vars._oo_mask_interacting.eval())
# assert np.isclose(tf_vars._oo_mask_interacting_sum_inv.eval(),
# tf_vars.oo_mask_interacting_sum_inv_2.eval())
obj_vxs_t = tf_vars.obj_2d_vertices_transformed
o_obj_vxs_t, o_joints, o_smooth_pairs, distances, o_mgrid_vxs_t = \
session.run([obj_vxs_t, deb_jo['joints'], smooth_pairs,
deb_jo['d'], tf_vars._obj_2d_mgrid_vertices_transformed])
o_polys, o_poly_indices = session.run(
[tf_vars.obj_2d_polys, tf_vars._obj_2d_poly_transform_indices])
o_oo_mask, o_d_oo = session.run([deb_oo['oo_mask'], deb_oo['d_oo']])
py_cat_ids = np.squeeze(tf_vars.cat_ids_polys.eval(), axis=0)
lg.debug("distances: %s" % repr(distances.shape))
lg.debug("obj_vxs_t: %s" % repr(o_obj_vxs_t.shape))
lg.debug("joints: %s" % repr(o_joints.shape))
mn1 = np.min(o_obj_vxs_t, axis=0)[[0, 2]]
mx1 = np.max(o_obj_vxs_t, axis=0)[[0, 2]]
mn2 = np.min(o_smooth_pairs, axis=0)
mn2 = np.minimum(mn2[:3], mn2[3:])[[0, 2]]
mx2 = np.max(o_smooth_pairs, axis=0)
mx2 = np.maximum(mx2[:3], mx2[3:])[[0, 2]]
mn = np.minimum(mn1, mn2)
mx = np.maximum(mx1, mx2)
assert o_joints.shape[0] // 5 == distances.shape[0]
o_joints = o_joints.reshape(5, -1, 3)[0, ...]
assert o_polys.shape[0] == distances.shape[1]
fig = plt.figure()
ax0 = fig.add_subplot(111, aspect='equal')
# for pair in o_smooth_pairs:
# ax0.plot([pair[0], pair[3]], [pair[2], pair[5]], 'k--')
# for id_pnt in range(1, o_joints.shape[0]):
# pnt0 = o_joints[id_pnt, :]
o_joints_ordered = np.asarray([
e[1]
for e in sorted([(um.pids_2_scenes[pid].frame_id, o_joints[pid, ...])
for pid in range(o_joints.shape[0])],
key=lambda e: e[0])
])
assert o_joints_ordered.ndim == 2 and o_joints_ordered.shape[1] == 3
ax0.plot(o_joints_ordered[:, 0], o_joints_ordered[:, 2], 'kx--')
for id_poly in range(distances.shape[1]):
idx_t = o_poly_indices[id_poly, 0]
color = tuple(c / 255. for c in colors[(idx_t + 1) % len(colors)])
d_sum = 0.
# poly = np.concatenate((
# o_obj_vxs_t[4*id_poly:4*(id_poly+1), :],
# o_obj_vxs_t[4*id_poly:4*id_poly+1, :]))
# ax0.plot(o_polys[id_poly, :, 0], o_polys[id_poly, :, 2], color=color)
shapely_poly = geom.asPolygon(o_polys[id_poly, :, [0, 2]].T)
patch = PolygonPatch(shapely_poly,
facecolor=color,
edgecolor=color,
alpha=0.25)
ax0.add_artist(patch)
cat = next(cat for cat in CATEGORIES
if CATEGORIES[cat] == py_cat_ids[id_poly])
xy = (shapely_poly.centroid.xy[0][0] - 0.1,
shapely_poly.centroid.xy[1][0])
ax0.annotate(cat, xy=xy, color=color, fontsize=6)
for id_pnt in range(distances.shape[0]):
d_ = distances[id_pnt, id_poly]
if d_ < 0.:
pnt = o_joints[id_pnt, :]
ax0.scatter(pnt[0],
pnt[2],
s=(5 * (1 + d_))**2,
color=color,
zorder=5)
d_sum += distances[id_pnt, id_poly]
ax0.annotate("%.2f" % d_,
xy=(np.random.rand() * 0.1 + pnt[0] - 0.05,
np.random.rand() * 0.1 + pnt[2] - 0.05),
fontsize=4)
for id_pnt in range(o_d_oo.shape[0]):
d_ = o_d_oo[id_pnt, id_poly]
if d_ < 0.:
pnt = o_mgrid_vxs_t[id_pnt, :]
ax0.scatter(pnt[0],
pnt[2],
s=(7 * (1 + d_))**2,
edgecolor=color,
zorder=5,
color=(1., 1., 1., 0.))
ax0.annotate("%.2f" % d_,
xy=(np.random.rand() * 0.1 + pnt[0] - 0.05,
np.random.rand() * 0.1 + pnt[2] - 0.05),
fontsize=8,
color='r',
zorder=6)
# fig.suptitle("d_sum: %s" % d_sum)
p_out = os.path.join(p_deb, "op_%s.png" % f_postfix)
plt.draw()
ax0.set_xlim(mn[0] - 0.2, mx[0] + 0.2)
ax0.set_ylim(mn[1] - 0.2, mx[1] + 0.2)
# plt.show()
try:
fig.savefig(p_out, dpi=300)
lg.debug("saved to %s" % p_out)
except PermissionError as e:
lg.error("Could not save %s" % e)
plt.close(fig)
def _load_chunk(path_file):
lg.debug("Opening %s" % path_file)
with open(path_file, 'rb') as fil:
return pickle.load(fil)
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 identify_actors(data):
m = Model('Stealth actors')
problem = ActorProblem()
objective = None
prev_pose_in_2d = None
prev_frame_id = None
for frame_str in sorted(data):
try:
frame_id = int(frame_str.split('_')[1])
except ValueError:
print("skipping key %s" % frame_id)
continue
pose_in = np.array(data[frame_str][u'centered_3d'])
pose_in_2d = np.array(data[frame_str][u'pose_2d'])
visible = np.array(data[frame_str][u'visible'])
assert pose_in_2d.ndim == 3, "no: %s" % repr(pose_in_2d.shape)
problem.add_frame(frame_id, pose_in_2d.shape[0])
if prev_pose_in_2d is not None:
for prev_pose_id in range(prev_pose_in_2d.shape[0]):
prev_pose = prev_pose_in_2d[prev_pose_id, :, :]
for pose_id in range(pose_in_2d.shape[0]):
pose = pose_in_2d[pose_id, :, :]
dist = prev_pose - pose
lg.debug("dist: %s" % repr(dist.shape))
cost = np.sum(np.linalg.norm(dist, axis=1), axis=0)
lg.debug("cost: %s" % cost)
problem.add_cost(prev_frame_id, prev_pose_id, frame_id, pose_id, cost)
prev_pose_in_2d = pose_in_2d
prev_frame_id = frame_id
gb_vars = m.addVars(problem.get_n_vars(), vtype=GRB.BINARY)
for (lin_id0, lin_id1), cost in problem._pw.items():
# lin_id0 = problem.get_lin_id(prev_frame_id, prev_pose_id)
# lin_id1 = problem.get_lin_id(frame_id, pose_id)
objective += gb_vars[lin_id0] * gb_vars[lin_id1] * cost
for frame_id, lin_ids in problem._constr.items():
constr = None
for lin_id in lin_ids:
if constr is None:
constr = gb_vars[lin_id]
else:
constr += gb_vars[lin_id]
m.addConstr(constr == 1)
m.setObjective(objective, GRB.MINIMIZE)
# m.solver.callSolver(m)
m.optimize()
pose_ids = dict()
for lin_id, v in enumerate(m.getVars()):
print(v.varName, v.x)
if v.x > 0.5:
frame_id, pose_id = problem.get_frame_id_pose_id(lin_id)
assert frame_id not in pose_ids, "no"
pose_ids[frame_id] = pose_id
# if we have more, pick the first...
# if len(pose_in.shape) > 2:
# pose_in = pose_in[0, :, :]
# pose_in_2d = pose_in_2d[0, :, :]
# visible = visible[0]
return pose_ids
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 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