def __draw_pose_confidence(self, robot_pose, confidence_plt, color: str, n_std=1.0): """ Draw confidence ellipse around the robot pose :param pojo.Pose robot_pose: robot's pose matplotlib.patches.Wedge confidence_plt: plot of robot position confidence str color: color used to render robot position confidence float n_std: number of std to determine ellipse's radius :return matplotlib.patches.Ellipse """ pose_x, pose_y, _ = robot_pose.get_position() covariance = robot_pose.get_covariance() scale = self._map_data.get_scale() # reference https://matplotlib.org/devdocs/gallery/statistics/confidence_ellipse.html # cov_xy / np.sqrt(cov_xx * cov_yy) pearson = covariance[0, 1] / np.sqrt( covariance[0, 0] * covariance[1, 1]) # compute eigenvalues and rescale ell_radius_x = np.sqrt(1 + pearson) / scale ell_radius_y = np.sqrt(1 - pearson) / scale # compute mean and std scale_x = np.sqrt(covariance[0, 0] / scale) * n_std scale_y = np.sqrt(covariance[1, 1] / scale) * n_std mean_x, mean_y = pose_x, pose_y transform = transforms.Affine2D().rotate_deg(45) \ .scale(scale_x, scale_y) \ .translate(mean_x, mean_y) if confidence_plt == None: confidence_plt = Ellipse((0, 0), width=ell_radius_x, height=ell_radius_y, facecolor='none', edgecolor=color) confidence_plt.set_transform(transform + self._plt_ax.transData) self._plt_ax.add_artist(confidence_plt) else: confidence_plt.width = ell_radius_x confidence_plt.height = ell_radius_y confidence_plt.set_transform(transform + self._plt_ax.transData) return confidence_plt
def velocity_ell(q1, q2): fig = plt.figure() ax = fig.add_subplot(111) plt.axis('equal') ax.set_xlim([-2.5, 3.5]) ax.set_ylim([-2.5, 3.5]) # load some test data for demonstration and plot a wireframe ax.plot([0, 1], [0, 0], [0, 0], c='blue') ax.plot([0, 0], [0, 1], [0, 0], c='green') link_1, = ax.plot([0, 0], [1, 0], lw=6, c='blue') link_2, = ax.plot([1, 2], [0, 0], lw=6, c='green') el = Ellipse((2, 0), 1, 0, angle=90, linewidth=3, fill=False) ax.add_patch(el) l1 = 1 l2 = 1 point1 = np.array([l1 * np.cos(q1), l1 * np.sin(q1)]) point2 = np.array( [point1[0] + l2 * np.cos(q2 + q1), point1[1] + l2 * np.sin(q2 + q1)]) link_1.set_data([0, point1[0]], [0, point1[1]]) link_2.set_data([point1[0], point2[0]], [point1[1], point2[1]]) Jac = np.array( [[-l1 * np.sin(q1) - l2 * np.sin(q1 + q2), -l2 * np.sin(q1 + q2)], [l1 * np.cos(q1) + l2 * np.cos(q1 + q2), l2 * np.cos(q1 + q2)]]) A = np.matmul(Jac, np.transpose(Jac)) w, v = np.linalg.eig(A) el.center = (point2[0], point2[1]) el.height = np.sqrt(min(w)) el.width = np.sqrt(max(w)) el.angle = np.degrees(np.arccos(v[1][0])) plt.show()
np.rad2deg(model_iris.model[4])) cE.set_facecolor('None') cE.set_edgecolor((1.0, 0.0, 0.0)) cL.set_facecolor('None') cL.set_edgecolor((0.0, 1.0, 0.0)) cI = plts.imshow(I) cM = plts.imshow(mask_woSkin, alpha=0.5) plts.add_patch(cE) plts.add_patch(cL) plt.show() plt.pause(.01) else: cE.center = tuple(pupil_loc) cE.angle = np.rad2deg(model_pupil.model[4]) cE.width = 2 * model_pupil.model[2] cE.height = 2 * model_pupil.model[3] cL.center = tuple(model_iris.model[0:2]) cL.width = 2 * model_iris.model[2] cL.height = 2 * model_iris.model[3] cL.angle = np.rad2deg(model_iris.model[-1]) cI.set_data(I) cM.set_data(mask_woSkin) mypause(0.01) i = i + 1 print('{} images: {}'.format(dirCond, i)) # Stack data Data['Images'] = np.stack(Data['Images'], axis=0) Data['Masks'] = np.stack(Data['Masks'], axis=0) Data['Masks_noSkin'] = np.stack(Data['Masks_noSkin'], axis=0) Data['pupil_loc'] = np.stack(Data['pupil_loc'], axis=0)
def animate_multiple_predictions_and_goal_likelihood(img, x, y, nUsedData, nGoals, goalsLikelihood, predictedXYVec, varXYVec, toFile): observedX = x[0:nUsedData] observedY = y[0:nUsedData] fig, ax = plt.subplots() plt.gca().set_axis_off() plt.gca().xaxis.set_major_locator(plt.NullLocator()) plt.gca().yaxis.set_major_locator(plt.NullLocator()) # Plot the observed data lineObs, = ax.plot(observedX, observedY, lw=3, color='c') # Show the image ax.imshow(img) plt.tight_layout() # Likelihoods maxLikelihood = max(goalsLikelihood) maxLW = 3.0 # For all potential goals ells = [] pls = [] #ax.set_aspect('equal') for i in range(nGoals): lw = max((goalsLikelihood[i] / maxLikelihood) * maxLW, 1) e = Ellipse([0, 0], 0, 0) e.set_fill(0) if (predictedXYVec[i].shape[0] == 0): l, = ax.plot([0], [0], lw=0, color='b') e.set_lw(0) else: l, = ax.plot(predictedXYVec[i][0, 0], predictedXYVec[i][0, 1], lw=lw, color='b') ## Ellipse center xy = [predictedXYVec[i][0, 0], predictedXYVec[i][0, 1]] vx = varXYVec[i][0, 0, 0] vy = varXYVec[i][1, 0, 0] e.center = xy e.width = 6.0 * math.sqrt(math.fabs(vx)) e.height = 6.0 * math.sqrt(math.fabs(vy)) e.set_edgecolor('b') e.set_lw(lw) ax.add_patch(e) ells.append(e) pls.append(l) v = [0, img.shape[1], img.shape[0], 0] plt.axis(v) def animate(i): for j in range(nGoals): if (predictedXYVec[j].shape[0] == 0): continue predictedN = predictedXYVec[j].shape[0] if (i < predictedN): p = [predictedXYVec[j][i, 0], predictedXYVec[j][i, 1]] vx = varXYVec[j][0, i, i] vy = varXYVec[j][1, i, i] pls[j].set_data(predictedXYVec[j][0:i, 0], predictedXYVec[j][0:i, 1]) ells[j].center = p ells[j].width = 6.0 * math.sqrt(math.fabs(vx)) ells[j].height = 6.0 * math.sqrt(math.fabs(vy)) anim = FuncAnimation(fig, animate, frames=100, interval=100) if toFile: anim.save('test-%d.mp4' % i) else: plt.show() return
def process(args, ds_num, fName): warnings.filterwarnings("error") PATH_IMAGES = os.path.join(PATH_DIR, fName, 'synthetic') PATH_MASK_SKIN = os.path.join(PATH_DIR, fName, 'mask-withskin') PATH_MASK_NOSKIN = os.path.join(PATH_DIR, fName, 'mask-withoutskin-noglasses') imList = glob.glob(os.path.join(PATH_IMAGES, '*.tif')) if not noDisp: fig, plts = plt.subplots(1, 1) Data, keydict = generateEmptyStorage( name='riteyes-{}'.format(args.which_riteyes_set), subset='riteyes-{}_{}'.format(args.which_riteyes_set, fName)) ds_name = 'riteyes-{}_{}_{}'.format(args.which_riteyes_set, fName, ds_num) fr_num = 0 for ele in imList: imName_withext = os.path.split(ele)[1] imName = os.path.splitext(ele)[0] path2im = os.path.join(PATH_IMAGES, imName_withext) path2mask = os.path.join(PATH_MASK_SKIN, imName_withext) path2mask_woskin = os.path.join(PATH_MASK_NOSKIN, imName_withext) try: I = np.asarray(Image.open(path2im).convert('L')) maskIm = np.asarray(Image.open(path2mask).convert('RGB')) maskIm_woskin = np.asarray( Image.open(path2mask_woskin).convert('RGB')) I = cv2.resize(I, (640, 480), interpolation=cv2.INTER_CUBIC) maskIm = cv2.resize(maskIm, (640, 480), interpolation=cv2.INTER_NEAREST) maskIm_woskin = cv2.resize(maskIm_woskin, (640, 480), interpolation=cv2.INTER_NEAREST) maskIm, maskIm_woskin = quantizeMask(maskIm, maskIm_woskin) except: print('Corrupt data found in {}.'.format(ele)) import pdb pdb.set_trace() pupilPts, irisPts = getValidPoints(maskIm_woskin) # Pupil ellipse fit model_pupil = ransac(pupilPts, ElliFit, 15, 40, 5e-3, 15).loop() pupil_fit_error = my_ellipse(model_pupil.model).verify(pupilPts) r, c = np.where(maskIm_woskin == 2) pupil_loc = model_pupil.model[: 2] if pupil_fit_error < 0.05 else np.stack( [np.mean(c), np.mean(r)], axis=0) # Iris ellipse fit model_iris = ransac(irisPts, ElliFit, 15, 40, 5e-3, 15).loop() iris_fit_error = my_ellipse(model_iris.model).verify(irisPts) if (pupil_fit_error > 0.05) | (iris_fit_error > 0.05): print('Skipping: {}'.format(imName)) continue Data['Info'].append(imName) Data['Masks'].append(maskIm) Data['Images'].append(I) Data['pupil_loc'].append(pupil_loc) Data['subject_id'].append(str(fName)) Data['Masks_noSkin'].append(maskIm_woskin) keydict['archive'].append(ds_name) keydict['pupil_loc'].append(pupil_loc) keydict['resolution'].append(I.shape) keydict['subject_id'].append(str(fName)) Data['Fits']['pupil'].append(model_pupil.model) Data['Fits']['iris'].append(model_iris.model) keydict['Fits']['pupil'].append(model_pupil.model) keydict['Fits']['iris'].append(model_iris.model) fr_num += 1 if not noDisp: if fr_num == 1: cE = Ellipse(tuple(pupil_loc), 2 * model_pupil.model[2], 2 * model_pupil.model[3], angle=np.rad2deg(model_pupil.model[-1])) cL = Ellipse(tuple(model_iris.model[0:2]), 2 * model_iris.model[2], 2 * model_iris.model[3], np.rad2deg(model_iris.model[4])) cE.set_facecolor('None') cE.set_edgecolor((1.0, 0.0, 0.0)) cL.set_facecolor('None') cL.set_edgecolor((0.0, 1.0, 0.0)) cI = plts.imshow(I) cM = plts.imshow(maskIm, alpha=0.5) cX = plts.scatter(pupil_loc[0], pupil_loc[1]) plts.add_patch(cE) plts.add_patch(cL) plt.show() plt.pause(.01) else: cE.center = tuple(pupil_loc) cE.angle = np.rad2deg(model_pupil.model[-1]) cE.width = 2 * model_pupil.model[2] cE.height = 2 * model_pupil.model[3] cL.center = tuple(model_iris.model[0:2]) cL.width = 2 * model_iris.model[2] cL.height = 2 * model_iris.model[3] cL.angle = np.rad2deg(model_iris.model[-1]) newLoc = np.array([pupil_loc[0], pupil_loc[1]]) cI.set_data(I) cM.set_data(maskIm) cX.set_offsets(newLoc) mypause(0.01) keydict['resolution'] = np.stack(keydict['resolution'], axis=0) keydict['subject_id'] = np.stack(keydict['subject_id'], axis=0) keydict['archive'] = np.stack(keydict['archive'], axis=0) keydict['pupil_loc'] = np.stack(keydict['pupil_loc'], axis=0) keydict['Fits']['pupil'] = np.stack(keydict['Fits']['pupil'], axis=0) keydict['Fits']['iris'] = np.stack(keydict['Fits']['iris'], axis=0) Data['subject_id'] = np.stack(Data['subject_id'], axis=0) Data['pupil_loc'] = np.stack(Data['pupil_loc'], axis=0) Data['Images'] = np.stack(Data['Images'], axis=0) Data['Masks'] = np.stack(Data['Masks'], axis=0) Data['Masks_noSkin'] = np.stack(Data['Masks_noSkin'], axis=0) Data['Fits']['pupil'] = np.stack(Data['Fits']['pupil'], axis=0) Data['Fits']['iris'] = np.stack(Data['Fits']['iris'], axis=0) warnings.filterwarnings("ignore") # Save data dd.io.save(os.path.join(PATH_DS, str(ds_name) + '.h5'), Data) scio.savemat(os.path.join(PATH_MASTER, str(ds_name) + '.mat'), keydict, appendmat=True) ds_num = ds_num + 1
cI = plts.imshow(I_cropped, cmap='gray') cE = Ellipse(tuple(irisFit.model[:2]), 2 * irisFit.model[2], 2 * irisFit.model[3], angle=np.rad2deg(irisFit.model[-1])) cE.set_facecolor('None') cE.set_edgecolor((0.0, 1.0, 0.0)) cM = plts.imshow(mask, alpha=0.3) plts.add_patch(cE) plt.show() plt.pause(.01) else: cE.center = tuple(irisFit.model[:2]) cE.angle = np.rad2deg(irisFit.model[-1]) cE.width = 2 * irisFit.model[2] cE.height = 2 * irisFit.model[3] cI.set_data(I_cropped) cM.set_data(mask) mypause(0.01) Image_counter = Image_counter + 1 Data['Images'] = np.stack(Data['Images'], axis=0) Data['Masks'] = np.stack(Data['Masks'], axis=0) Data['Masks_noSkin'] = np.stack(Data['Masks_noSkin'], axis=0) Data['pupil_loc'] = np.stack(Data['pupil_loc'], axis=0) keydict['pupil_loc'] = np.stack(keydict['pupil_loc'], axis=0) keydict['resolution'] = np.stack(keydict['resolution'], axis=0) keydict['archive'] = np.stack(keydict['archive'], axis=0) Data['pupil_loc'] = np.stack(Data['pupil_loc'], axis=0) Data['Fits']['iris'] = np.stack(Data['Fits']['iris'], axis=0)