def get_first_point(point, kdt, X):
"""
match point to nearest segment
:param point: point to be matched
:param kdt: kdtree
:param X:
:return:
"""
dist, ind = kdt.query([point], k=30)
pts = []
seg_set = set()
for i in ind[0]:
pts.append([X[i][0], X[i][1]])
node_id = nodeid_list[i]
edge_list = map_node_dict[node_id].link_list
for e, nd in edge_list:
seg_set.add(e.edge_index)
min_dist, sel = 1e20, -1
for idx in seg_set:
n0, n1 = map_edge_list[idx].nodeid0, map_edge_list[idx].nodeid1
p0, p1 = map_node_dict[n0].point, map_node_dict[n1].point
dist = point2segment(point, p0, p1)
if min_dist > dist:
min_dist, sel = dist, idx
sel_edge = map_edge_list[sel]
sel_node0, sel_node1 = sel_edge.nodeid0, sel_edge.nodeid1
x0, y0 = map_node_dict[sel_node0].point[0:2]
x1, y1 = map_node_dict[sel_node1].point[0:2]
x, y = point[0:2]
rx, ry, _ = point_project(x, y, x0, y0, x1, y1)
return rx, ry, sel_edge
def _get_mod_point_later(candidate, point, last_point, cnt):
"""
:param candidate:
:param point: current position point
:param last_point: last position point
:return: project_point, sel_edge, score
"""
min_score, sel_edge = 1e10, None
for edge in candidate:
p0, p1 = edge.node0.point, edge.node1.point
w0, w1 = 1.0, 10.0
# 加权计算分数，考虑夹角的影响
dist = point2segment(point, p0, p1)
angle = calc_included_angle(last_point, point, p0, p1)
if not edge.oneway and angle < 0:
angle = -angle
score = w0 * dist + w1 * (1 - angle)
if score < min_score:
min_score, sel_edge = score, edge
# if cnt == 147:
# print edge.edge_index, dist, score, angle
if sel_edge is None:
return None, None, 0
project_point, _, state = point_project(point, sel_edge.node0.point, sel_edge.node1.point)
if state == 1:
# 点落在线段末端外
project_point = sel_edge.node1.point
elif state == -1:
project_point = sel_edge.node0.point
return project_point, sel_edge, min_score
def get_dyn_points_later(self, candidate, point, last_point, cnt):
point_list = []
for edge in candidate:
p0, p1 = edge.node0.point, edge.node1.point
w0, w1 = 1.0, 10.0
# 加权计算分数，考虑夹角的影响
dist = point2segment(point, p0, p1)
angle = calc_included_angle_math(last_point, point, p0, p1)
if not edge.oneway and angle < 0:
angle = -angle
score = w0 * dist + w1 * (1 - angle)
# if cnt == 10:
# print edge.edge_index, dist, score, angle
if angle < 0:
continue
if score < 100:
project_point, _, state = point_project(point, edge.node0.point, edge.node1.point)
if state == 1:
# 点落在线段末端外
project_point = edge.node1.point
elif state == -1:
project_point = edge.node0.point
ms = MatchState(edge, project_point, dist, score)
point_list.append(ms)
return point_list
def get_mod_points(taxi_data, candidate, last_point, last_edge, cnt=-1):
"""
get all fit points
:param taxi_data:
:param candidate:
:param last_point: last gps point
:param last_edge: last matched edge
:param cnt:
:return: edge, mod_point, dist, score
"""
bt = clock()
point = [taxi_data.px, taxi_data.py]
edge_list, mod_list, dist_list, score_list = [], [], [], []
for edge in candidate:
score, dist = get_score(point, last_point, edge)
# s2, d2 = get_st_score(point, last_point, edge, last_edge)
edge_list.append(edge)
dist_list.append(dist)
score_list.append(score)
# 以下代码作废
state_list = []
in_road = False
for edge in edge_list:
project_point, _, state = point_project(point, edge.node0.point,
edge.node1.point)
if state == 1:
# 点落在线段末端外
project_point = edge.node1.point
elif state == -1:
project_point = edge.node0.point
else:
in_road = True
mod_list.append(project_point)
state_list.append(state)
temp = zip(mod_list, edge_list, dist_list, score_list)
match_list = []
for i, mtc in enumerate(temp):
# state = state_list[i]
# if in_road and state != 0:
# continue
match_list.append(mtc)
return match_list
def get_dyn_points_first(self, point, candidate):
"""
get modified points
:param candidate: Edges 候选项
:param point:
:return: point_list: list of MatchState
"""
# first point
point_list = []
for edge in candidate:
p0, p1 = edge.node0.point, edge.node1.point
dist = point2segment(point, p0, p1)
project_point, _, state = point_project(point, p0, p1)
ms = MatchState(edge, project_point, dist, dist)
if dist < 100:
point_list.append(ms)
return point_list
def _get_mod_point_first(candidate, point):
"""
:param candidate:
:param point: current point
:return: project_point, sel_edge
"""
min_dist, sel_edge = 1e20, None
# first point
for edge in candidate:
# n0, n1 = edge.node0, edge.nodeid1
p0, p1 = edge.node0.point, edge.node1.point
dist = point2segment(point, p0, p1)
if min_dist > dist:
min_dist, sel_edge = dist, edge
sel_node0, sel_node1 = sel_edge.node0, sel_edge.node1
project_point, _, state = point_project(point, sel_node0.point, sel_node1.point)
# print sel_edge.edge_index, min_dist
return project_point, sel_edge, min_dist