def draw_person_top(x, y, angle, ax): """Draws a persons from a top view.""" top_y = HUMAN_Y / 2 top_x = HUMAN_X / 2 plot_ellipse(semimaj=top_x, semimin=top_y, phi=angle, x_cent=x, y_cent=y, ax=ax)
def plot_person(x, y, angle, ax, plot_kwargs): """ Plots a person from a top view.""" r = 10 # or whatever fits you ax.arrow(x, y, r * math.cos(angle), r * math.sin(angle), head_length=1, head_width=1, shape='full', color='blue') ax.plot(x, y, 'bo', markersize=8) top_y = HUMAN_Y / 2 top_x = HUMAN_X / 2 plot_ellipse(semimaj=top_x, semimin=top_y, phi=angle, x_cent=x, y_cent=y, ax=ax)
def draw_personalspace(x, y, angle, ax, sx, sy, plot_kwargs, idx): """Draws personal space of an inidivdual and it.""" draw_arrow(x, y, angle, ax) # orientation arrow angle in radians ax.plot(x, y, 'bo', markersize=8) draw_person_top(x, y, angle, ax) ax.text(x + 10, y + 10, "$P_" + str(idx) + "$", fontsize=12) plot_ellipse(semimaj=sx, semimin=sy, phi=angle, x_cent=x, y_cent=y, ax=ax, plot_kwargs=plot_kwargs) # Multiply sx and sy by 2 to get the diameter return patches.Ellipse((x, y), sx * 2, sy * 2, math.degrees(angle))
def plot_person(x, y, angle, ax, plot_kwargs): """ Plots a person from a top view.""" draw_arrow(x, y, angle, ax) ax.plot(x, y, 'bo', markersize=8) top_y = HUMAN_Y / 2 top_x = HUMAN_X / 2 plot_ellipse(semimaj=top_x, semimin=top_y, phi=angle, x_cent=x, y_cent=y, ax=ax)
def plot_robot(pose, ax): """Draws a robot from a top view.""" x = pose[0] y = pose[1] angle = pose[2] top_y = HUMAN_Y / 2 top_x = HUMAN_Y / 2 plot_kwargs = {'color': 'black', 'linestyle': '-', 'linewidth': 1} plot_ellipse(semimaj=top_x, semimin=top_y, phi=angle, x_cent=x, y_cent=y, ax=ax, plot_kwargs=plot_kwargs) draw_arrow(x, y, angle, ax) # orientation arrow angle in radians ax.plot(x, y, 'o', color='black', markersize=5)
def getMatrixDifferenceEllipsePoints(cell,targetface, diffmatrix,primordialfaceid = 135,zoomfactor=1.): #getting the Target Form Matrix faces = qd.CellFaceIterator(cell) face = faces.next() while True: if face.getID() == targetface: break face = faces.next() #print "Face Id : ", face.getID() targetformmatrix = diffmatrix unitx = face.getUnitx() unity = face.getUnity() unitz = face.getUnitz() unit_mat = np.matrix([[qd.doublearray_getitem(unitx,0),qd.doublearray_getitem(unitx,1),qd.doublearray_getitem(unitx,2)], [qd.doublearray_getitem(unity,0),qd.doublearray_getitem(unity,1),qd.doublearray_getitem(unity,2)], [qd.doublearray_getitem(unitz,0),qd.doublearray_getitem(unitz,1),qd.doublearray_getitem(unitz,2)]]) #transposing unitmatrix transpose_unitmat = np.matrix(np.transpose(unit_mat)) #Getting Centroid of face xcent = face.getXCentralised() ycent = face.getYCentralised() zcent = face.getZCentralised() ##### getting data from ellipse & getting transformed coordinate to 3d Cartesian data = ep.plot_ellipse(cov=targetformmatrix, data_out=True,norm = True) points = zoomfactor*np.matrix(np.vstack((data,np.zeros(len(data[0]))))) transformedpoints = transpose_unitmat*points transformedpoints[0]+= xcent transformedpoints[1]+= ycent transformedpoints[2]+= zcent return transformedpoints
def approaching_heuristic(group_radius, pspace_radius, group_pos, approaching_filter, contour_points, approaching_zones): """ """ approaching_radius = group_radius approaching_radius += R_STEP if not approaching_filter: while not approaching_filter and approaching_radius <= pspace_radius: approaching_area = None approaching_filter = None approaching_zones = None approaching_area = plot_ellipse(semimaj=approaching_radius, semimin=approaching_radius, x_cent=group_pos[0], y_cent=group_pos[1], data_out=True) approaching_filter, approaching_zones = approaching_area_filtering( approaching_area, contour_points) approaching_radius += R_STEP return approaching_filter, approaching_zones
def plot_group(group_pose, group_radius, pspace_radius, ospace_radius, ax, persons, sx, sy): """Plots o-space, p-space, group center and approaching area.""" # O Space Modeling ax.plot(group_pose[0], group_pose[1], 'rx', markersize=8) plot_kwargs = {'color': 'r', 'linestyle': '-', 'linewidth': 1} plot_ellipse(semimaj=ospace_radius, semimin=ospace_radius, x_cent=group_pose[0], y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs) # P Space Modeling plot_ellipse(semimaj=pspace_radius, semimin=pspace_radius, x_cent=group_pose[0], y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs) # approaching circle area plot_kwargs = {'color': 'c', 'linestyle': ':', 'linewidth': 2} plot_ellipse(semimaj=group_radius, semimin=group_radius, x_cent=group_pose[0], y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs) approaching_area = plot_ellipse(semimaj=group_radius, semimin=group_radius, x_cent=group_pose[0], y_cent=group_pose[1], data_out=True) # Possible approaching area computation and personal space ploting plot_kwargs = {'color': 'g', 'linestyle': '-', 'linewidth': 0.8} approaching_filter = approaching_area for idx, person in enumerate(persons, start=1): shapely_diff_sy = 0.5 # Error between python modules used shapely_diff_sx = 1 personal_space = draw_personalspace( person[0], person[1], person[2], ax, sx - shapely_diff_sx, sy - shapely_diff_sy, plot_kwargs, idx) # plot using ellipse.py functions # Approaching Area filtering - remove points that are inside the personal space of a person approaching_filter = approachingfiltering_ellipses( personal_space, approaching_filter, idx) # possible approaching positions approaching_x = [j[0] for j in approaching_filter] approaching_y = [k[1] for k in approaching_filter] ax.plot(approaching_x, approaching_y, 'c.', markersize=5)
def plot_group(group_pose, group_radius, pspace_radius, ospace_radius, ax): """Plots the group o space, p space and approaching circle area. """ # O Space Modeling ax.plot(group_pose[0], group_pose[1], 'rx', markersize=8) plot_kwargs = {'color': 'r', 'linestyle': '-', 'linewidth': 1} plot_ellipse(semimaj=ospace_radius, semimin=ospace_radius, x_cent=group_pose[0], y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs) # P Space Modeling plot_ellipse(semimaj=pspace_radius, semimin=pspace_radius, x_cent=group_pose[0], y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs) # approaching circle area plot_kwargs = {'color': 'c', 'linestyle': ':', 'linewidth': 2} plot_ellipse(semimaj=group_radius, semimin=group_radius, x_cent=group_pose[0], y_cent=group_pose[1], ax=ax, plot_kwargs=plot_kwargs) approaching_area = plot_ellipse(semimaj=group_radius, semimin=group_radius, x_cent=group_pose[0], y_cent=group_pose[1], data_out=True) return approaching_area
blobs_log = blob_log(data, max_sigma=3, num_sigma=3, threshold=.1) y, x, sigma = blobs_log.T plt.show() pts = np.vstack((x, y)).transpose() # cluster points radius = 100 import time t0 = time.time() labels = clusterPts(pts, radius, num_clusters=10) t1 = time.time() - t0 for lbl in labels: ell = pts[lbl, :].astype(float) shift = ell.min(axis=0) scale = ell.max() pp = (ell - shift) / scale if len(pp) > 3: p, _ = ellipse.fit_ellipse(pp[:, 0], pp[:, 1]) xx, yy = ellipse.plot_ellipse(p) xx = xx * scale + shift[0] yy = yy * scale + shift[1] plt.imshow(data) plt.scatter(ell[:, 0], ell[:, 1]) plt.plot(xx, yy) plt.show() print('num of clusters = %d' % len(labels)) print('time taken = %f' % t1)
ax.scatter(data_all[pid][0:20, 0], data_all[pid][0:20, 1], s=20, color='blue', alpha=alpha, marker='o') # Plot generated exemplars ax.scatter(data_all[pid][20:, 0], data_all[pid][20:, 1], s=20, color='red', alpha=alpha, marker='x') # Plot the ellipses plot_ellipse(ax, ms_post, ss_post) # Standardize axes lim = 2.5 ax.get_xaxis().set_visible(False) ax.get_yaxis().set_visible(False) ax.set_ylim(-lim, lim) ax.set_xlim(-lim, lim) ax.set_title(pid) # Save the figure for easy future access plt.savefig('real_fit%d.pdf' % ki) ## Model comparison #Convert model and trace into dictionary pairs dict_pairs = dict(zip(gmm_all, traces)) #Perform WAIC comparison
def run_behavior(self): """ """ while not rospy.is_shutdown(): if self.people_received and self.groups: self.people_received = False if self.costmap_received and self.groups: self.costmap_received = False # # Choose the nearest pose dis = 0 for idx, group in enumerate(self.groups): # Choose the nearest group dis_aux = euclidean_distance(group["pose"][0], group["pose"][1], self.pose[0], self.pose[1]) if idx == 0: goal_group = group dis = dis_aux elif dis > dis_aux: goal_group = group dis = dis_aux # Meter algures uma condicap que verifica que se nao for possivel aproximar o grupo escolher outro #Tentar plotar costmap # Choose the nearest group group = goal_group g_radius = group["g_radius"] # Margin for safer results pspace_radius = group["pspace_radius"] ospace_radius = group["ospace_radius"] approaching_area = plot_ellipse(semimaj=g_radius, semimin=g_radius, x_cent=group["pose"][0], y_cent=group["pose"][1], data_out=True) approaching_filter, approaching_zones = approaching_area_filtering( approaching_area, self.costmap) approaching_filter, approaching_zones = approaching_heuristic( g_radius, pspace_radius, group["pose"], approaching_filter, self.costmap, approaching_zones) center_x, center_y, orientation = zones_center( approaching_zones, group["pose"], g_radius) approaching_poses = [] for l, _ in enumerate(center_x): approaching_poses.append( (center_x[l], center_y[l], orientation[l])) fig = plt.figure() ax = fig.add_subplot(1, 1, 1) plot_kwargs = { 'color': 'g', 'linestyle': '-', 'linewidth': 0.8 } for person in group["members"]: plot_person(person[0], person[1], person[2], ax, plot_kwargs) _ = plot_group(group["pose"], g_radius, pspace_radius, ospace_radius, ax) for i, angle in enumerate(orientation): draw_arrow(center_x[i], center_y[i], angle, ax) x_approach = [j[0] for j in approaching_filter] y_approach = [k[1] for k in approaching_filter] ax.plot(x_approach, y_approach, 'c.', markersize=5) ax.plot(center_x, center_y, 'r.', markersize=5) ax.set_aspect(aspect=1) #fig.tight_layout() plt.show() # Verificar se zonas de aproximacao estao vazias, se estiverm escolher outro grupo ou fazer break if approaching_zones: len_areas = [] for zone in approaching_zones: len_areas.append(len(zone)) # Choose the pose in the biggest approaching area idx = len_areas.index(max(len_areas)) goal_pose = approaching_poses[idx][0:2] goal_quaternion = tf.transformations.quaternion_from_euler( 0, 0, approaching_poses[idx][2]) try: rospy.loginfo("Approaching group!") result = movebase_client(goal_pose, goal_quaternion) if result: rospy.loginfo("Goal execution done!") break except rospy.ROSInterruptException: rospy.loginfo("Navigation test finished.") else: rospy.loginfo("Impossible to approach group.")
def run_behavior(self): """ """ while not rospy.is_shutdown(): if self.people_received and self.groups: self.people_received = False if self.costmap_up_received and self.groups: self.costmap_up_received = False # # Choose the nearest pose dis = 0 for idx, group in enumerate(self.groups): # Choose the nearest group dis_aux = euclidean_distance(group["pose"][0], group["pose"][1], self.pose[0], self.pose[1]) if idx == 0: goal_group = group dis = dis_aux elif dis > dis_aux: goal_group = group dis = dis_aux # Meter algures uma condicap que verifica que se nao for possivel aproximar o grupo escolher outro #Tentar plotar costmap # Choose the nearest group group = goal_group g_radius = group["g_radius"] #Margin to for safer results pspace_radius = group["pspace_radius"] ospace_radius = group["ospace_radius"] approaching_area = plot_ellipse(semimaj=g_radius, semimin=g_radius, x_cent=group["pose"][0], y_cent=group["pose"][1], data_out=True) approaching_filter, approaching_zones = approaching_area_filtering( approaching_area, self.costmap) approaching_filter, approaching_zones = approaching_heuristic( g_radius, pspace_radius, group["pose"], approaching_filter, self.costmap, approaching_zones) ################################################################################################################################# #Approaching area marker marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE_LIST marker.action = marker.ADD marker.pose.orientation.w = 1 for point_f in approaching_filter: m_point = Point() m_point.x = point_f[0] m_point.y = point_f[1] m_point.z = 0.1 marker.points.append(m_point) t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.03 marker.scale.y = 0.03 marker.scale.z = 0.03 marker.color.a = 1.0 marker.color.r = 1.0 self.pubm.publish(marker) ####################################################################################### # Approaching pose Pose Array ap_points = PoseArray() ap_points.header.frame_id = "/map" ap_points.header.stamp = rospy.Time.now() center_x, center_y, orientation = zones_center( approaching_zones, group["pose"], g_radius) approaching_poses = [] for l, _ in enumerate(center_x): approaching_poses.append( (center_x[l], center_y[l], orientation[l])) ap_pose = Pose() ap_pose.position.x = center_x[l] ap_pose.position.y = center_y[l] ap_pose.position.z = 0.1 quaternion = tf.transformations.quaternion_from_euler( 0, 0, orientation[l]) ap_pose.orientation.x = quaternion[0] ap_pose.orientation.y = quaternion[1] ap_pose.orientation.z = quaternion[2] ap_pose.orientation.w = quaternion[3] ap_points.poses.append(ap_pose) self.pubg.publish(ap_points)
z[:, np.newaxis] * np.random.multivariate_normal(m, s, size=N) for z, m, s in zip(zs, mus, sigmas) ] # Stack data into single array data = np.sum(np.dstack(xs), axis=2) ## Plot them nicely # Prepare subplots fig, ax = plt.subplots(figsize=(8, 6)) # First, scatter plt.scatter(data[:, 0], data[:, 1], c='g', alpha=0.5) plt.scatter(mus[0, 0], mus[0, 1], c='r', s=100) plt.scatter(mus[1, 0], mus[1, 1], c='b', s=100) plt.scatter(mus[2, 0], mus[2, 1], c='y', s=100) # Then, ellipses plot_ellipse(ax, mus, sigmas) ax.axis('equal') plt.show() ## Build model and sample # Number of iterations for sampler draws = 2000 # Prepare lists of starting points for mu to prevent label-switching problem testvals = [[-2, -2], [0, 0], [2, 2]] # Model structure with pm.Model() as mvgmm: # Prior over component weights p = pm.Dirichlet('p', a=np.array([1.] * K)) # Prior over component means