295 lines
12 KiB
Python
295 lines
12 KiB
Python
|
from typing import Any
|
|||
|
|
import numpy as np
|
|||
|
|
import cv2
|
|||
|
|
|
|||
|
|
LOG_FILE = 'log_hand_select.txt'
|
|||
|
|
LOG_LEVEL = 2 #0 2
|
|||
|
|
FULL_LOG = (lambda x: print(x, file=open(LOG_FILE, 'a'))) if LOG_LEVEL > 1 else (lambda x: None)
|
|||
|
|
LOG = (lambda x: print(x, file=open(LOG_FILE, 'a'))) if LOG_LEVEL > 0 else (lambda x: None)
|
|||
|
|
|
|||
|
|
def views_from_dimGroups(dimGroups):
|
|||
|
|
views = np.zeros(dimGroups[-1], dtype=np.int)
|
|||
|
|
for nv in range(len(dimGroups) - 1):
|
|||
|
|
views[dimGroups[nv]:dimGroups[nv+1]] = nv
|
|||
|
|
return views
|
|||
|
|
|
|||
|
|
class Select_Views:
|
|||
|
|
def __init__(self, camtoworld, handtype) -> None:
|
|||
|
|
self.camtoworld = camtoworld
|
|||
|
|
self.results = []
|
|||
|
|
self.DIST_MAX = 50
|
|||
|
|
self.threshold = 2
|
|||
|
|
self.handtype = handtype
|
|||
|
|
|
|||
|
|
self.threshold2 = 0.3
|
|||
|
|
|
|||
|
|
self.count = 0
|
|||
|
|
self.mode = 0 #[0,1] 0-sum 1-max&sum
|
|||
|
|
|
|||
|
|
def cvt_Rh_Rot(self, Rh):
|
|||
|
|
import cv2
|
|||
|
|
RotList = []
|
|||
|
|
for i in range(Rh.shape[0]):
|
|||
|
|
RotList.append(cv2.Rodrigues(Rh[i])[0])
|
|||
|
|
return np.stack(RotList)
|
|||
|
|
|
|||
|
|
def get_dis_Rh(self, Rh1, Rh2):
|
|||
|
|
rh_dis = (self.cvt_Rh_Rot(Rh1) - self.cvt_Rh_Rot(Rh2))**2
|
|||
|
|
return rh_dis.sum(axis=(1,2))
|
|||
|
|
|
|||
|
|
def match_with_lastframe(self, lastpose, new_poses):
|
|||
|
|
# breakpoint()
|
|||
|
|
if self.mode==0:
|
|||
|
|
rh_dis = self.get_dis_Rh(np.array(new_poses)[:,:3], lastpose[None][:,:3])
|
|||
|
|
dis = ((np.array(new_poses)[:,3:] - lastpose[None][:,3:])**2).sum(axis=1)
|
|||
|
|
dis+=rh_dis
|
|||
|
|
minid = np.argmin(dis)
|
|||
|
|
return new_poses[minid], dis[minid], minid, dis
|
|||
|
|
else:
|
|||
|
|
# breakpoint()
|
|||
|
|
dis1 = ((np.array(new_poses) - lastpose[None])**2).sum(axis=1)
|
|||
|
|
dis2 = ((np.array(new_poses) - lastpose[None])**2).max(axis=1)
|
|||
|
|
dis = np.stack([dis2,dis1]).T
|
|||
|
|
val_idx = dis[:,0]<self.threshold2
|
|||
|
|
dis = dis[val_idx,:]
|
|||
|
|
if(len(dis)==0):
|
|||
|
|
dis = ((np.array(new_poses) - lastpose[None])**2).sum(axis=1)
|
|||
|
|
minid = np.argmin(dis1)
|
|||
|
|
mindis = dis[minid]
|
|||
|
|
return new_poses[minid], mindis, minid, dis
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
minid = np.argmin(dis[:,1])
|
|||
|
|
mindis = dis[minid,1]
|
|||
|
|
# breakpoint()
|
|||
|
|
# minid = val_idx[minid]
|
|||
|
|
return np.array(new_poses)[val_idx,:][minid], mindis, minid, dis
|
|||
|
|
|
|||
|
|
|
|||
|
|
# breakpoint()
|
|||
|
|
# dis = ((np.array(new_poses) - lastpose[None])**2).sum(axis=1)
|
|||
|
|
# minid = np.argmin(dis)
|
|||
|
|
# return new_poses[minid], mindis, minid, dis
|
|||
|
|
|
|||
|
|
def calculate_aff(self, poseslist, DIST_MAX):
|
|||
|
|
#TODO Rh的距离不能这么求，最好是转成Rot再求误差
|
|||
|
|
M = len(poseslist)
|
|||
|
|
distance = np.zeros((M, M), dtype=np.float32)
|
|||
|
|
for id0 in range(M):
|
|||
|
|
for id1 in range(id0+1,M):
|
|||
|
|
p0 = poseslist[id0]
|
|||
|
|
p1 = poseslist[id1]
|
|||
|
|
dis = ((p0-p1)**2).sum()
|
|||
|
|
distance[id0,id1]=dis
|
|||
|
|
distance[id1,id0]=dis
|
|||
|
|
DIST_MAX = max(DIST_MAX, distance.max())
|
|||
|
|
# breakpoint()
|
|||
|
|
# return distance
|
|||
|
|
for nv in range(M):
|
|||
|
|
distance[nv,nv]=DIST_MAX
|
|||
|
|
# for nv in range(nViews):
|
|||
|
|
# distance[dimGroups[nv]:dimGroups[nv+1], dimGroups[nv]:dimGroups[nv+1]] = DIST_MAX
|
|||
|
|
distance -= np.eye(M) * DIST_MAX
|
|||
|
|
aff = (DIST_MAX - distance)/DIST_MAX
|
|||
|
|
aff = np.clip(aff, 0, 1)
|
|||
|
|
return aff
|
|||
|
|
|
|||
|
|
def Hierarchical_Cluster(self, data,threshold=2):
|
|||
|
|
# import matplotlib.pyplot as plt
|
|||
|
|
# breakpoint()
|
|||
|
|
if(len(data)==1):
|
|||
|
|
return [[0]]
|
|||
|
|
import scipy.cluster.hierarchy as sch
|
|||
|
|
|
|||
|
|
out = sch.linkage(data , method = 'ward')
|
|||
|
|
ret=[]
|
|||
|
|
vis=[]
|
|||
|
|
for i in range(len(data)):
|
|||
|
|
ret.append([i])
|
|||
|
|
vis.append(0)
|
|||
|
|
|
|||
|
|
for i in range(out.shape[0]):
|
|||
|
|
if(out[i][2]>threshold):
|
|||
|
|
break
|
|||
|
|
id1 = int(out[i][0])
|
|||
|
|
id2 = int(out[i][1])
|
|||
|
|
vis[id1]=1
|
|||
|
|
vis[id2]=1
|
|||
|
|
vis.append(0)
|
|||
|
|
ret.append(ret[id1]+ret[id2])
|
|||
|
|
|
|||
|
|
groups = []
|
|||
|
|
for i in range(len(ret)):
|
|||
|
|
if vis[i]==1:
|
|||
|
|
continue
|
|||
|
|
groups.append(ret[i])
|
|||
|
|
|
|||
|
|
return groups
|
|||
|
|
|
|||
|
|
def aff_to_groups(data, affinity, dimGroups, prev_id):
|
|||
|
|
sum1 = np.zeros((affinity.shape[0]))
|
|||
|
|
for i in range(len(dimGroups)-1):
|
|||
|
|
start, end = dimGroups[i], dimGroups[i+1]
|
|||
|
|
if end == start:continue
|
|||
|
|
sum1 += affinity[:, start:end].max(axis=-1)
|
|||
|
|
n2d = affinity.shape[0]
|
|||
|
|
nViews = len(dimGroups) - 1
|
|||
|
|
idx_zero = np.zeros(nViews, dtype=np.int) - 1
|
|||
|
|
views = views_from_dimGroups(dimGroups)
|
|||
|
|
# the assigned results of each person
|
|||
|
|
p2dAssigned = np.zeros(n2d, dtype=np.int) - 1
|
|||
|
|
visited = np.zeros(n2d, dtype=np.int)
|
|||
|
|
sortidx = np.argsort(-sum1)
|
|||
|
|
pid = 0
|
|||
|
|
k3dresults = []
|
|||
|
|
breakpoint()
|
|||
|
|
|
|||
|
|
return k3dresults
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
def __call__(self, posel , cameras, match3d_l):
|
|||
|
|
hand_list=[]
|
|||
|
|
# breakpoint()
|
|||
|
|
for pid in range(len(match3d_l)):
|
|||
|
|
dt = match3d_l[pid]
|
|||
|
|
|
|||
|
|
Merge_list=[]
|
|||
|
|
Merge_list_rot = []
|
|||
|
|
|
|||
|
|
if(isinstance(dt,int)):
|
|||
|
|
# TODO:处理-1的情况，也就是没有找到合适的匹配到的手
|
|||
|
|
# hand_list.append(np.zeros((48,)))
|
|||
|
|
Merge_list_rot.append(np.zeros((54,)))
|
|||
|
|
# continue
|
|||
|
|
else:
|
|||
|
|
for cid in range(len(dt['views'])):
|
|||
|
|
nv = dt['views'][cid]
|
|||
|
|
poseid = dt['indices'][cid]
|
|||
|
|
pose = posel[nv][poseid].copy()
|
|||
|
|
|
|||
|
|
if self.camtoworld:
|
|||
|
|
Rh = pose[:,:3].copy()
|
|||
|
|
invR = np.linalg.inv(cameras['R'][nv])
|
|||
|
|
Rh_m_old = np.matrix(cv2.Rodrigues(Rh)[0])
|
|||
|
|
Rh_m_new = invR @ Rh_m_old
|
|||
|
|
Rh = cv2.Rodrigues(Rh_m_new)[0]
|
|||
|
|
Merge_list.append(np.hstack((Rh.reshape(3),pose[:,3:].reshape(-1))))
|
|||
|
|
# breakpoint()
|
|||
|
|
Merge_list_rot.append(np.hstack((np.array(Rh_m_new).reshape(-1),pose[:,3:].reshape(-1))))
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
Merge_list.append(pose.reshape(-1))
|
|||
|
|
|
|||
|
|
Rh = pose[:,:3].copy()
|
|||
|
|
Rh_m_old = np.matrix(cv2.Rodrigues(Rh)[0])
|
|||
|
|
Merge_list_rot.append(np.hstack((np.array(Rh_m_old).reshape(-1),pose[:,3:].reshape(-1))))
|
|||
|
|
|
|||
|
|
#将坐标系转换，及视角选择完的pose整理成新的集合。
|
|||
|
|
|
|||
|
|
|
|||
|
|
# breakpoint()
|
|||
|
|
# self.count, pid, self.handtype, str(groups), (0,1) 0的话是,选了哪一组？ 1 xuanle怎么选择的
|
|||
|
|
#用层次聚类的方法进行视角的选择
|
|||
|
|
# groups = self.Hierarchical_Cluster(Merge_list, self.threshold)
|
|||
|
|
|
|||
|
|
groups = self.Hierarchical_Cluster(Merge_list_rot, self.threshold)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# #求亲和矩阵，即任意两个pose之间的距离。
|
|||
|
|
# affinity = self.calculate_aff(Merge_list,self.DIST_MAX)
|
|||
|
|
# N2D = affinity.shape[0]
|
|||
|
|
# prev_id = np.zeros(N2D) - 1
|
|||
|
|
# dims = [1]*N2D
|
|||
|
|
# dimGroups = np.cumsum([0] + dims)
|
|||
|
|
# groups = self.aff_to_groups(Merge_list, affinity, dimGroups, prev_id)
|
|||
|
|
# # #根据亲和矩阵进行分组，这里可以考虑将分组的结果Merge起来。
|
|||
|
|
# groups = []
|
|||
|
|
|
|||
|
|
FULL_LOG('[select views] frame:{}, pid:{}, handtype:{}'.format(self.count, pid, self.handtype))
|
|||
|
|
FULL_LOG('[groups] groups:{}'.format(str(groups)))
|
|||
|
|
|
|||
|
|
|
|||
|
|
#合并分组结果
|
|||
|
|
new_poses = []
|
|||
|
|
for gp in groups:
|
|||
|
|
# merge_pose = np.array(Merge_list)[gp].mean(axis=0)
|
|||
|
|
merge_pose = np.array(Merge_list_rot)[gp].mean(axis=0)
|
|||
|
|
# breakpoint()
|
|||
|
|
|
|||
|
|
|
|||
|
|
Rot = merge_pose[:9].reshape((3,3))
|
|||
|
|
Rh = cv2.Rodrigues(Rot)[0]
|
|||
|
|
merge_pose = np.hstack((Rh.reshape(3),merge_pose[9:].reshape(-1)))
|
|||
|
|
|
|||
|
|
new_poses.append(merge_pose)
|
|||
|
|
#多个组，求每个组和上一帧结果之间的距离。（找出上一帧匹配的手，和这帧对应的手）
|
|||
|
|
#根据该距离在多个组之间进行选择。选出距离更小的组。
|
|||
|
|
# if self.handtype == 'handr':
|
|||
|
|
# breakpoint()
|
|||
|
|
if (len(self.results)>pid): # False and
|
|||
|
|
# TODO 求与前一帧的距离，如果发现距离过大？则尝试重启跟踪？即选择视角最多的
|
|||
|
|
pose_, dis, minid, dis_ = self.match_with_lastframe(self.results[pid],new_poses)
|
|||
|
|
FULL_LOG('[select 0 ] minid:{}'.format(minid))
|
|||
|
|
FULL_LOG('[select 0 ] dis:{}'.format(str(dis_.tolist())))
|
|||
|
|
if isinstance(dt,int) or dis_.min()>10: # 没有合适的视角检测到手，或者所有视角检测到的都与上一帧差的很远
|
|||
|
|
FULL_LOG('[select 0 ] las pose')
|
|||
|
|
pose_ = self.results[pid].copy()
|
|||
|
|
else:
|
|||
|
|
threshold_=0.3
|
|||
|
|
if self.mode==1:
|
|||
|
|
threshold_=1
|
|||
|
|
if(dis>threshold_):# 超过一定阈值，假定上一帧不是很好，则这帧重选
|
|||
|
|
array_len = np.array([len(gp) for gp in groups])
|
|||
|
|
a_max = array_len.max()
|
|||
|
|
d_max = 500
|
|||
|
|
idx=0
|
|||
|
|
for gid in range(array_len.shape[0]):
|
|||
|
|
# breakpoint()
|
|||
|
|
if array_len[gid]==a_max and dis_[gid]<d_max:
|
|||
|
|
d_max = dis_[gid]
|
|||
|
|
idx=gid
|
|||
|
|
# dis_[array_len==a_max
|
|||
|
|
# breakpoint()
|
|||
|
|
# dis_
|
|||
|
|
# idx=np.argmax([len(gp) for gp in groups])
|
|||
|
|
pose_ = new_poses[idx].copy()
|
|||
|
|
|
|||
|
|
FULL_LOG('[select 0 ] max len(groups):{}\n'.format(idx))
|
|||
|
|
|
|||
|
|
self.results[pid] = pose_.copy()
|
|||
|
|
else:
|
|||
|
|
#TODO如果没有前一帧的监督，一种可以用所有组的结果进行处理，另外就是可以用数量较多的组的结果
|
|||
|
|
#TODO 如果数量相同的有多组，需要进一步处理 比如根据aff求sum最大的？
|
|||
|
|
idx=np.argmax([len(gp) for gp in groups])
|
|||
|
|
pose_ = new_poses[idx].copy()
|
|||
|
|
self.results.append(pose_.copy())
|
|||
|
|
|
|||
|
|
FULL_LOG('[select 1 ] max len(groups):{}\n'.format(idx))
|
|||
|
|
#将结果整理返回，有一组和身体id对应的左手或者右手的Pose集合（在世界坐标系下的），也可以返回Params ,看卡params是个list还是dict?
|
|||
|
|
|
|||
|
|
hand_list.append(pose_)
|
|||
|
|
poses_ = np.stack(hand_list)
|
|||
|
|
Rh = poses_[:,:3].copy()
|
|||
|
|
poses_[:,:3] = 0
|
|||
|
|
params={
|
|||
|
|
'Rh':Rh,
|
|||
|
|
'Th':np.zeros_like(Rh),
|
|||
|
|
'poses':poses_,
|
|||
|
|
'shapes':np.zeros((Rh.shape[0],10)),
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
self.count+=1
|
|||
|
|
|
|||
|
|
return {'params': params}
|
|||
|
|
|
|||
|
|
class Select_Views_handlr:
|
|||
|
|
def __init__(self, camtoworld) -> None:
|
|||
|
|
self.camtoworld = camtoworld
|
|||
|
|
self.model_l = Select_Views(camtoworld, 'handl')
|
|||
|
|
self.model_r = Select_Views(camtoworld, 'handr')
|
|||
|
|
|
|||
|
|
def __call__(self, posel, poser, match3d_l, match3d_r, cameras) -> Any:
|
|||
|
|
params_l = self.model_l(posel, cameras, match3d_l)
|
|||
|
|
params_r = self.model_r(poser, cameras, match3d_r)
|
|||
|
|
return {'params_l':params_l['params'], 'params_r':params_r['params']}
|