def is_loc_visible_from_pose(loc_st, theta_st, loc_end, fov=None):
"""
Returns true if loc is visible from start_loc, start_theta with the given field of view
"""
abs_orient = atan2(loc_end[1] - loc_st[1], loc_end[0] - loc_st[0])
if math2d.dist(loc_st, loc_end) < 0.00000001:
return True
elif (0 >= tklib_normalize_theta(theta_st - fov / 2.0 - abs_orient)
and 0 <= tklib_normalize_theta(theta_st + fov / 2.0 - abs_orient)):
return True
else:
return False
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 tklib_rotate_pts_match_pose(model_pose, measured_pose, measured_pts):
th_diff = tklib_normalize_theta(model_pose.theta - measured_pose.theta)
R = array([[cos(th_diff), -sin(th_diff)], [sin(th_diff), cos(th_diff)]])
new_pts = dot(R,
measured_pts - array([[measured_pose.x], [measured_pose.y]]))
new_pts += array([[model_pose.x], [model_pose.y]])
return new_pts
def get_total_turn_amount(x1, y1, th1, x2, y2, th2, debug=False):
theta = atan2(y2 - y1, x2 - x1)
total_turn = abs(tklib_normalize_theta(th1 - theta)) + abs(
tklib_normalize_theta(th2 - theta))
if debug:
print "theta", math.degrees(theta)
print "totalturn", math.degrees(total_turn)
print "x1,y1,th1", x1, y1, math.degrees(th1)
print "x2,y2,th2", x2, y2, math.degrees(th2)
if x1 == x2 and y1 == y2 and th1 == th2:
total_turn = 0
direction = None
if (abs(tklib_normalize_theta(theta - th1)) > abs(
tklib_normalize_theta(th2 - theta))):
direction = sign(tklib_normalize_theta(theta - th1))
else:
direction = sign(tklib_normalize_theta(th2 - theta))
return total_turn, direction
def get_transition_matrix(self, direction="straight", p_self=1.0):
"""
Use children and grandchildren
"""
T_mat = zeros([
self.num_regions * self.num_viewpoints,
self.num_regions * self.num_viewpoints
]) * 1.0
if (direction == "straight"):
new_tmap = self.get_topological_map_viewpoint(pi, 0)
elif (direction == "right"):
new_tmap = self.get_topological_map_viewpoint(pi, -1.0 * pi / 2.0)
elif (direction == "left"):
new_tmap = self.get_topological_map_viewpoint(pi, +1.0 * pi / 2.0)
#for each of the viewpoints
for i in range(len(self.viewpoints)):
connections = new_tmap[self.viewpoints[i]]
#find the areas connected and if they are reasonable
for conn in connections:
cconn = conn
connofconn = [conn]
connofconn.extend(new_tmap[conn])
#if(i==0):
# print "start viewpoint:", self.viewpoints[i]
# print "tmap:", self.tmap[0.0], "to", self.tmap_locs[11.0]
# print "connections", connofconn
# raw_input()
for conn_i, cconn in enumerate(connofconn):
#convert this into topo numbers
topo_st, topo_st_ang = self.viewpoints[i].split("_")
topo_end, topo_end_ang = cconn.split("_")
if (cconn < 0 or self.viewpoints[i] == -1):
continue
elif (topo_st == topo_end):
continue
loc1 = self.tmap_locs[float(topo_st)]
loc2 = self.tmap_locs[float(topo_end)]
#for each of the to-node's orientations, check its relative angle to
# the from node's orientation and give it a relative probability
# accordingly
start_num = self.vpt_to_num[self.viewpoints[i]]
end_num = self.vpt_to_num[cconn]
#make this dependent on how much we turn in total
orient_diff, d_turn = get_total_turn_amount(
loc1[0], loc1[1], radians(float(topo_st_ang)), loc2[0],
loc2[1], radians(float(topo_end_ang)))
#going to end_num from i
if (direction == 'straight'):
T_mat[end_num, start_num] = max(
-1.75 / pi * abs(orient_diff) + 1, 0.0)
#T_mat[start_num][end_num] = max(-1.5/pi*abs(orient_diff)+1, 0.0)
elif (direction == 'right' and d_turn < 0):
diff_from_neg_pi2 = tklib_normalize_theta(d_turn *
orient_diff +
(pi / 2.0))
T_mat[end_num, start_num] = max(
-2.5 / pi * abs(diff_from_neg_pi2) + 1, 0.0)
#T_mat[start_num][end_num] = max(-1.5/pi*abs(diff_from_neg_pi2)+1, 0.0)
elif (direction == 'left' and d_turn > 0):
diff_from_pi2 = tklib_normalize_theta(d_turn *
orient_diff -
(pi / 2.0))
T_mat[end_num, start_num] = max(
-2.5 / pi * abs(diff_from_pi2) + 1, 0.0)
#T_mat[start_num][end_num] = max(-1.5/pi*abs(diff_from_pi2)+1, 0.0)
if (not conn == cconn):
T_mat[end_num,
start_num] = T_mat[end_num, start_num] * 0.3
#connect self transitions
topo_st, topo_st_ang = self.viewpoints[i].split("_")
if (direction == "straight"):
T_mat[i, i] = p_self
elif (direction == "right"):
newang = mod(
int(float(topo_st_ang)) -
int(float(self.get_viewpoint_diff())), 360) * 1.0
T_mat[self.vpt_to_num[str(topo_st) + "_" + str(newang)],
i] = p_self
elif (direction == "left"):
newang = mod(
int(float(topo_st_ang)) +
int(float(self.get_viewpoint_diff())), 360.0) * 1.0
T_mat[self.vpt_to_num[str(topo_st) + "_" + str(newang)],
i] = p_self
return T_mat
def normalize_theta(theta):
return tklib_normalize_theta(theta)
def get_topological_map_viewpoint(self, fov, turn_amount):
tmap_new = {}
#iterate through all the viewpoints
for i in range(len(self.viewpoints)):
#get the start location
tf, tfor = self.viewpoints[i].split("_")
tf = float(tf)
loc_st = self.tmap_locs[tf]
st_orient = float(tfor) * pi / 180.0
tmap_new[self.viewpoints[i]] = []
#iterate through all the viewpoints
for j in range(len(self.viewpoints)):
#get the end location
tt, ttor = self.viewpoints[j].split("_")
tt = float(tt)
loc_end = self.tmap_locs[tt]
end_orient = float(ttor) * pi / 180.0
#absolute difference in orientation
abs_orient = atan2(loc_end[1] - loc_st[1],
loc_end[0] - loc_st[0])
#if we're connected and we're in the fov
if i == 40 and j == 42 and True:
print i, j
print "vp1", self.viewpoints[i]
print "vp2", self.viewpoints[j]
print "l1", loc_st, "=>", loc_end
print
print "tt", tt in self.tmap[tf]
if fov != None:
print "fov/2.0", math.degrees(fov / 2.0)
print "c1", (0 >= tklib_normalize_theta(
st_orient - fov / 2.0 - abs_orient + turn_amount))
print "c2", (0 <= tklib_normalize_theta(
st_orient + fov / 2.0 - abs_orient + turn_amount))
print "c1 angle", math.degrees(st_orient - fov / 2.0 -
abs_orient +
turn_amount)
print "c2 angle", math.degrees(st_orient + fov / 2.0 -
abs_orient +
turn_amount)
print "normalized"
print "c1 angle", tklib_normalize_theta(
math.degrees(st_orient - fov / 2.0 - abs_orient +
turn_amount))
print "c2 angle", tklib_normalize_theta(
math.degrees(st_orient + fov / 2.0 - abs_orient +
turn_amount))
else:
print "fov is None"
print
print "st_orient", math.degrees(st_orient)
print "abs_orient", abs_orient
print "turn_amount", turn_amount
print
if tt in self.tmap[tf]:
append = False
if fov == None:
append = True
elif (0 >= tklib_normalize_theta(st_orient - fov / 2.0 -
abs_orient + turn_amount)
and 0 <=
tklib_normalize_theta(st_orient + fov / 2.0 -
abs_orient + turn_amount)):
append = True
elif math2d.dist(loc_st, loc_end) < 0.00001:
append = True
if append:
tmap_new[self.viewpoints[i]].append(self.viewpoints[j])
return tmap_new
def get_transition_matrix(self, direction="straight", p_self=1.0):
T_mat = zeros([
self.num_regions * self.num_viewpoints,
self.num_regions * self.num_viewpoints
]) * 1.0
print
print "direction", direction
if (direction == "straight"):
new_tmap = self.get_topological_map_viewpoint(pi, 0)
elif (direction == "back"):
new_tmap = self.get_topological_map_viewpoint(pi, pi)
elif (direction == "right"):
new_tmap = self.get_topological_map_viewpoint(pi, -1.0 * pi / 2.0)
elif (direction == "left"):
new_tmap = self.get_topological_map_viewpoint(pi, +1.0 * pi / 2.0)
elif (direction == "face"):
new_tmap = self.get_topological_map_viewpoint(None, 0)
else:
raise ValueError("Unexpected direction: " + ` direction `)
#for each of the viewpoints
for i in range(len(self.viewpoints)):
connections = new_tmap[self.viewpoints[i]]
#find the areas connected and if they are reasonable
for conn in connections:
cconn = conn
#connofconn = [conn]
#connofconn.extend(new_tmap[conn])
#for conn_i, cconn in enumerate(connofconn):
#convert this into topo numbers
topo_st, topo_st_ang = self.viewpoints[i].split("_")
topo_end, topo_end_ang = cconn.split("_")
if (cconn < 0 or self.viewpoints[i] == -1):
continue
loc1 = self.tmap_locs[float(topo_st)]
loc2 = self.tmap_locs[float(topo_end)]
#for each of the to-node's orientations, check its relative angle to
# the from node's orientation and give it a relative probability
# accordingly
start_num = self.vpt_to_num[self.viewpoints[i]]
end_num = self.vpt_to_num[cconn]
#make this dependent on how much we turn in total
debug = False
if (start_num == 18
or start_num == 16) and end_num == 42 and False:
print "***************", start_num, end_num
debug = True
orient_diff, d_turn = get_total_turn_amount(
loc1[0],
loc1[1],
radians(float(topo_st_ang)),
loc2[0],
loc2[1],
radians(float(topo_end_ang)),
debug=debug)
#going to end_num from i
if (direction == 'straight'):
if debug:
print "val", max(-1.75 / pi * abs(orient_diff) + 1,
0.0)
T_mat[end_num, start_num] = -abs(orient_diff)
T_mat[end_num, start_num] = sigmoid(T_mat[end_num,
start_num])
elif (direction == 'back'):
diff_from_pi = tklib_normalize_theta(d_turn * orient_diff -
pi)
T_mat[end_num, start_num] = -abs(diff_from_pi)
T_mat[end_num, start_num] = sigmoid(T_mat[end_num,
start_num])
#T_mat[start_num][end_num] = max(-1.5/pi*abs(orient_diff)+1, 0.0)
elif (direction == 'right' and d_turn < 0):
diff_from_neg_pi2 = tklib_normalize_theta(d_turn *
orient_diff +
(pi / 2.0))
T_mat[end_num, start_num] = -abs(diff_from_neg_pi2)
T_mat[end_num, start_num] = sigmoid(T_mat[end_num,
start_num])
#T_mat[start_num][end_num] = max(-1.5/pi*abs(diff_from_neg_pi2)+1, 0.0)
elif (direction == 'left' and d_turn > 0):
diff_from_pi2 = tklib_normalize_theta(d_turn *
orient_diff -
(pi / 2.0))
T_mat[end_num, start_num] = -abs(diff_from_pi2)
T_mat[end_num, start_num] = sigmoid(T_mat[end_num,
start_num])
elif direction == "face":
if math2d.dist(loc1, loc2) < 0.01:
T_mat[end_num, start_num] = p_self
else:
T_mat[end_num, start_num] = 0
assert 0 <= T_mat[end_num, start_num] <= 1, T_mat[end_num,
start_num]
#T_mat[start_num][end_num] = max(-1.5/pi*abs(diff_from_pi2)+1, 0.0)
#if(not conn == cconn):
# T_mat[end_num,start_num] = T_mat[end_num,start_num]*0.3
#connect self transitions
topo_st, topo_st_ang = self.viewpoints[i].split("_")
if (direction == "straight"):
T_mat[i, i] = p_self
elif (direction == "right"):
newang = mod(
int(float(topo_st_ang)) -
int(float(self.get_viewpoint_diff())), 360.0) * 1.0
T_mat[self.vpt_to_num[str(topo_st) + "_" + str(newang)],
i] = p_self
elif (direction == "left"):
newang = mod(
int(float(topo_st_ang)) +
int(float(self.get_viewpoint_diff())), 360.0) * 1.0
T_mat[self.vpt_to_num[str(topo_st) + "_" + str(newang)],
i] = p_self
elif (direction == "back"):
newang = mod(int(float(topo_st_ang)) + 180, 360) * 1.0
T_mat[self.vpt_to_num[str(topo_st) + "_" + str(newang)],
i] = p_self
return T_mat
