def get_nearest_point_with_index(self, point, min_index):
lane_id, index = self.lane_info[min_index]
lane_line = self.__lanes_table[lane_id].centerline
line = Line.init_from_pb(
lane_line.point[index], lane_line.point[index + 1])
nearest, _ = line.get_nearest_point_on_line(point)
return nearest
def find_line(self, output, **data):
lane_id, index = output[0]
lane_line = self.__lanes_table[lane_id].centerline
line = Line.init_from_pb(
lane_line.point[index], lane_line.point[index + 1])
name = data['func_name']
data.pop('func_name')
squared_distance = line.__getattribute__(name)(**data)
return squared_distance
def get_rough_lane_right_width(self, id, s):
lane = self.get_lane_by_id(id)
if lane is None:
return False, 100000.0
has_found, lane_point = self.get_point_and_lane_heading_by_s(id, s)
if not has_found:
return False, 100000.0
if lane.right_boundary_id == 0:
return False, 100000.0
right_lane_id = lane.right_boundary_id
right_boundary = self.get_lane_boundary_by_id(right_lane_id)
if right_boundary is None:
return False, 100000.0
right_boundary_points = right_boundary.boundary.point
min_square_distance = 100000
min_index = -1
for idx, boundary_point in enumerate(right_boundary_points):
p = Point.init_from_other_type(boundary_point)
square_distance = p.square_distance_to(lane_point)
if square_distance < min_square_distance:
min_square_distance = square_distance
min_index = idx
if min_index == -1:
return False, 100000.0
if min_index == 0 and len(right_boundary_points) == 1:
return True, math.sqrt(min_square_distance)
flag = False
square_right_width = 100000.0
if min_index != 0:
line = Line.init_from_pb(
right_boundary_points[min_index], right_boundary_points[min_index - 1])
square_right_width = line.get_mini_square_distance(lane_point)
flag = True
if min_index < len(right_boundary_points) - 1:
line = Line.init_from_pb(
right_boundary_points[min_index], right_boundary_points[min_index + 1])
square_distance = line.get_mini_square_distance(lane_point)
if (flag and square_distance < square_right_width) or (not flag):
square_right_width = square_distance
flag = True
return flag, math.sqrt(square_right_width)
def get_nearest_point(self, point):
output = self.kdtree.query_with_k(point, k=4)
if len(output[0]) <= 0:
return []
fix_data = {"pt": point,
"func_name": "get_mini_square_distance"}
pfunc = partial(self.find_line, **fix_data)
distance_list = map(pfunc, output[0])
min_pack = np.argmin(distance_list)
min_id, min_index = output[0][min_pack][0]
lane_line = self.__lanes_table[min_id].centerline
line = Line.init_from_pb(
lane_line.point[min_index], lane_line.point[min_index + 1])
nearest_point, _ = line.get_nearest_point_on_line(point)
return nearest_point
def get_nearest_lane(self, point):
output = self.kdtree.query_with_k(point, k=4)
if len(output[0]) <= 0:
return False, [], None
fix_data = {"pt": point,
"func_name": "get_mini_square_distance"
}
pfunc = partial(self.find_line, **fix_data)
distance_list = map(pfunc, output[0])
min_pack = np.argmin(distance_list)
min_id, min_index = output[0][min_pack][0]
lane_line = self.__lanes_table[min_id].centerline
line = Line.init_from_pb(
lane_line.point[min_index], lane_line.point[min_index + 1])
s, l = line.get_sl(point)
s = self.get_accumulate_distance(min_id, min_index) + s
return True, [s, l], min_id
def get_nearest_lane_with_heading(self, point, heading):
output = self.kdtree.query_with_k(point, k=4)
if len(output[0]) <= 0:
return False, [], None
heading_vec = Point(np.cos(heading), np.sin(heading))
fix_data = {"pt": point,
"func_name": "combo_inner_product",
"heading": heading_vec}
pfunc = partial(self.find_line, **fix_data)
distance_list = list(map(pfunc, output[0]))
min_pack = distance_list.index(min(distance_list))
min_id, min_index = output[0][min_pack][0]
lane_line = self.__lanes_table[min_id].centerline
line = Line.init_from_pb(
lane_line.point[min_index], lane_line.point[min_index + 1])
s, l = line.get_sl(point)
s = self.get_accumulate_distance(min_id, min_index) + s
return True, [s, l], min_id
def get_point_and_lane_heading_by_s(self, id, s):
lane = self.get_lane_by_id(id)
if lane is None:
return False, None
if lane.centerline is None or len(lane.centerline.point) < 1:
return False, None
if s <= self.centerline_s_dict[id][0]:
return True, Point.init_from_other_type(lane.centerline.point[0])
if s >= self.centerline_s_dict[id][-1]:
return True, Point.init_from_other_type(lane.centerline.point[-1])
index = np.array(self.centerline_s_dict[id]).searchsorted(s)
delta_s = self.centerline_s_dict[id][index] - s
if delta_s < 1e-10:
return True, Point.init_from_other_type(lane.centerline.point[index])
p2 = Point.init_from_other_type(lane.centerline.point[index])
p1 = Point.init_from_other_type(lane.centerline.point[index - 1])
unit_directions_ = Line(p1, p2).unit_v
smooth_point = p2 - unit_directions_ * delta_s
return True, smooth_point