class graph3D: fig = None ax = None jointFrames = [] # all the frames all_laban = [] timeS = [] joints = [] # temporary joints to be drawn # a joint point, 'ts' stands for tracking status jType = np.dtype({ 'names': ['x', 'y', 'z', 'ts'], 'formats': [float, float, float, int] }) # a body bType = np.dtype({ 'names': [ 'timeS', # milliseconds 'filled', # filled gap 'spineB', 'spineM', # meter 'neck', 'head', 'shoulderL', 'elbowL', 'wristL', 'handL', # tracked=2, inferred=1, nottracked=0 'shoulderR', 'elbowR', 'wristR', 'handR', 'hipL', 'kneeL', 'ankleL', 'footL', 'hipR', 'kneeR', 'ankleR', 'footR', 'spineS', 'handTL', 'thumbL', 'handTR', 'thumbR' ], 'formats': [ long, bool, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType, jType ] }) isInterpolatedKeyFrame = False annSelection = None #------------------------------------------------------------------------------ # Class initialization # def __init__(self): self.strTitle = '3D Joint Data' self.fig = plt.figure() self.fig.canvas.set_window_title(self.strTitle) self.fig.set_size_inches((settings.screen_cx * 0.49) / self.fig.dpi, (settings.screen_cy * 0.465) / self.fig.dpi) self.fig.canvas.mpl_connect('resize_event', self.onresize) self.fig.canvas.mpl_connect('close_event', self.onclose) offset = 0.08 self.ax = Axes3D(self.fig, rect=[0, offset, 1.0, 1.0 - offset]) # (left, bottom, width, height) self.ax.view_init(10, 10) self.ax.dist = 9 self.axSliderTime = plt.axes([0.08, 0.055, 0.80, 0.04], facecolor='lightgoldenrodyellow') self.slidertime = Slider(self.axSliderTime, 'Time', 0.0, 1.0, valinit=0, valstep=0.005, valfmt='%0.03fs') self.cidSlider = self.slidertime.on_changed(self.onsliderupdate) # set currect axes context back to main axes self.ax plt.sca(self.ax) offset = 0.112 rect = [0.08, offset + 0.036, 0.80, 0.03] self.axFrameBlocks1 = plt.axes(rect) rect = [0.08, offset, 0.80, 0.03] self.axFrameBlocks2 = plt.axes(rect) # set currect axes context back to main axes self.ax plt.sca(self.ax) # add empty joints placeholders # [x, y, z, father joint] serial number is the same as Kinect self.joints.append([0, 0, 0, -1]) # spineBase,#0 self.joints.append([0, 0, 0, 0]) # spineMid,#1 self.joints.append([0, 0, 0, 20]) # neck,#2 self.joints.append([0, 0, 0, 2]) # head, #3 self.joints.append([0, 0, 0, 20]) # shoulderLeft, #4 self.joints.append([0, 0, 0, 4]) # elbowLeft, #5 self.joints.append([0, 0, 0, 5]) # wristLeft, #6 self.joints.append([0, 0, 0, 6]) # handLeft, #7 self.joints.append([0, 0, 0, 20]) # shoulderRight, #8 self.joints.append([0, 0, 0, 8]) # elbowRight, #9 self.joints.append([0, 0, 0, 9]) # wristRight, #10 self.joints.append([0, 0, 0, 10]) # handRight, #11 self.joints.append([0, 0, 0, 0]) # hipLeft, #12 self.joints.append([0, 0, 0, 12]) # kneeLeft, #13 self.joints.append([0, 0, 0, 13]) # ankleLeft, #14 self.joints.append([0, 0, 0, 14]) # footLeft, #15 self.joints.append([0, 0, 0, 0]) # hipRight, #16 self.joints.append([0, 0, 0, 16]) # kneeRight, #17 self.joints.append([0, 0, 0, 17]) # ankleRight, #18 self.joints.append([0, 0, 0, 18]) # footRight, #19 self.joints.append([0, 0, 0, 1]) # spineSoulder, #20 self.joints.append([0, 0, 0, 7]) # handTipLeft, #21 self.joints.append([0, 0, 0, 6]) # thumbLeft, #22 self.joints.append([0, 0, 0, 11]) # handTipRight, #23 self.joints.append([0, 0, 0, 10]) # thumbTipRight, #24 # ----------------------------------------------------------------------------- # canvas close event # def onclose(self, event): self.fig = None # if user closes this figure, let the main application know and to exit settings.application.close() #------------------------------------------------------------------------------ # canvas resize event # def onresize(self, event): self.setAxisLimits() #------------------------------------------------------------------------------ # slider update event # def onsliderupdate(self, val): # map to [0..1] p = self.slidertime.val / (self.slidertime.valmax - self.slidertime.valmin) self.selectTime(p, False) # call main application so that other graphs can be updated as well settings.application.selectTime(p, self) # ----------------------------------------------------------------------------- # def updateInputName(self): self.fig.canvas.set_window_title( self.strTitle + ' - [' + settings.application.strBeautifiedInputFile + ']') #------------------------------------------------------------------------------ # def saveView(self): if (self.fig is None): return filePath = os.path.join( settings.application.outputFolder, settings.application.outputName + '_3DJointData.png') filePath = settings.checkFileAlreadyExists(filePath, fileExt=".png", fileTypes=[ ('png files', '.png'), ('all files', '.*') ]) if (filePath is None): return try: self.fig.savefig(filePath, bbox_inches='tight') settings.application.logMessage( "3D Joint Data view was saved to '" + settings.beautifyPath(filePath) + "'") except Exception as e: strError = e settings.application.logMessage( "Exception saving 3D Joint Data view to '" + settings.beautifyPath(filePath) + "': " + str(e)) #------------------------------------------------------------------------------ # render frame blocks for both kinect and labanotation # def renderFrameBlocks(self): self.axFrameBlocks1.clear() self.axFrameBlocks2.clear() # after a clear() annSelection object is gone. reset variable self.annSelection = None padding = 0.0 duration = 0.0 cnt = len(self.jointFrames) if (cnt > 0): duration = self.jointFrames[cnt - 1]['timeS'][0] maxTime = (duration / 1000.0) def format_func(value, tick_number): return r"${:.2f}$".format(value) self.axSliderTime.set_xlim((0, maxTime)) self.axSliderTime.tick_params(axis='x', labelsize=8) self.axSliderTime.get_xaxis().set_major_locator( ticker.AutoLocator()) self.axSliderTime.get_xaxis().set_minor_locator( ticker.AutoMinorLocator()) # show minor ticks every 5 frames and major ticks every 10 frames on the top of frame block 1 onetick = (maxTime) / float(cnt) self.axFrameBlocks1.set_xlim((0, maxTime)) self.axFrameBlocks1.xaxis.tick_top() self.axFrameBlocks1.xaxis.set_minor_locator( ticker.MultipleLocator(onetick * 5)) self.axFrameBlocks1.xaxis.set_major_locator( ticker.MultipleLocator(onetick * 10)) self.axFrameBlocks1.set_xticklabels([]) self.axFrameBlocks1.get_yaxis().set_visible(False) self.axFrameBlocks2.set_xlim((0, maxTime)) self.axFrameBlocks2.get_xaxis().set_visible(False) self.axFrameBlocks2.get_yaxis().set_visible(False) # render individual kinect joint frame blocks xx = self.axFrameBlocks1.get_xlim() yy = self.axFrameBlocks1.get_ylim() cx = (xx[1] - xx[0]) cy = (yy[1] - yy[0]) padding = cx * 0.01 cnt = len(self.jointFrames) if (cnt > 0): padding = (cx / cnt) * 0.3 for i in range(0, cnt): start = (self.jointFrames[i]['timeS'][0] / 1000.0) - (padding / 2.0) x_width = padding isFilled = self.jointFrames[i]['filled'][0] c = 'r' if (isFilled) else 'b' p = patches.Rectangle((start, yy[0]), x_width, cy, alpha=0.50, color=c) self.axFrameBlocks1.add_patch(p) self.axFrameBlocks1.text((maxTime), (cy / 2.0), ' Original', horizontalalignment='left', verticalalignment='center', color='black') # render individual laban frame blocks xx = self.axFrameBlocks2.get_xlim() yy = self.axFrameBlocks2.get_ylim() cx = (xx[1] - xx[0]) cy = (yy[1] - yy[0]) cnt = len(self.timeS) if (cnt > 0): for i in range(0, cnt): start = (self.timeS[i] / 1000.0) - (padding / 2.0) x_width = padding p = patches.Rectangle((start, yy[0]), x_width, cy, alpha=0.50, color='g') self.axFrameBlocks2.add_patch(p) self.axFrameBlocks2.text((maxTime), (cy / 2.0), ' Labanotation', horizontalalignment='left', verticalalignment='center', color='black') self.fig.canvas.draw_idle() #------------------------------------------------------------------------------ # def setJointFrames(self, jointFrames_in): self.jointFrames = copy.copy(jointFrames_in) if (len(self.jointFrames) > 0): timeS0 = self.jointFrames[len(self.jointFrames) - 1]['timeS'][0] self.slidertime.valmax = float(timeS0) / 1000.0 else: self.slidertime.valmax = 0.0 # set the slider axes to take valmax change self.slidertime.ax.set_xlim(self.slidertime.valmin, self.slidertime.valmax) self.fig.canvas.draw_idle() self.renderFrameBlocks() #------------------------------------------------------------------------------ # def setLabanotation(self, timeS, all_laban): self.timeS = copy.copy(timeS) self.all_laban = copy.copy(all_laban) self.renderFrameBlocks() #------------------------------------------------------------------------------ # def selectTime(self, time, fUpdateSlider=False): time = self.slidertime.valmin + ( time * (self.slidertime.valmax - self.slidertime.valmin)) # disconnect slider update callback to avoid endless loop. Reconnect # once slider value reset if (fUpdateSlider): self.slidertime.disconnect(self.cidSlider) self.slidertime.set_val(time) self.cidSlider = self.slidertime.on_changed(self.onsliderupdate) cnt = len(self.jointFrames) if (cnt == 0): return # find the frame corresponding to the given time for idx in range(0, cnt): temp = self.jointFrames[idx] kt = float(temp['timeS'][0]) / 1000.0 if (kt >= time): break self.isInterpolatedKeyFrame = temp['filled'][0] # take a kinect snapshot of joints in time and render them. # Map to graph's xyz space a = self.joints for i in range(0, 25): a[i][0] = -temp[0][i + 2][2] a[i][1] = -temp[0][i + 2][0] a[i][2] = temp[0][i + 2][1] self.drawKinectSkeleton() if ((self.all_laban is not None) and (len(self.all_laban) > 0)): cnt = len(self.timeS) # find the frame corresponding to the given time for idx in range(0, cnt): kt = (self.timeS[idx]) / 1000.0 if (kt >= time): break laban = self.all_laban[idx] time = int(self.timeS[idx]) if (settings.fVerbose): print('Right Elbow:' + str(elR[t][1]) + ', ' + str(elR[t][2])) print('Right Wrist:' + str(wrR[t][1]) + ', ' + str(wrR[t][2])) print('Left Elbow:' + str(elL[t][1]) + ', ' + str(elL[t][2])) print('Left Wrist:' + str(wrL[t][1]) + ', ' + str(wrL[t][2])) curr_laban = [ 'Start Time:' + str(time), 'Duration:0', 'Head:Forward:Normal', 'Right Elbow:' + laban[0][0] + ':' + laban[0][1], 'Right Wrist:' + laban[1][0] + ':' + laban[1][1], 'Left Elbow:' + laban[2][0] + ':' + laban[2][1], 'Left Wrist:' + laban[3][0] + ':' + laban[3][1], 'Rotation:ToLeft:0.0' ] self.drawLabanotationSkeleton(laban=curr_laban) if (self.axFrameBlocks2 != None): if (self.annSelection != None): self.annSelection.remove() color = 'green' time = time / 1000.0 self.annSelection = self.axFrameBlocks2.annotate( '', xy=(time, 0.0), xytext=(time, -0.5), weight='bold', color=color, arrowprops=dict(arrowstyle='wedge', connectionstyle="arc3", color=color)) #------------------------------------------------------------------------------ # def setAxisLimits(self): # get canvas size in pixels size = self.fig.get_size_inches() * self.fig.dpi # calculate axis limits while keeping aspect ratio at 1 aspect = size[0] / size[1] min = -20 max = 20 axmin = (min) if (aspect < 1) else (min * aspect) axmax = (max) if (aspect < 1) else (max * aspect) aymin = (min) if (aspect > 1) else (min / aspect) aymax = (max) if (aspect > 1) else (max / aspect) # set limits. Remember our axis mapped (x=z), (y=x) and (z=y) self.ax.set_xlim3d(min, max) self.ax.set_ylim3d(axmin, axmax) self.ax.set_zlim3d(aymin, aymax) #------------------------------------------------------------------------------ # def resetGraph(self): # clear canvas self.ax.clear() self.setAxisLimits() # Draw x, y, and z axis markers in the same way you were in # the code snippet in your question... xspan, yspan, zspan = 3 * [np.linspace(0, 6, 15)] zero = np.zeros_like(xspan) self.ax.plot3D(xspan, zero, zero, 'k--', gid='axis', color='black') self.ax.plot3D(zero, yspan, zero, 'k--', gid='axis', color='black') self.ax.plot3D(zero, zero, zspan, 'k--', gid='axis', color='black') q = -.20 w = 1 self.ax.text(xspan.max() + w, q, q, "z", gid='axis', color='black') self.ax.text(q, yspan.max() + w, q, "x", gid='axis', color='black') self.ax.text(q, q, zspan.max() + w, "y", gid='axis', color='black') self.ax.xaxis._axinfo['tick']['color'] = (1.0, 1.0, 1.0, 0.5) self.ax.yaxis._axinfo['tick']['color'] = (1.0, 1.0, 1.0, 0.5) self.ax.zaxis._axinfo['tick']['color'] = (1.0, 1.0, 1.0, 0.5) self.ax.xaxis._axinfo['axisline']['color'] = (1.0, 1.0, 1.0, 0.5) self.ax.yaxis._axinfo['axisline']['color'] = (1.0, 1.0, 1.0, 0.5) self.ax.zaxis._axinfo['axisline']['color'] = (1.0, 1.0, 1.0, 0.5) # get rid of the panes self.ax.w_xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) self.ax.w_yaxis.set_pane_color((1.0, 1.0, 1.0, 0.5)) # self.ax.w_zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) # get rid of the spines #self.ax.xaxis.line.set_color((1.0, 1.0, 1.0, 0.0)) #self.ax.yaxis.line.set_color((1.0, 1.0, 1.0, 0.5)) #self.ax.zaxis.line.set_color((1.0, 1.0, 1.0, 0.5)) # don't show tick labels self.ax.xaxis.set_major_formatter(plt.NullFormatter()) self.ax.yaxis.set_major_formatter(plt.NullFormatter()) self.ax.zaxis.set_major_formatter(plt.NullFormatter()) #------------------------------------------------------------------------------ # # rotate around x axis: rotate around y axis: rotates around # np.array([ np.array([ np.array([ # [1, 0, 0], [c, 0, -s], [c, -s, 0], # [0, c, -s], [0, 1, 0], [s, c, 0], # [0, s, c] [s, 0, c] [0, 0, 1], # ])) ])) ])) def correctSkeletonRotation(self): shL = np.zeros(3) shR = np.zeros(3) spM = np.zeros(3) shL[0] = self.joints[4][1] # left shoulder shL[1] = self.joints[4][2] shL[2] = self.joints[4][0] shR[0] = self.joints[8][1] # right shoulder shR[1] = self.joints[8][2] shR[2] = self.joints[8][0] spM[0] = self.joints[1][1] # spine middle spM[1] = self.joints[1][2] spM[2] = self.joints[1][0] # find where the performer is facing, rotate the body figure sh = np.zeros((3, 3)) v1 = shL - shR v2 = spM - shR sh[0] = np.cross(v2, v1) # x axis x = sh[0][0] y = sh[0][1] z = sh[0][2] # rotate around y axis r = math.sqrt(z * z + x * x) sinth = x / r costh = z / r conv = np.zeros((3, 3)) conv[0][0] = costh conv[0][1] = 0 conv[0][2] = -sinth conv[1][0] = 0 conv[1][1] = 1 conv[1][2] = 0 conv[2][0] = sinth conv[2][1] = 0 conv[2][2] = costh for i in range(len(self.joints)): tmp_in = np.zeros(3) tmp_in[0] = self.joints[i][1] tmp_in[1] = self.joints[i][2] tmp_in[2] = self.joints[i][0] tmp_out = np.dot(conv, tmp_in) self.joints[i][1] = tmp_out[0] self.joints[i][2] = tmp_out[1] self.joints[i][0] = tmp_out[2] #------------------------------------------------------------------------------ # def draw_joints(self, c, a=1.0): alpha = a cnt = len(self.joints) - 4 # do not draw hand tips and thumbs for i in range(cnt): if (i == 7) or (i == 11): # do not draw hands continue if (i == 20) or (i == 4) or (i == 5) or (i == 6) or (i == 8) or ( i == 9) or (i == 10): alpha = a else: alpha = 0.3 p = self.joints[i] self.ax.scatter(p[0], p[1], p[2], color=c, alpha=alpha) #------------------------------------------------------------------------------ # def draw_limbs(self): for i in range(1, len(self.joints) - 4): if (i == 7) or (i == 11): # do not draw hands continue if (i == 4) or (i == 5) or (i == 6) or (i == 8) or (i == 9) or ( i == 10): alpha = 1.0 else: alpha = 0.3 a = self.joints[i] start = [a[0], a[1], a[2]] b = self.joints[self.joints[i][3]] end = [b[0], b[1], b[2]] self.ax.plot([start[0], end[0]], [start[1], end[1]], [start[2], end[2]], '--', color='k', alpha=alpha, gid='limbs') # body frame a = self.joints[1] start = [a[0], a[1], a[2]] b = self.joints[4] end = [b[0], b[1], b[2]] self.ax.plot([start[0], end[0]], [start[1], end[1]], [start[2], end[2]], '--', color='k', alpha=0.3, gid='limbs') b = self.joints[8] end = [b[0], b[1], b[2]] self.ax.plot([start[0], end[0]], [start[1], end[1]], [start[2], end[2]], '--', color='k', alpha=0.3, gid='limbs') #------------------------------------------------------------------------------ # def drawKinectSkeleton(self): self.resetGraph() # center skeleton around its spineB move_0 = self.joints[0][0] move_1 = self.joints[0][1] move_2 = self.joints[0][2] for i in range(0, len(self.joints)): self.joints[i][0] = self.joints[i][0] - move_0 self.joints[i][1] = self.joints[i][1] - move_1 self.joints[i][2] = self.joints[i][2] - move_2 # rotate skeleton to face forward towards camera self.correctSkeletonRotation() # scale, spine_shoulder->spine_midlle is 5 d0 = self.joints[20][0] - self.joints[1][0] d1 = self.joints[20][1] - self.joints[1][1] d2 = self.joints[20][2] - self.joints[1][2] d = (d0**2 + d1**2 + d2**2)**0.5 self.scale = (5.0 / d) for i in range(0, len(self.joints)): self.joints[i][0] = (self.joints[i][0] * self.scale) self.joints[i][1] = (self.joints[i][1] * self.scale) - 15 # offset from center self.joints[i][2] = (self.joints[i][2] * self.scale) faceColor = 'r' if self.isInterpolatedKeyFrame else 'b' self.draw_joints(faceColor, 0.50) self.draw_limbs() # skeleton title self.ax.text(self.joints[3][0], self.joints[3][1], self.joints[3][2] + 2, 'Original', bbox={ 'facecolor': faceColor, 'alpha': 0.50, 'edgecolor': 'gray', 'pad': 3.5 }, ha='center', va='bottom', color='w') self.fig.canvas.draw_idle() #------------------------------------------------------------------------------ # convert labanotation to joint points # def mapLabanotation2Joints(self, laban): self.calc_joint(1, [0, 6, 0]) # spineBase->spineMid self.calc_joint(20, [0, 5, 0]) # spineMid->spineShoulder self.calc_joint(2, [0, 2, 0]) # spineShoulder->neck self.calc_joint(3, [0, 3, 0]) # neck->head self.calc_joint(4, [4, -0.5, 0]) # spineShoulder->shoulderLeft vec = self.laban2vec(laban, "left elbow") # shoulderLeft->elbowLeft self.calc_joint(5, vec) vec = self.laban2vec(laban, "left wrist") # elbowLeft->wristLeft self.calc_joint(6, vec) self.calc_joint(7, [0, 0, 0]) # wristLeft->handLeft self.calc_joint(8, [-4, -0.5, 0]) # spineShoulder->shoulderRight vec = self.laban2vec(laban, "right elbow") # shoulderRight->elbowRight self.calc_joint(9, vec) vec = self.laban2vec(laban, "right wrist") # elbowRight->wristRight self.calc_joint(10, vec) self.calc_joint(11, [0, 0, 0]) # wristLeft->handLeft self.calc_joint(12, [1.5, -0, 0.5]) # spineBase->hipLeft self.calc_joint(13, [0, -6, 0]) # hipLeft->kneeLeft self.calc_joint(14, [0, -7, 0]) # kneeLeft->ankleLeft self.calc_joint(15, [0.2, -0.2, 3]) # ankleLeft->footLeft self.calc_joint(16, [-1.5, -0, 0.5]) # spineBase->hipRight self.calc_joint(17, [0, -6, 0]) # hipRight->kneeRight self.calc_joint(18, [0, -7, 0]) # kneeRight->ankleRight self.calc_joint(19, [-0.2, -0.2, 3]) # ankleRight->footRight #------------------------------------------------------------------------------ # calculate the given joint using its parent joint and a vector # def calc_joint(self, target, vec): self.joints[target][0] = self.joints[self.joints[target] [3]][0] + vec[0] self.joints[target][1] = self.joints[self.joints[target] [3]][1] + vec[1] self.joints[target][2] = self.joints[self.joints[target] [3]][2] + vec[2] #------------------------------------------------------------------------------ # convert the labanotation for a given limb to a vector # def laban2vec(self, laban, limb): theta = 175 phi = 0 pi = 3.1415926 for i in range(0, len(laban)): laban[i] = laban[i].lower() tmp_str = laban[i].split(":") if tmp_str[0] == limb: lv = tmp_str[2] if lv == "high": theta = 45 elif lv == "normal": theta = 90 elif lv == "low": theta = 135 else: theta = 180 print('Unknown Level.') dire = tmp_str[1] if dire == "forward": phi = 0 elif dire == "right forward": phi = -45 elif dire == "right": phi = -90 elif dire == "right backward": phi = -135 elif dire == "backward": phi = 180 elif dire == "left backward": phi = 135 elif dire == "left": phi = 90 elif dire == "left forward": phi = 45 elif dire == "place": if lv == "high": theta = 5 phi = 0 elif lv == "low": theta = 175 phi = 0 else: theta = 180 phi = 0 print('Unknown Place') else: phi = 0 print('Unknown Direction.') break # length of the limb l = 5 y = l * math.cos(theta / 180.0 * pi) x = l * math.sin(theta / 180.0 * pi) * math.sin(phi / 180.0 * pi) z = l * math.sin(theta / 180.0 * pi) * math.cos(phi / 180.0 * pi) return [x, y, z] #------------------------------------------------------------------------------ # def drawLabanotationSkeleton(self, laban=""): if (laban == ''): return for i in range(0, len(self.joints)): self.joints[i][0] = 0 self.joints[i][1] = 0 self.joints[i][2] = 0 self.mapLabanotation2Joints(laban) # map to graph xyz for i in range(0, len(self.joints)): x = self.joints[i][0] y = self.joints[i][1] z = self.joints[i][2] self.joints[i][0] = z self.joints[i][1] = x + 15 # offset from center self.joints[i][2] = y self.draw_joints('g', 0.50) self.draw_limbs() # skeleton title self.ax.text(self.joints[3][0], self.joints[3][1], self.joints[3][2] + 2, 'Labanotation', bbox={ 'facecolor': 'g', 'alpha': 0.50, 'edgecolor': 'gray', 'pad': 3.5 }, ha='center', va='bottom', color='w') self.fig.canvas.draw_idle()