Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
+173
@@ -0,0 +1,173 @@
|
||||
import argparse
|
||||
import os
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from rdp import rdp
|
||||
|
||||
from utils import hull
|
||||
|
||||
FINGER_NAMES = ["Thumb", "Index", "Middle", "Ring", "Pinky(Little)"]
|
||||
BONE_NAMES = ["PIP", "DIP", "TIP"]
|
||||
|
||||
|
||||
def count_num_in_convex_hull(points_, convex_hull_):
|
||||
'''
|
||||
|
||||
:param points_: points in plane, N*2
|
||||
:param convex_hull_: list of points to construct a convex hull , M*2
|
||||
:return: numbers of points in convex hull
|
||||
'''
|
||||
points = points_.copy()
|
||||
convex_hull = convex_hull_.copy()
|
||||
convex_hull = np.append(convex_hull, [convex_hull[0]], axis=0)
|
||||
|
||||
v = convex_hull[1:] - convex_hull[:-1] # NUM*2
|
||||
v = np.tile(v, (points_.shape[0], 1, 1))
|
||||
|
||||
w = -np.tile(convex_hull[:-1], (points_.shape[0], 1, 1)) + points[:, np.newaxis, :] # N*NUM*2
|
||||
# 2d cross product (w1, w2) x (v1, v2) := w1*v2 - w2*v1
|
||||
cross_product_2d = w[:, :, 0] * v[:, :, 1] - w[:, :, 1] * v[:, :, 0]
|
||||
tmp = (cross_product_2d < 1e-6).all(axis=-1).sum()
|
||||
|
||||
return tmp
|
||||
|
||||
|
||||
def calculate_convex_hull(joint_angles, args):
|
||||
all_del_hulls = []
|
||||
for ID in range(15):
|
||||
print('*' * 40)
|
||||
ja = joint_angles[:, ID]
|
||||
ja_list = [tuple(x) for x in ja.tolist()]
|
||||
|
||||
# original convex hull
|
||||
convex = hull.convex(ja_list)
|
||||
convex = np.array(convex)
|
||||
print("ori_hull.shape=", convex.shape)
|
||||
|
||||
convex_list = convex.tolist()
|
||||
# convex hull simplified by the Ramer-Douglas-Peucker algorithm, a polygon simplification algorithm
|
||||
rdp_convex = rdp(convex_list, epsilon=args.epsilon)
|
||||
rdp_convex = np.array(rdp_convex)
|
||||
print("rdp_hull.shape=", rdp_convex.shape)
|
||||
|
||||
# convex hull further simplified by a greedy algorithm
|
||||
dep_hull = rdp_convex.copy()
|
||||
for i in range(rdp_convex.shape[0]):
|
||||
tmp_index = np.argwhere(dep_hull == rdp_convex[i])[0][0]
|
||||
tmp_num = count_num_in_convex_hull(ja, np.delete(dep_hull, tmp_index, axis=0))
|
||||
tmp_ratio = tmp_num / ja.shape[0]
|
||||
print("tmp_ratio=", tmp_ratio)
|
||||
|
||||
if tmp_ratio > args.ratio:
|
||||
dep_hull = np.delete(dep_hull, tmp_index, axis=0)
|
||||
elif args.ratio - args.delta <= tmp_ratio <= args.ratio:
|
||||
dep_hull = np.delete(dep_hull, tmp_index, axis=0)
|
||||
break
|
||||
else:
|
||||
break
|
||||
print("dep_hull.shape=", dep_hull.shape)
|
||||
all_del_hulls.append(dep_hull)
|
||||
|
||||
if args.visualize:
|
||||
fig = plt.figure()
|
||||
figManager = plt.get_current_fig_manager()
|
||||
figManager.window.showMaximized()
|
||||
ax = fig.add_subplot(111)
|
||||
ax.scatter(ja[:, 0], ja[:, 1], s=0.05, c='r')
|
||||
ax.set_xlabel("flexion")
|
||||
ax.set_ylabel("abduction")
|
||||
ax.set_title('ID:{} Finger:{} Bone:{}'.format(ID, FINGER_NAMES[ID % 5], BONE_NAMES[int(ID / 5)]))
|
||||
|
||||
plt.plot(np.append(convex[:, 0], convex[0, 0]), np.append(convex[:, 1], convex[0, 1]), 'ro--', linewidth=3,
|
||||
label='ori_convex_hull')
|
||||
plt.plot(np.append(rdp_convex[:, 0], rdp_convex[0, 0]), np.append(rdp_convex[:, 1], rdp_convex[0, 1]),
|
||||
'gv--',
|
||||
linewidth=2, label='rdp_convex_hull')
|
||||
plt.plot(np.append(dep_hull[:, 0], dep_hull[0, 0]),
|
||||
np.append(dep_hull[:, 1], dep_hull[0, 1]),
|
||||
'-b*', linewidth=1, label='del_convex_hull')
|
||||
|
||||
ja_min = np.min(ja, axis=0)
|
||||
ja_max = np.max(ja, axis=0)
|
||||
|
||||
plt.gca().add_patch(
|
||||
plt.Rectangle((ja_min[0], ja_min[1]), ja_max[0] - ja_min[0], ja_max[1] - ja_min[1], edgecolor="yellow",
|
||||
fill=False, linewidth=2, label='rectangle'))
|
||||
|
||||
plt.xticks(np.arange(-3, 4, 1))
|
||||
plt.yticks(np.arange(-2, 2, 0.5))
|
||||
plt.legend(title='Convex hull category:')
|
||||
|
||||
plt.show()
|
||||
|
||||
# check if hull_test is in counter-clockwise order
|
||||
# hull_test = convex
|
||||
# for END in range(
|
||||
# hull_test.shape[0]):
|
||||
# fig = plt.figure()
|
||||
# figManager = plt.get_current_fig_manager()
|
||||
# figManager.window.showMaximized()
|
||||
# ax = fig.add_subplot(111)
|
||||
# ax.scatter(ja[:, 0], ja[:, 1], s=0.1, c='r')
|
||||
# ax.set_xlabel("flexion")
|
||||
# ax.set_ylabel("abduction")
|
||||
#
|
||||
# plt.plot(hull_test[:END, 0], hull_test[:END, 1], 'y--', linewidth=1)
|
||||
# plt.xticks(np.arange(-3, 4, 1))
|
||||
# plt.yticks(np.arange(-2, 2, 0.5))
|
||||
# plt.show()
|
||||
|
||||
all_del_hulls = np.array(all_del_hulls, dtype=object)
|
||||
np.save("BMC/CONVEX_HULLS.npy", all_del_hulls)
|
||||
print("all_del_hulls.shape=", all_del_hulls.shape)
|
||||
|
||||
|
||||
def main(args):
|
||||
joint_angles = np.load(os.path.join(args.path, "joint_angles.npy"))
|
||||
print("joint_angles.shape=", joint_angles.shape)
|
||||
calculate_convex_hull(joint_angles, args)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(
|
||||
description='calculate convex hull for joint angles ')
|
||||
|
||||
parser.add_argument(
|
||||
'-p',
|
||||
'--path',
|
||||
default='BMC',
|
||||
type=str,
|
||||
metavar='data_root',
|
||||
help='directory')
|
||||
|
||||
parser.add_argument(
|
||||
'-vis',
|
||||
'--visualize',
|
||||
action='store_true',
|
||||
help='visualize reconstruction result',
|
||||
default=True
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'--epsilon',
|
||||
type=float,
|
||||
default=5e-4,
|
||||
help='epsilon0.'
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'--ratio',
|
||||
type=float,
|
||||
default=0.9995,
|
||||
help='ration0.'
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'--delta',
|
||||
type=float,
|
||||
default=0.0005,
|
||||
help='ration_delta.'
|
||||
)
|
||||
|
||||
main(parser.parse_args())
|
||||
Reference in New Issue
Block a user