【Apollo自动驾驶源码解读】车道线的感知和高精地图融合
模式选择
在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的车道线也就是高精地图和感知通过权重进行融合 没有很特别很精妙的算法 如有不对之处还请指教
【Apollo自动驾驶源码解读】车道线的感知和高精地图融合的更多相关文章
- AFNetworking 3.0 源码解读 总结(干货)(下)
承接上一篇AFNetworking 3.0 源码解读 总结(干货)(上) 21.网络服务类型NSURLRequestNetworkServiceType 示例代码: typedef NS_ENUM(N ...
- AFNetworking 3.0 源码解读(四)之 AFURLResponseSerialization
本篇是AFNetworking 3.0 源码解读的第四篇了. AFNetworking 3.0 源码解读(一)之 AFNetworkReachabilityManager AFNetworking 3 ...
- QCustomplot使用分享(二) 源码解读
一.头文件概述 从这篇文章开始,我们将正式的进入到QCustomPlot的实践学习中来,首先我们先来学习下QCustomPlot的类图,如果下载了QCustomPlot源码的同学可以自己去QCusto ...
- SDWebImage源码解读之干货大总结
这是我认为的一些重要的知识点进行的总结. 1.图片编码简介 大家都知道,数据在网络中是以二进制流的形式传播的,那么我们该如何把那些1和0解析成我们需要的数据格式呢? 说的简单一点就是,当文件都使用二进 ...
- swoft 源码解读【转】
官网: https://www.swoft.org/ 源码解读: http://naotu.baidu.com/file/814e81c9781b733e04218ac7a0494e2a?toke ...
- SDWebImage源码解读之SDWebImageDownloaderOperation
第七篇 前言 本篇文章主要讲解下载操作的相关知识,SDWebImageDownloaderOperation的主要任务是把一张图片从服务器下载到内存中.下载数据并不难,如何对下载这一系列的任务进行设计 ...
- SDWebImage源码解读 之 SDWebImageCompat
第三篇 前言 本篇主要解读SDWebImage的配置文件.正如compat的定义,该配置文件主要是兼容Apple的其他设备.也许我们真实的开发平台只有一个,但考虑各个平台的兼容性,对于框架有着很重要的 ...
- SDWebImage源码解读之SDWebImageCache(下)
第六篇 前言 我们在SDWebImageCache(上)中了解了这个缓存类大概的功能是什么?那么接下来就要看看这些功能是如何实现的? 再次强调,不管是图片的缓存还是其他各种不同形式的缓存,在原理上都极 ...
- AFNetworking 3.0 源码解读(十)之 UIActivityIndicatorView/UIRefreshControl/UIImageView + AFNetworking
我们应该看到过很多类似这样的例子:某个控件拥有加载网络图片的能力.但这究竟是怎么做到的呢?看完这篇文章就明白了. 前言 这篇我们会介绍 AFNetworking 中的3个UIKit中的分类.UIAct ...
随机推荐
- AT2347 [ARC070C] NarrowRectangles
首先不难看出一个暴力的 \(dp\) 解法,考虑令 \(dp_{i, j}\) 表示考虑完前 \(i\) 个矩形,第 \(i\) 个矩形左端点在 \(j\) 时所需要的最小花费. 不难有转移: \[d ...
- Swift字符串的介绍
字符串的介绍 字符串在任何的开发中使用都是非常频繁的 OC和Swift中字符串的区别 在OC中字符串类型时NSString,在Swift中字符串类型是String OC中字符串@"" ...
- NS前缀
NS来自于NeXTStep的一个软件 NeXT Software OC中不支持命名空间(namespace) NS是为了避免命名冲突而给的前缀 看到NS前缀就知道是Cocoa中的系统类的名称
- 扩展NSDate类实现快捷使用 —— 昉
获取当前日期和时间: +(NSDate *)getCurrentDate{ NSDate *now = [NSDate date]; return now; } 将日期转换为字符串: +(NSStri ...
- Kubernetes(K8s)基础概念 —— 凿壁偷光
Kubernetes(K8s)基础概念 -- 凿壁偷光 K8s是什么:全称 kubernetes (k12345678s) 作用:用于自动部署,扩展和管理"容器化应用程序"的 ...
- 程序员的情人节「GitHub 热点速览 v.22.07」
又是一年情人日,刚好还是发文的今天.也没什么好送的,送点程序员的浪漫--代码和开源项目吧.记得在本周特推查收这份来自程序员的独有浪漫. 本周 GitHub 霸榜的项目基本上都是老项目,从老项目中挖点新 ...
- Solution -「AGC 012F」「AT 2366」Prefix Median
\(\mathcal{Description}\) Link. 给定序列 \(\{a_{2n-1}\}\),将 \(\{a_{2n-1}\}\) 按任意顺序排列后,令序列 \(b_i\) 为前 ...
- Dubbo的前世今生
搜索关注微信公众号"捉虫大师",后端技术分享,架构设计.性能优化.源码阅读.问题排查.踩坑实践. 本文已收录 https://github.com/lkxiaolou/lkxiao ...
- Owin Katana 的分析介绍
本文首要是对美团的分布式ID结构Leaf的原理进行介绍,针对Leaf原项目中的一些issue,对Leaf项目进行功用增强,问题修正及优化改善,改善后的项目地址在这里: Leaf项目改善计划 https ...
- 蓝桥杯2022年java试题
一:基础练习: (本文只附代码,解析后续修改后添上) 1.a+b问题: 代码如下: 1 import java.util.*; 2 public class Main { 3 public stati ...