def r_solve(self):
start = time.time()
x = [coord[0] for coord in self.problema.values()]
y = [coord[1] for coord in self.problema.values()]
center = [np.mean(x), np.mean(y)]
self.solution.sort(
key=lambda point: angle(self.problema[point], center))
end = time.time()
self.ordenar_solucion()
print(f'Solucion r: {self.compute_dist()} m')
return end - start
def actualize(self, mouse, t):
self.mouse = Vector(*mouse)
# FIXME the derivative part doesn't seem to work well
self.a = self.error * self.kp - self.d_error * self.kd
self.v += self.a * t
self.error_ant = self.error
self.trim_vel()
self.verts[0]['pos'] += self.v * t
# Calculate positions of the rest ones
for i, vert in enumerate(self.verts[1:]):
u = (vert['pos'] - self.verts[i]['pos']).unit()
u_angle = angle(Vector(1, 0), u)
# print u_angle
pygame.draw.line(self.screen, (200, 0, 255),
vert['pos'].get_comps(False),
(vert['pos'] - u * self.space).get_comps(False),
1)
vert['pos'] = self.verts[i]['pos'] + u * self.space
if vert['feet']:
if vert['shoulder angle'] + u_angle < math.pi / 6.0:
self.status = self.moving_feet
if vert['shoulder angle'] + u_angle > 1 * math.pi / 2.0:
self.status = self.static_feet
# print self.status.__name__
self.status(vert)
for e, mult in enumerate([-1, 1]):
vert['elbow'][e] = VectorPolar(
self.humerus, mult * vert['shoulder angle'] +
u_angle).to_cartesian() + vert['pos']
vert['feet'][e] = VectorPolar(
self.ulna_radius,
mult * (-vert['shoulder angle']) +
u_angle).to_cartesian() + vert['elbow'][e]
for joint in jsonfinal.keys():
if joint!='frame' and joint.find('ag')==-1 and joint.find('dis')==-1 and joint.find('step')==-1:
new_pos=[]
for jf in jsonfinal[joint]:
jf=tools.VIto3D(jf, jsondataS, joint[0], joint[2:])
new_pos.append(jf)
jj=joint.replace('Y', 'z')
jj=jj.replace('Z', 'y')
jj=jj.replace('X', 'x')
jsonfifinal[jj]=new_pos
jsonfifinal['frame']=tfl
jsonfifinal['step']=jsonfinal['step']
jsonfifinal['kangle_l']=[]
jsonfifinal['kangle_r']=[]
for i in range(len(tfl)):
jsonfifinal['kangle_l'].append(180-tools.angle((jsonfifinal['x_las'][i],jsonfifinal['y_las'][i],jsonfifinal['z_las'][i]),(jsonfifinal['x_lkn'][i], jsonfifinal['y_lkn'][i],jsonfifinal['z_lkn'][i]),(jsonfifinal['x_lak'][i],jsonfifinal['y_lak'][i],jsonfifinal['z_lak'][i])))
jsonfifinal['kangle_r'].append(180-tools.angle((jsonfifinal['x_ras'][i],jsonfifinal['y_ras'][i],jsonfifinal['z_ras'][i]),(jsonfifinal['x_rkn'][i], jsonfifinal['y_rkn'][i],jsonfifinal['z_rkn'][i]),(jsonfifinal['x_rak'][i],jsonfifinal['y_rak'][i],jsonfifinal['z_rak'][i])))
data=pd.DataFrame(jsonfifinal)
data.to_csv('testVedios'+'/test'+nb_video+'/'+number+'_GT-Vicon_LCR-NET_3DKinect_'+direction+'_respectiveTF.csv', encoding='gbk')
print('GT_Kinect'+keywords_file12.upper()[:-2]+'.csv', 'saved!')
# save jsonfififinal : gt transformée générale à Kinect par rapport de l'Algo
jsonfififinal={}
for joint in jsonfinal.keys():
if joint!='frame' and joint.find('ag')==-1 and joint.find('dis')==-1 and joint.find('step')==-1:
new_pos_g=[]
for jf in jsonfinal[joint]:
jf=tools.VIto3D(jf, jsondataS, joint[0])
new_pos_g.append(jf)
jj=joint.replace('Y', 'z')
print('-----filter for kn-----')
k['z_rkn'] = filter(list(k['z_rkn']), size, flt_type)
k['z_lkn'] = filter(list(k['z_lkn']), size, flt_type)
k['x_rkn'] = filter(list(k['x_rkn']), size, flt_type)
k['x_lkn'] = filter(list(k['x_lkn']), size, flt_type)
k['y_rkn'] = filter(list(k['y_rkn']), size, flt_type)
k['y_lkn'] = filter(list(k['y_lkn']), size, flt_type)
ax5.plot(k['frames'], k['y_lkn'], marker='.', color='b', lw=0.2)
ax5.plot(k['frames'], k['y_rkn'], marker='.', color='r', lw=0.2)
ax5.set_title('z\' of KN_kinect')
# show corrected knee angles
for i in range(len(k['frames'])):
k['kangle_l'][i] = 180 - tools.angle(
(k['x_lak'][i], k['y_lak'][i], k['z_lak'][i]),
(k['x_lkn'][i], k['y_lkn'][i], k['z_lkn'][i]),
(k['x_las'][i], k['y_las'][i], k['z_las'][i]), False)
k['kangle_r'][i] = 180 - tools.angle(
(k['x_rak'][i], k['y_rak'][i], k['z_rak'][i]),
(k['x_rkn'][i], k['y_rkn'][i], k['z_rkn'][i]),
(k['x_ras'][i], k['y_ras'][i], k['z_ras'][i]), False)
fig3 = plt.figure()
ax2 = fig3.add_subplot(211)
ax2.plot(k['frames'], k['kangle_l'])
ax3 = fig3.add_subplot(212)
ax3.plot(k['frames'], k['kangle_r'], color='r')
ax2.set_title('Left Knee angle (filter: ' + flt_type + ' size: ' +
str(size) + ')')
ax3.set_title('Right Knee angle')
# show corrected ankle x y and z
def show_angle3d(nb_vedio, frames):
files = os.listdir('testVedios/test' + nb_vedio + '/')
for i in files:
if (len(re.findall('.*C\.json', i)) != 0):
filejson = i
if (len(re.findall('.*D.mp4', i)) != 0):
filedepth = i
number = filedepth[:-5]
#if (len(re.findall('_distance_data.csv', i))!=0):
# filedis=i
print(number)
#print(filedis)
print(filejson)
#disdata=pd.DataFrame(pd.read_csv('testVedios/test'+nb_vedio+'/'+filedis))
fig = plt.figure()
ax = fig.add_subplot(211)
ax2 = fig.add_subplot(212)
ax.set_title('3d kinect LCR-NET_Improved')
ax.set_xlabel('Frame')
ax.set_ylabel('Angle_left')
ax2.set_xlabel('Frame')
ax2.set_ylabel('Angle_right')
with open('testVedios/test' + nb_vedio + '/' + filejson) as json_data:
d = json.load(json_data)
v = cv2.VideoCapture('testVedios/test' + nb_vedio + '/' + filedepth)
anglesl = []
anglesr = []
points3dx = [[] for p in range(13)]
points3dy = [[] for p in range(13)]
points3dz = [[] for p in range(13)]
points2dx = [[] for p in range(13)]
points2dy = [[] for p in range(13)]
points2ddx = [[] for p in range(13)]
points2ddy = [[] for p in range(13)]
akdis = []
kndis = []
step = []
bad_frame = [0] * (frames[1] - frames[0])
l_kn_ak = []
r_kn_ak = []
for t in tqdm(range(frames[0], frames[1])):
#print(t)
v.set(cv2.CAP_PROP_POS_FRAMES, t)
ret, im = v.read()
img_m = cv2.medianBlur(im, 7)
font = cv2.FONT_HERSHEY_SIMPLEX
'''
for i in range(800,980,2):
for j in range(300,860,5):
a=RGBto3D((i,j),im)
x.append(a[0])
y.append(a[1])
z.append(a[2])
print(i)
#ax.scatter(x,z,y)
'''
if len(d['frames'][t]) != 0:
xjoints2d = d['frames'][t][0]['pose2d'][:13]
yjoints2d = d['frames'][t][0]['pose2d'][13:]
xjoints3d = []
yjoints3d = []
zjoints3d = []
xjoints2dd = []
yjoints2dd = []
###!!!!!!!!!11111!!!!
leg = ['z_rak', 'z_lak', 'z_rkn', 'z_lkn', 'z_ras', 'z_las']
for i in range(13):
if i <= -5:
z3d = float(disdata[leg[i]][disdata['frame'] == t])
print(z3d)
joints = tools.RGBto3D((xjoints2d[i], yjoints2d[i], z3d),
im, t)
xjoints3d.append(joints[0])
yjoints3d.append(joints[1])
zjoints3d.append(joints[2])
else:
joints = tools.RGBto3D((xjoints2d[i], yjoints2d[i]), im, t,
True, 7)
xjoints3d.append(joints[0])
yjoints3d.append(joints[1])
zjoints3d.append(joints[2])
joints2dd = tools.RGBtoD((xjoints2d[i], yjoints2d[i]), t)
xjoints2dd.append(joints2dd[0])
yjoints2dd.append(joints2dd[1])
for p in range(13):
points3dx[p].append(xjoints3d[p])
points3dy[p].append(yjoints3d[p])
points3dz[p].append(zjoints3d[p])
points2dx[p].append(xjoints2d[p])
points2dy[p].append(yjoints2d[p])
points2ddx[p].append(xjoints2dd[p])
points2ddy[p].append(yjoints2dd[p])
anglesl.append(180 -
tools.angle((xjoints3d[1], yjoints3d[1], zjoints3d[1]),
(xjoints3d[3], yjoints3d[3],
zjoints3d[3]), (xjoints3d[5], yjoints3d[5],
zjoints3d[5]), False))
anglesr.append(180 -
tools.angle((xjoints3d[0], yjoints3d[0], zjoints3d[0]),
(xjoints3d[2], yjoints3d[2],
zjoints3d[2]), (xjoints3d[4], yjoints3d[4],
zjoints3d[4]), False))
# static analysis
# save the important information:dis between ankles, as height, leg length
l_kn_ak.append(
tools.get_distance((xjoints3d[1], yjoints3d[1], zjoints3d[1]),
(xjoints3d[3], yjoints3d[3], zjoints3d[3])))
r_kn_ak.append(
tools.get_distance((xjoints3d[0], yjoints3d[0], zjoints3d[0]),
(xjoints3d[2], yjoints3d[2], zjoints3d[2])))
dak = (zjoints3d[0] - zjoints3d[1])
dkn = (zjoints3d[2] - zjoints3d[3])
akdis.append(dak)
kndis.append(dkn)
# dynamic analysis
# gen qian mian de zhen xiang bi jiao
# Model : step automatic by Vicon !!!!front back!!!!! condition 1 2 3
# dis>0 : left joint is in front, dis<0 : right joint is in front
if dkn <= 0 and dak > -100:
step.append(0)
elif dak <= -100 and dkn < 0:
step.append(1)
elif dkn >= 0 and dak < 100:
step.append(2)
elif dak >= 100 and dkn > 0:
step.append(3)
else:
step.append(-1)
# find the wrong frame
# +++++++++++
# correct the wrong ponts
# +++++++++++
# angle 2d
# anglesl.append(180-tools.angle((yjoints[1],zjoints[1]),(yjoints[3],zjoints[3]),(yjoints[5],zjoints[5])))
# anglesr.append(180-tools.angle((yjoints[0],zjoints[0]),(yjoints[2],zjoints[2]),(yjoints[4],zjoints[4])))
'''
print(xjoints)
print(yjoints)
print(zjoints)
ax2.scatter(xjoints, zjoints, yjoints, color='r')
'''
v.release()
#b, a = signal.butter(8, 0.3, 'lowpass')
#anglesl = signal.filtfilt(b, a, anglesl)
#anglesr = signal.filtfilt(b, a, anglesr)
frames = range(frames[0], frames[1])
ax.plot(frames, anglesl)
ax2.plot(frames, anglesr, color='r') #, marker='.')
#for i in range(50): y1.append(i) # 每迭代一次，将i放入y1中画出来 ax.cla() # 清除键 ax.bar(y1, label='test', height=y1, width=0.3) ax.legend() plt.pause(0.1)
data2d = {}
data = {}
data2dd = {}
data2dd['frames'] = frames
data2d['frames'] = frames
data['frames'] = frames
data['kangle_l'] = anglesl
data['kangle_r'] = anglesr
data['akdis'] = akdis
data['kndis'] = kndis
data['l_kn_ak'] = l_kn_ak
data['r_kn_ak'] = r_kn_ak
data['bad_frame'] = bad_frame
print(len(step))
data['step'] = step
j = [
'rak', 'lak', 'rkn', 'lkn', 'ras', 'las', 'rwr', 'lwr', 'rel', 'lel',
'rsh', 'lsh', 'head'
]
for p in range(13):
data['x_' + j[p]] = points3dx[p]
data['y_' + j[p]] = points3dy[p]
data['z_' + j[p]] = points3dz[p]
data2d['x_' + j[p]] = points2dx[p]
data2d['y_' + j[p]] = points2dy[p]
data2dd['x_' + j[p]] = points2ddx[p]
data2dd['y_' + j[p]] = points2ddy[p]
data = pd.DataFrame(data)
data2d = pd.DataFrame(data2d)
data2dd = pd.DataFrame(data2dd)
if frames[0] > 250:
data.to_csv('testVedios' + '/test' + nb_vedio + '/' + number +
'_LCR-NET_joints_3DKinect_back.csv',
encoding='gbk')
data2d.to_csv('testVedios' + '/test' + nb_vedio + '/' + number +
'_LCR-NET_joints_2DColor_back.csv',
encoding='gbk')
data2dd.to_csv('testVedios' + '/test' + nb_vedio + '/' + number +
'_LCR-NET_joints_2DDepth_back.csv',
encoding='gbk')
#data1=pd.DataFrame({'frame':frames, 'angle_left':anglesl, 'angle_right':anglesr})
#data1.to_csv('testVedios'+'/test'+nb_vedio+'/'+number+'_LCR-NET_angles_3DKinect_back.csv',encoding='gbk')
else:
data2dd.to_csv('testVedios' + '/test' + nb_vedio + '/' + number +
'_LCR-NET_joints_2DDepth_front.csv',
encoding='gbk')
data2d.to_csv('testVedios' + '/test' + nb_vedio + '/' + number +
'_LCR-NET_joints_2DColor_front.csv',
encoding='gbk')
data.to_csv('testVedios' + '/test' + nb_vedio + '/' + number +
'_LCR-NET_joints_3DKinect_front.csv',
encoding='gbk')
def show_angle3d(nb_video, frames):
files=os.listdir('testVideos/test'+nb_video+'/')
for i in files:
if (len(re.findall('.*C\.json', i))!=0):
filejson=i
if (len(re.findall('.*D.mp4', i))!=0):
filedepth=i
number=filedepth[:-5]
if (len(re.findall('_PifPaf_joints_3DKinect_front.csv', i))!=0):
filedis=i
print(number)
print(filejson)
print(filedis)
print('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_3DKinect_back.csv')
disdata=pd.DataFrame(pd.read_csv('testVideos/test'+nb_video+'/'+filedis))
fig=plt.figure()
ax=fig.add_subplot(211)
ax2=fig.add_subplot(212)
ax.set_title('3d kinect PifPaf_Original')
ax.set_xlabel('Frame')
ax.set_ylabel('Angle_left')
ax2.set_xlabel('Frame')
ax2.set_ylabel('Angle_right')
with open('testVideos/test'+nb_video+'/'+filejson) as json_data:
d = json.load(json_data)
v=cv2.VideoCapture('testVideos/test'+nb_video+'/'+filedepth)
anglesl=[]
anglesr=[]
points3dx=[[] for p in range(17)]
points3dy=[[] for p in range(17)]
points3dz=[[] for p in range(17)]
points2dx=[[] for p in range(17)]
points2dy=[[] for p in range(17)]
points2ddx=[[] for p in range(17)]
points2ddy=[[] for p in range(17)]
akdis=[]
kndis=[]
step=[]
bad_frame=[0]*(frames[1]-frames[0])
l_kn_ak=[]
r_kn_ak=[]
for t in tqdm(range(frames[0],frames[1])):
#print(t)
v.set(cv2.CAP_PROP_POS_FRAMES, t)
ret, im=v.read()
img_m=cv2.medianBlur(im,5)
font=cv2.FONT_HERSHEY_SIMPLEX
'''
for i in range(800,980,2):
for j in range(300,860,5):
a=RGBto3D((i,j),im)
x.append(a[0])
y.append(a[1])
z.append(a[2])
print(i)
#ax.scatter(x,z,y)
'''
if len(d['frames'][t])!=0:
xjoints2d=d['frames'][t][0]['pose2d'][:17]
yjoints2d=d['frames'][t][0]['pose2d'][17:]
zjoints3d=[]
xjoints3d=[]
yjoints3d=[]
xjoints2dd=[]
yjoints2dd=[]
leg=['z_las','z_ras','z_lkn','z_rkn','z_lak','z_rak']
for i in range(17):
if i in [11,13,15,12,14,16]:
z3d=float(disdata[leg[i-11]][disdata['frames']==t])
print(z3d)
joints=tools.RGBto3D((xjoints2d[i], yjoints2d[i], z3d), im, t)
xjoints3d.append(joints[0])
yjoints3d.append(joints[1])
zjoints3d.append(joints[2])
else:
joints=tools.RGBto3D((xjoints2d[i], yjoints2d[i]), im, t, True, 7)
xjoints3d.append(joints[0])
yjoints3d.append(joints[1])
zjoints3d.append(joints[2])
joints2dd=tools.RGBtoD((xjoints2d[i], yjoints2d[i]), t)
xjoints2dd.append(joints2dd[0])
yjoints2dd.append(joints2dd[1])
anglesl.append(180-tools.angle((xjoints3d[11],yjoints3d[11],zjoints3d[11]),(xjoints3d[13],yjoints3d[13],zjoints3d[13]),(xjoints3d[15],yjoints3d[15],zjoints3d[15]),False))
anglesr.append(180-tools.angle((xjoints3d[12],yjoints3d[12],zjoints3d[12]),(xjoints3d[14],yjoints3d[14],zjoints3d[14]),(xjoints3d[16],yjoints3d[16],zjoints3d[16]),False))
# static analysis
# save the important information:dis between ankles, as height, leg length
l_kn_ak.append(tools.get_distance((xjoints3d[12],yjoints3d[12],zjoints3d[12]),(xjoints3d[13],yjoints3d[13],zjoints3d[13])))
r_kn_ak.append(tools.get_distance((xjoints3d[9],yjoints3d[9],zjoints3d[9]),(xjoints3d[10],yjoints3d[10],zjoints3d[10])))
dak=(zjoints3d[10]-zjoints3d[13])
dkn=(zjoints3d[9]-zjoints3d[12])
akdis.append(dak)
kndis.append(dkn)
for p in range(17):
points3dx[p].append(xjoints3d[p])
points3dy[p].append(yjoints3d[p])
points3dz[p].append(zjoints3d[p])
points2dx[p].append(xjoints2d[p])
points2dy[p].append(yjoints2d[p])
points2ddx[p].append(xjoints2dd[p])
points2ddy[p].append(yjoints2dd[p])
v.release()
#b, a = signal.butter(8, 0.3, 'lowpass')
#anglesl = signal.filtfilt(b, a, anglesl)
#anglesr = signal.filtfilt(b, a, anglesr)
frames=range(frames[0],frames[1])
ax.plot(frames, anglesl)
ax2.plot(frames, anglesr, color='r')
data2d={}
data={}
data2dd={}
data2dd['frames']=frames
data2d['frames']=frames
data['frames']=frames
data['kangle_l']=anglesl
data['kangle_r']=anglesr
data['akdis']=akdis
data['kndis']=kndis
data['l_kn_ak']=l_kn_ak
data['r_kn_ak']=r_kn_ak
data['bad_frame']=bad_frame
j=['head','leye','reye','lear','rear','lsh','rsh','lel','rel','lwr','rwr','las','ras','lkn','rkn','lak','rak']
for p in range(17):
data['x_'+j[p]]=points3dx[p]
data['y_'+j[p]]=points3dy[p]
data['z_'+j[p]]=points3dz[p]
data2d['x_'+j[p]]=points2dx[p]
data2d['y_'+j[p]]=points2dy[p]
data2dd['x_'+j[p]]=points2ddx[p]
data2dd['y_'+j[p]]=points2ddy[p]
data=pd.DataFrame(data)
data2d=pd.DataFrame(data2d)
data2dd=pd.DataFrame(data2dd)
if frames[0]>250:
data.to_csv('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_3DKinect_back.csv',encoding='gbk')
data2d.to_csv('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_2DColor_back.csv',encoding='gbk')
data2dd.to_csv('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_2DDepth_back.csv',encoding='gbk')
#data1=pd.DataFrame({'frame':frames, 'angle_left':anglesl, 'angle_right':anglesr})
#data1.to_csv('testVedios'+'/test'+nb_vedio+'/'+number+'_LCR-NET_angles_3DKinect_back.csv',encoding='gbk')
else:
data2dd.to_csv('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_2DDepth_front.csv',encoding='gbk')
data2d.to_csv('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_2DColor_front.csv',encoding='gbk')
data.to_csv('testVideos'+'/test'+nb_video+'/'+number+'_PifPaf_joints_3DKinect_front.csv',encoding='gbk')
# Se calcula las distancias de todos los átomos con respecto
# al vertice. Falta identificar los átomos que participan en
# el ángulo
out = tl.rv(vatom,rmax,NALA,MXYZ)
NAV = out[0]
if NAV < 2 :
print (f"No se calculo ángulo, ya que se necesitan almenos 2 átomos diferentes al vertice")
else : #Empieza el calculo de los ángulos
VR1 = out[1]
VR = np.zeros((NAV,4))
for i in range(NAV) :
for j in range(4) :
VR[i,j] = VR1[i,j]
out = tl.angle(NAV,VR)
NA = out[0]
VA1 = out[1]
VA = np.zeros((NA))
print (f"Cantidad de angulos : ",NA)
for i in range(NA):
VA[i] = float(VA1[i])
print (f"Angulos : ",VA[i]) #Falta identificar los átomos, por ejemplo, en el orden que están en xyz
#########################################################################################
# Calculo del ángulo dihedral, que se encuentra entre cada mnolécula de H2O y el Hg #
#########################################################################################
# Se busca los oxígenos más cercanos al Hg, según el límite rmaxo. Además, se
# supone que las moléculas de H2O no están disociadas
#rmaxo = float(input("Distancia máxima O a Hg : "))
rmaxo = 2.53