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]))
