模式選擇
在modules/map/relative_map/conf/relative_map_config.pb.txt檔案中對模式進行修改：
lane_source: OFFLINE_GENERATED
存在兩種模式：OFFLINE_GENERATED 和 PERCPTION
當使用前者時 高精地圖和感知進行融合
使用後者時 只是用感知生成相對地圖
本文只討論前者
函式入口
檔案路徑：modules/map/relative_map/navigation_lane.cc
函式名：bool NavigationLane::GeneratePath() 這個是重點分析的函式
下面進入這個函式
邏輯分析
在這裡進行了模式檢查：
if (config_.lane_source() == NavigationLaneConfig::OFFLINE_GENERATED && navigation_line_num > 0)
現在進入這個if中
for (int i = 0; i < navigation_line_num; ++i) {
auto current_navi_path = std::make_shared<NavigationPath>();
auto *path = current_navi_path->mutable_path();
if (ConvertNavigationLineToPath(i, path)) {
current_navi_path->set_path_priority(
navigation_info_.navigation_path(i).path_priority());
navigation_path_list_.emplace_back(
i, default_left_width_, default_right_width_, current_navi_path);
}
}
這裡根據導航資料對 navigation_path_list_ 進行了填充 接下來馬上進行判斷：
if (navigation_path_list_.empty()) {
generate_path_on_perception_func();
return true;
}
如果沒有資料 只能使用感知了
如果資料不為空 我們繼續
這裡對導航路線進行排序 還有設定當前車輛所在路徑
其中 current_navi_path_tuple_ 這個變數記錄了當前車所在的道路的索引 下面我們將這個變數稱為當前車道
//根據車的方向 將導航路線從左到右排序
navigation_path_list_.sort(
[](const NaviPathTuple &left, const NaviPathTuple &right) {
double left_y = std::get<3>(left)->path().path_point(0).y();
double right_y = std::get<3>(right)->path().path_point(0).y();
return left_y > right_y;
});
// Get which navigation path the vehicle is currently on.
double min_d = std::numeric_limits<double>::max();
for (const auto &navi_path_tuple : navigation_path_list_) {
int current_line_index = std::get<0>(navi_path_tuple);
ADEBUG << "Current navigation path index is: " << current_line_index;
double current_d = std::numeric_limits<double>::max();
auto item_iter = last_project_index_map_.find(current_line_index);
if (item_iter != last_project_index_map_.end()) {
current_d = item_iter->second.second;
}
if (current_d < min_d) {
min_d = current_d;
//設定當前車輛所在路徑
current_navi_path_tuple_ = navi_path_tuple;
}
}
接下來 將當前車道進行融合 而不是將所有車道進行融合！
//融合感知和導航車道線
// Merge current navigation path where the vehicle is located with perceived
// lane markers.
auto *path = std::get<3>(current_navi_path_tuple_)->mutable_path();
MergeNavigationLineAndLaneMarker(std::get<0>(current_navi_path_tuple_),
path);
進入融合函式
void NavigationLane::MergeNavigationLineAndLaneMarker(const int line_index, common::Path *const path)
進入之後直接進行判斷
如果點的數量少於2 則首先生成導航的車道線
if (path->path_point_size() < 2) {
path->Clear();
ConvertNavigationLineToPath(line_index, path);
}
如果還是點的數量過少 那隻能使用感知的了 並直接return 不用融合了
//如果路徑點還是小於2 則根據感知生成導航車道線
// If the size of "path" points is still smaller than 2, just generate a
// navigation path based on perceived lane markers.
if (path->path_point_size() < 2) {
path->Clear();
ConvertLaneMarkerToPath(perception_obstacles_.lane_marker(), path);
return;
}
common::Path lane_marker_path;
ConvertLaneMarkerToPath(perception_obstacles_.lane_marker(),
&lane_marker_path);
// If the size of lane marker path points is smaller than 2, merging is not
// required.
if (lane_marker_path.path_point_size() < 2) {
return;
}
下面進行融合
前提是隻對當前車道進行車道線的融合 因為傳入的就只是當前車道
融合原理大概是預先設定一個權重 目前感覺這個權重應該是不變的 這裡存個疑
然後遍歷導航中這條線每個點 在導航中找到這個點對應的點 根據權重進行融合
程式碼展示
int lane_marker_index = 0;
double navigation_line_weight = 1.0 - config_.lane_marker_weight();
for (int i = 0; i < path->path_point_size(); ++i) {
auto *point = path->mutable_path_point(i);
double s = point->s();
//計算s距離之後的匹配點 並返回匹配點的索引
//lane_marker_path為感知發來的道路
auto lane_maker_point = GetPathPointByS(lane_marker_path, lane_marker_index,
s, &lane_marker_index);
// For the beginning and ending portions of a navigation path beyond the
// perceived path, only the y-coordinates in the FLU coordinate system are
// used for merging.
const int marker_size = lane_marker_path.path_point_size();
if (lane_marker_index < 0 || lane_marker_index > (marker_size - 1)) {
point->set_y(navigation_line_weight * point->y() +
(1 - navigation_line_weight) * lane_maker_point.y());
lane_marker_index = 0;
continue;
}
*point = common::util::GetWeightedAverageOfTwoPathPoints(
*point, lane_maker_point, navigation_line_weight,
1 - navigation_line_weight);
}
然後回到之前的函式
下面就是各種設定寬度了
先提取了感知的道路寬度
遍歷導航中所有的道路（這裡不僅僅是處理當前道路）
如果遍歷到了當前道路 就設定為感知到的道路寬度
程式碼展示
//設定導航道路的寬度
// Set the width for the navigation path which the vehicle is currently on.
//注意感知車道線寬度是在這裡儲存的
double left_width = perceived_left_width_ > 0.0 ? perceived_left_width_
: default_left_width_;
double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
: default_right_width_;
if (!IsFloatEqual(left_width, default_left_width_) &&
!IsFloatEqual(right_width, default_right_width_)) {
left_width = left_width > default_left_width_ ? left_width - min_d
: left_width + min_d;
right_width = right_width > default_right_width_ ? right_width - min_d
: right_width + min_d;
}
ADEBUG << "The left width of current lane is: " << left_width
<< " and the right width of current lane is: " << right_width;
std::get<1>(current_navi_path_tuple_) = left_width;
std::get<2>(current_navi_path_tuple_) = right_width;
auto curr_navi_path_iter = std::find_if(
std::begin(navigation_path_list_), std::end(navigation_path_list_),
[this](const NaviPathTuple &item) {
return std::get<0>(item) == std::get<0>(current_navi_path_tuple_);
});
if (curr_navi_path_iter != std::end(navigation_path_list_)) {
std::get<1>(*curr_navi_path_iter) = left_width;
std::get<2>(*curr_navi_path_iter) = right_width;
}
// Set the width between each navigation path and its adjacent path.
// The reason for using average of multiple points is to prevent too much
// interference from a singularity.
// If current navigation path is the path which the vehicle is currently
// on, the current lane width uses the perceived width.
//設定每個導航路徑與其相鄰路徑之間的寬度。
//使用多個點的平均值是為了防止奇點產生過多干擾。
//如果當前導航路徑是車輛當前所在的路徑，則當前車道寬度使用感知寬度。
int average_point_size = 5;
for (auto iter = navigation_path_list_.begin();
iter != navigation_path_list_.end(); ++iter) {
const auto &curr_path = std::get<3>(*iter)->path();
// Left neighbor
auto prev_iter = std::prev(iter);
if (prev_iter != navigation_path_list_.end()) {
const auto &prev_path = std::get<3>(*prev_iter)->path();
average_point_size = std::min(
average_point_size,
std::min(curr_path.path_point_size(), prev_path.path_point_size()));
double lateral_distance_sum = 0.0;
for (int i = 0; i < average_point_size; ++i) {
lateral_distance_sum +=
fabs(curr_path.path_point(i).y() - prev_path.path_point(i).y());
}
double width = lateral_distance_sum /
static_cast<double>(average_point_size) / 2.0;
width = common::math::Clamp(width, config_.min_lane_half_width(),
config_.max_lane_half_width());
auto &curr_left_width = std::get<1>(*iter);
auto &prev_right_width = std::get<2>(*prev_iter);
if (std::get<0>(*iter) == std::get<0>(current_navi_path_tuple_)) {
prev_right_width = 2.0 * width - curr_left_width;
} else {
curr_left_width = width;
prev_right_width = width;
}
}
// Right neighbor
auto next_iter = std::next(iter);
if (next_iter != navigation_path_list_.end()) {
const auto &next_path = std::get<3>(*next_iter)->path();
average_point_size = std::min(
average_point_size,
std::min(curr_path.path_point_size(), next_path.path_point_size()));
double lateral_distance_sum = 0.0;
for (int i = 0; i < average_point_size; ++i) {
lateral_distance_sum +=
fabs(curr_path.path_point(i).y() - next_path.path_point(i).y());
}
double width = lateral_distance_sum /
static_cast<double>(average_point_size) / 2.0;
width = common::math::Clamp(width, config_.min_lane_half_width(),
config_.max_lane_half_width());
auto &curr_right_width = std::get<2>(*iter);
auto &next_left_width = std::get<1>(*next_iter);
if (std::get<0>(*iter) == std::get<0>(current_navi_path_tuple_)) {
next_left_width = 2.0 * width - curr_right_width;
} else {
next_left_width = width;
curr_right_width = width;
}
}
總結
如果權重是動態變化的還請大家評論糾正我
感覺apollo的車道線也就是高精地圖和感知通過權重進行融合 沒有很特別很精妙的演算法 如有不對之處還請指教