def get_nearest_tag(self, x, y): nt = None dt = 10000000000000000000 nx, ny = 0, 0 mymap = self.get_map() for p in self.points: xp, yp = mymap.to_xy([p.x, p.y]) if (get_euclidean_distance([xp, yp], [x, y]) < dt): nt = p.tag dt = get_euclidean_distance([xp, yp], [x, y]) nx, ny = xp, yp for p in self.polygons: p.tag xp, yp = mymap.to_xy([mean(p.X), mean(p.Y)]) for i in range(len(p.X)): xp, yp = mymap.to_xy([p.X[i], p.Y[i]]) if (get_euclidean_distance([xp, yp], [x, y]) < dt): nt = p.tag dt = get_euclidean_distance([xp, yp], [x, y]) nx, ny = xp, yp return nt, [nx, ny]
def get_visible_polygons_orient(self, x, y, rtheta, fov, max_dist=5.0): polygons = self.polygons mymap = self.get_map() visible_polygons = [] invisible_polygons = [] for polygon in polygons: seen = False for i in range(polygon.num_segments()): xi, yi = polygon.get_segment(i) xp, yp = mymap.to_xy([xi, yi]) theta = atan2(yp - y, xp - x) if (abs(tklib_normalize_theta(theta - rtheta)) > fov / 2.0): continue d, = mymap.ray_trace(x, y, [theta]) #d_elt = sqrt((y-yp)**2.0 + (x-xp)**2.0) d_elt = get_euclidean_distance([x, y], [xp, yp]) if (d_elt < d and d_elt < max_dist): visible_polygons.append(polygon) seen = True break elif (abs(d - d_elt) < 0.3 and d_elt < max_dist + 0.3): visible_polygons.append(polygon) seen = True break if (not seen): invisible_polygons.append(polygon) return visible_polygons, invisible_polygons
def get_visible_points_orient(self, x, y, rtheta, fov, max_dist=5.0): visible_pts = [] invisible_pts = [] pts = self.points mymap = self.get_map() for pt in pts: xp, yp = mymap.to_xy([pt.x, pt.y]) theta = atan2(yp - y, xp - x) #print pt.tag, " th:", theta, " rtheta:", rtheta #print "normalized", abs(tklib_normalize_theta(theta - rtheta)) if (abs(tklib_normalize_theta(theta - rtheta)) >= fov / 2.0): continue #this probably just made a bug d, = mymap.ray_trace(x, y, [theta]) #d_elt = sqrt((y-yp)**2.0 + (x-xp)**2.0) d_elt = get_euclidean_distance([x, y], [xp, yp]) if (d_elt < d and d_elt < max_dist): visible_pts.append(pt) elif (abs(d - d_elt) < 0.3 and d_elt < max_dist + 0.3): visible_pts.append(pt) else: invisible_pts.append(pt) return visible_pts, invisible_pts
def get_graph(self): """ Returns a dictionary, and an array of indices. Keys are indices in get_skeleton_indices. Values are a map whose keys are connected indices, and values are the distance between the two indices. The graph contains a node for every point in the skeleton, as a grid map. Use get_junction_points to get the junctions. """ G = {} I = array(self.get_skeleton_indices()) index_to_num_hash = self.get_skeleton_indices_index() for i in range(len(I[0])): G[i] = {} neighs = self.get_neighbors(I[0, i], I[1, i]) if (len(neighs) == 0): continue for j in range(len(neighs[0])): myi, myj = neighs[:, j] G[i][index_to_num_hash[str([myi, myj])]] = get_euclidean_distance( I[:, i], [myi, myj]) return G, I
def spectral_gradient_descent(X, alpha, threshold=0.1, iterations=10000000, g_rot_init=None): done = False g_rot = None if (g_rot_init != None): init_thetas = g_rot_init.toThetaArry().tolist() init_thetas.append(0.0) #get the initial givens rotation g_rot = GivensRotation(len(X[0]), init_thetas) else: g_rot = GivensRotation(len(X[0])) #initialize the current and previous thetas prev_thetas = g_rot.toThetaArry() i = 0 #print "***************************" #print "gettting number of clusters" while (1): #convert to an array curr_thetas = g_rot.toThetaArry() prev_thetas = curr_thetas #update the current thetas #print "jacobian", get_jacobian(X, g_rot) #print "curr_thetas", curr_thetas #print "thetas", curr_thetas J = get_jacobian(X, g_rot) #print "jacobian", J curr_thetas = curr_thetas - alpha * J #print "new_thetas", curr_thetas #plug it back into the given rotation g_rot.fromThetaArry(curr_thetas) #see if we are at our finishing condition #print "diff", (curr_thetas-prev_thetas)**2 d = get_euclidean_distance(curr_thetas, prev_thetas) #print "d:", d if (d < threshold): print "threadhold broken, exiting" break if (i > iterations): print "spectral_gradient_descent did not converge" break i += 1 return g_rot
def perpendicular_distance_arry(self, pt): if (self.m == 0): m2 = sys.maxint else: m2 = -1 * (1 / (self.m * 1.0)) b2 = pt[1] - (m2 * pt[0]) x = (b2 - self.b) / (1.0 * self.m - m2) y = self.m * x + self.b return carmen_util.get_euclidean_distance([x, y], pt)
def is_visible(self, p1, p2, max_dist=5.0, edist=None): d_elt = None if (edist != None): d_elt = edist else: d_elt = get_euclidean_distance(p1, p2) if (d_elt >= max_dist + 0.3): return False x, y = p1 xp, yp = p2 theta = atan2(yp - y, xp - x) mymap = self.get_map() d, = mymap.ray_trace(x, y, [theta]) if (d_elt < d and d_elt < max_dist): return True elif (abs(d - d_elt) < 0.3 and d_elt < max_dist + 0.3): return True else: return False
def min_dist(self, pt): return get_euclidean_distance(pt, [self.x, self.y])
def min_dist(self, pt): from pyTklib import NN return get_euclidean_distance(pt, NN(pt, [self.X, self.Y]))