apollo自动驾驶-感知-地图ROI过滤：pointcloud_map_based_roi
发布时间
阅读量：
阅读量
apollo自动驾驶-感知-地图ROI过滤：pointcloud_map_based_roi
-
功能介绍
-
- 提取高精地图结构
- 标记哪些点云在地图内
-
核心代码解析
-
如何获取高质量地图的结构信息
- 如何实现多边形信息在分辨率图中的编码
-
效果展示
-
- 提取高精地图结构
- 点云过滤效果展示
-
功能介绍
该组件的作用是对 incoming LiDAR 点云进行过滤处理，并旨在筛选出位于规划区域内的有效数据。该系统的主要功能包含两个关键步骤：首先需要生成精确的地图架构；其次需要判断哪些采集到的LiDAR数据处于规划区域内并加以保留
提取高精地图结构
目标为获取车辆周围指定距离范围内的道路和交叉口所形成的全部多边形集合
为此，分为以下几步实现：
- 第一步是解析高精度地图文件信息并对其进行细致划分工作（建立lane_table_、junction_table_等数据表），随后构建KD二叉树以便于后续快速定位周围的道路段和交叉路口。
- 接着利用 KD 树定位出相关道路数据并提取其边缘点；随后定位出所有交叉路口区域并提取其形状信息。
- 最终通过对道路边缘点进行优化采样处理得到精确的道路轮廓描述；与此同时直接提取了各交叉路口的具体形状数据。
标记哪些点云在地图内
这里使用了位图的技术，节省了存储空间。其实现思路如下：
- 通过划分指定范围内的空间为尺寸为0.25×0.25的小方块；
- 该地图由road和junction的多边形构成；将多边形相关信息编码到位图中。
- 对于激光点云，在标记为1的区域内的点被认为是位于地图内；其他区域内的点被认为是位于地图外。
这里很关键的一步是如何将多边形信息编码到位图中，现介绍如下：
如上图所示，在其中一条分割线为例
核心代码解读
若您有兴趣深入了解本模块的相关信息，请参阅上方与该文章配套的资源包，默认情况下会包含源码详尽的注释说明及打印输出以助大家更好地理解和掌握相关内容。
如何提取高精地图结构
解析高精度地图数据并进行详细划分（生成lane_table_和junction_table_等），随后用于建立KD树以便快速定位车辆周围的road和junction信息。
高精地图初始化入口函数，会转到map_manager.cc执行初始化函数。
if (!map_manager_.Init(map_manager_init_options)) {
AINFO << "Failed to init map manager.";
use_map_manager_ = false;
}
cpp该初始化将获取高精度地图数据，并特别提醒：Apollo的高精度地图遵循OpenDrive规范，请各位务必深入学习这一规定。随后将转向hdmap_input.cc以获取所需地图数据
if (!hdmap_input_->Init()) {
AINFO << "Failed to init hdmap input.";
return false;
}
cpp加载地图文件，会转到hdmap_impl.cc读取地图数据和进行数据处理。
if (hdmap_->LoadMapFromFile(hdmap_file_) != 0) {
AERROR << "Failed to load hadmap file: " << hdmap_file_;
return false;
}
cpp创建LaneInfo和JunctionInfo等数据结构，创建KD二叉树，以方便处理。
int HDMapImpl::LoadMapFromProto(const Map& map_proto) {
if (&map_proto != &map_) { // avoid an unnecessary copy
Clear();
map_ = map_proto;
}
// 根据地图中lane信息，创建LaneInfo类，并把LaneInfo类加入到lane_table_表中
// LaneInfo中有对lane的精细处理，是必须理解的
for (const auto& lane : map_.lane()) {
lane_table_[lane.id().id()].reset(new LaneInfo(lane));
}
// 根据地图中junction（交叉口）信息，创建JunctionInfo类，并加入到junction_table_表中
// JunctionInfo在本模块是需要使用的，是必须理解的
for (const auto& junction : map_.junction()) {
junction_table_[junction.id().id()].reset(new JunctionInfo(junction));
}
。。。
// 创建lane中segment的KD二叉树,以LaneSegment为例进行详细说明
BuildLaneSegmentKDTree();
。。。
cpp
就BuildLaneSegmentKDTree二叉树构建过程而言进行阐述。该方法将转向aaboxkdtree2d.h文件构建二叉树。
void HDMapImpl::BuildLaneSegmentKDTree() {
AABoxKDTreeParams params;
// 设定叶子最大距离跨度
params.max_leaf_dimension = 5.0; // meters.
// 设定叶子所能包含的最大对象数量
params.max_leaf_size = 16;
// 转到aaboxkdtree2d.h文件创建二叉树
BuildSegmentKDTree(lane_table_, params, &lane_segment_boxes_, &lane_segment_kdtree_);
}
void HDMapImpl::BuildSegmentKDTree(
const Table& table,
const AABoxKDTreeParams& params,
BoxTable* const box_table,
std::unique_ptr<KDTree>* const kdtree) {
box_table->clear();
// 对每条lane创建LaneSegmentBox类型的线段轴对齐包围盒
for (const auto& info_with_id : table) {
const auto* info = info_with_id.second.get();
for (size_t id = 0; id < info->segments().size(); ++id) {
const auto& segment = info->segments()[id];
box_table->emplace_back(apollo::common::math::AABox2d(segment.start(), segment.end()), info, &segment, id);
}
}
// 使用线段轴对齐包围盒容器创建二叉树
kdtree->reset(new KDTree(*box_table, params));
}
cpp
初始化构建二叉树的过程。通过AABoxKDTree2dNode的构建机制可以看出，在代码实现中始终采用递归调用的方式进行操作。其中，SplitToSubNodes(objects, params)函数负责判断何时生成子节点，并确定这些子节点的生成条件；InitObjects(objects)函数则决定了子节点内部对象的排序方式。
创建KD二叉树的过程结束。
AABoxKDTree2d(const std::vector<ObjectType> &objects, const AABoxKDTreeParams ¶ms) {
if (!objects.empty()) {
std::vector<ObjectPtr> object_ptrs;
for (const auto &object : objects) {
object_ptrs.push_back(&object);
}
// 创建根节点，设置深度是0
root_.reset(new AABoxKDTree2dNode<ObjectType>(object_ptrs, params, 0));
}
}
AABoxKDTree2dNode(const std::vector<ObjectPtr> &objects, const AABoxKDTreeParams ¶ms, int depth) :
depth_(depth) {
ACHECK(!objects.empty());
// 计算本节点所有对象的轴对齐后的最大边界
ComputeBoundary(objects);
// 确定分区方案，以范围更大的轴方向作为参考
ComputePartition();
// 确定是否分子节点
if (SplitToSubNodes(objects, params)) {
std::vector<ObjectPtr> left_subnode_objects;
std::vector<ObjectPtr> right_subnode_objects;
// 将该节点分为左右子节点
PartitionObjects(objects, &left_subnode_objects, &right_subnode_objects);
// Split to sub-nodes.
// 若已经分出类子节点，则采用递归调用的方法，反复产生子节点，直到不可再分，由此产生类二叉树
if (!left_subnode_objects.empty()) {
left_subnode_.reset(new AABoxKDTree2dNode<ObjectType>(left_subnode_objects, params, depth + 1));
}
if (!right_subnode_objects.empty()) {
right_subnode_.reset(new AABoxKDTree2dNode<ObjectType>(right_subnode_objects, params, depth + 1));
}
} else {
InitObjects(objects);
}
}
cpp
2）接着利用二叉树进行查找，在地图上定位出车辆活动范围内的道路段，并确定其左右边界坐标。随后利用同样的方法提取了各个交叉路口区域对应的多边形信息。最后通过对道路左右边缘坐标的采样处理，在每个采样点上构建起完整的道路轮廓形状。在此过程中同步记录了所有交叉路口所对应的区域形状数据。
解析城市道路网络模型主体功能的主要目的是通过road和junction多边形数据获取其相关特征参数，并由程序模块位于map_manager.cc中负责实现。
// 提取地图拓扑结构，主要包括road和junction
if (!map_manager_.Update(map_manager_options, message->lidar_frame_.get())) {
AINFO << "Failed to update map structure.";
return false;
}
cpp提取高精地图拓扑结构的主体函数，会转到hdmap_input.cc函数中执行。
// 提取高精地图拓扑结构的主体函数
if (!hdmap_input_->GetRoiHDMapStruct(point, roi_search_distance_, frame->hdmap_struct)) {
frame->hdmap_struct->road_polygons.clear();
frame->hdmap_struct->road_boundary.clear();
frame->hdmap_struct->hole_polygons.clear();
frame->hdmap_struct->junction_polygons.clear();
AINFO << "Failed to get roi from hdmap.";
}
cpp该功能旨在完成地图拓扑结构的关键模块。
该程序利用二叉树搜索算法最终确定了每条道路两侧的边界区域。
同样地，在交叉点处应用同样的方法确定了所有交叉口的多边形区域。
该程序通过调用MergeBoundaryJunction函数实现了对所有道路及其交汇点区域的详细建模。
bool HDMapInput::GetRoiHDMapStruct(
const base::PointD& pointd,
const double distance,
std::shared_ptr<base::HdmapStruct> hdmap_struct_ptr) {
lib::MutexLock lock(&mutex_);
if (hdmap_.get() == nullptr) {
AERROR << "hdmap is not available";
return false;
}
// Get original road boundary and junction
// 用于存储道路边界，主要包括左边界和右边界，结构体的定义
std::vector<RoadRoiPtr> road_boundary_vec;
std::vector<JunctionInfoConstPtr> junctions_vec;
apollo::common::PointENU point;
point.set_x(pointd.x);
point.set_y(pointd.y);
point.set_z(pointd.z);
// 获取road边界
if (hdmap_->GetRoadBoundaries(point, distance, &road_boundary_vec, &junctions_vec) != 0) {
AERROR << "Failed to get road boundary, point: " << point.DebugString();
return false;
}
junctions_vec.clear();
// 获取指定范围内的JunctionInfo
if (hdmap_->GetJunctions(point, distance, &junctions_vec) != 0) {
AERROR << "Failed to get junctions, point: " << point.DebugString();
return false;
}
if (hdmap_struct_ptr == nullptr) {
return false;
}
hdmap_struct_ptr->hole_polygons.clear();
hdmap_struct_ptr->junction_polygons.clear();
hdmap_struct_ptr->road_boundary.clear();
hdmap_struct_ptr->road_polygons.clear();
// Merge boundary and junction
// 将上述的道路边界和交叉口联合考虑，得出道路多边形和交叉口多边形
// road_boundary_vec：道路边界
// junctions_vec：指定范围内的JunctionInfo
// road_boundaries：存储每个road执行下采样后的左右边界的点
// road_polygons：存储每个road的左右边界的点
// junction_polygons：存储每个交叉口多边形的点
EigenVector<base::RoadBoundary> road_boundaries;
MergeBoundaryJunction(
road_boundary_vec,
junctions_vec,
&road_boundaries,
&(hdmap_struct_ptr->road_polygons),
&(hdmap_struct_ptr->junction_polygons));
// Filter road boundary by junction
// 滤除road边界点在交叉口多边形内的点，存储在hdmap_struct_ptr->road_boundary中
GetRoadBoundaryFilteredByJunctions(
road_boundaries, hdmap_struct_ptr->junction_polygons, &(hdmap_struct_ptr->road_boundary));
return true;
}
int HDMapImpl::GetRoadBoundaries(
const PointENU& point,
double radius,
std::vector<RoadRoiPtr>* road_boundaries,
std::vector<JunctionInfoConstPtr>* junctions) const {
if (road_boundaries == nullptr || junctions == nullptr) {
AERROR << "the pointer in parameter is null";
return -1;
}
road_boundaries->clear();
junctions->clear();
std::set<std::string> junction_id_set;
std::vector<RoadInfoConstPtr> roads;
// 获得车辆在radius范围内的所有road
if (GetRoads(point, radius, &roads) != 0) {
AERROR << "can not get roads in the range.";
return -1;
}
for (const auto& road_ptr : roads) {
if (road_ptr->has_junction_id()) {
// 对于含有交叉口的road，获取road_id中junction_id对应的交叉口信息JunctionInfo
JunctionInfoConstPtr junction_ptr = GetJunctionById(road_ptr->junction_id());
if (junction_id_set.find(junction_ptr->id().id()) == junction_id_set.end()) {
// 把搜索到的JunctionInfo的id号push到junction_id_set中
junctions->push_back(junction_ptr);
junction_id_set.insert(junction_ptr->id().id());
}
} else {
// 对于普通的road
RoadRoiPtr road_boundary_ptr(new RoadRoi());
// 获取road的边界信息，主要包括左右边界
const std::vector<apollo::hdmap::RoadBoundary>& temp_road_boundaries = road_ptr->GetBoundaries();
road_boundary_ptr->id = road_ptr->id();
// 针对road每个section的边界进行处理
for (const auto& temp_road_boundary : temp_road_boundaries) {
// 获取section边界的外围多边形，这个读取的是地图proto格式的信息
apollo::hdmap::BoundaryPolygon boundary_polygon = temp_road_boundary.outer_polygon();
for (const auto& edge : boundary_polygon.edge()) {
if (edge.type() == apollo::hdmap::BoundaryEdge::LEFT_BOUNDARY) {
// 边类型为左边界
for (const auto& s : edge.curve().segment()) {
for (const auto& p : s.line_segment().point()) {
// 把左边界的所有点push到RoadRoi对象的left_boundary中
road_boundary_ptr->left_boundary.line_points.push_back(p);
}
}
}
if (edge.type() == apollo::hdmap::BoundaryEdge::RIGHT_BOUNDARY) {
for (const auto& s : edge.curve().segment()) {
for (const auto& p : s.line_segment().point()) {
road_boundary_ptr->right_boundary.line_points.push_back(p);
}
}
}
}
// 对于道路上有洞洞的情况，增加了一个hole_boundary类型
if (temp_road_boundary.hole_size() != 0) {
for (const auto& hole : temp_road_boundary.hole()) {
PolygonBoundary hole_boundary;
for (const auto& edge : hole.edge()) {
if (edge.type() == apollo::hdmap::BoundaryEdge::NORMAL) {
for (const auto& s : edge.curve().segment()) {
for (const auto& p : s.line_segment().point()) {
hole_boundary.polygon_points.push_back(p);
}
}
}
}
road_boundary_ptr->holes_boundary.push_back(hole_boundary);
}
}
}
// road_boundary_ptr为获取的每个road的边界信息
// road_boundaries存储类所有指定范围内的road的边界信息
road_boundaries->push_back(road_boundary_ptr);
}
}
return 0;
}
void HDMapInput::MergeBoundaryJunction(
const std::vector<apollo::hdmap::RoadRoiPtr>& boundary,
const std::vector<apollo::hdmap::JunctionInfoConstPtr>& junctions,
EigenVector<base::RoadBoundary>* road_boundaries_ptr,
EigenVector<base::PointCloud<base::PointD>>* road_polygons_ptr,
EigenVector<base::PointCloud<base::PointD>>* junction_polygons_ptr) {
const int boundary_size = static_cast<int>(boundary.size());
const int junctions_size = static_cast<int>(junctions.size());
const int polygon_size = boundary_size;
road_boundaries_ptr->clear();
road_polygons_ptr->clear();
junction_polygons_ptr->clear();
road_polygons_ptr->resize(polygon_size);
junction_polygons_ptr->resize(junctions_size);
road_boundaries_ptr->resize(polygon_size);
int polygons_index = 0;
// Merge boundary
PointDCloudPtr temp_cloud = base::PointDCloudPool::Instance().Get();
for (int step = 0, i = 0; i < polygon_size; ++i) {
temp_cloud->clear();
const LineBoundary& left_boundary = boundary[i]->left_boundary;
const std::vector<apollo::common::PointENU>& left_line_points = left_boundary.line_points;
ADEBUG << "Input left road_boundary size = " << left_line_points.size();
// 用于稀疏采样
step = (left_line_points.size() > 2) ? hdmap_sample_step_ : 1;
for (unsigned int idx = 0; idx < left_line_points.size(); idx += step) {
PointD pointd;
pointd.x = left_line_points.at(idx).x();
pointd.y = left_line_points.at(idx).y();
pointd.z = left_line_points.at(idx).z();
// 道路左边界的点push到temp_cloud，里面存储该road里面所有的左边界的点
temp_cloud->push_back(pointd);
}
// 执行road左边界下采样，放到RoadBoundary的left_boundary边界下
DownsamplePoints(temp_cloud, &(road_boundaries_ptr->at(polygons_index).left_boundary));
for (unsigned int index = 0; index < road_boundaries_ptr->at(polygons_index).left_boundary.size(); ++index) {
// 把road左边界的点push到对应road的多边形中
road_polygons_ptr->at(polygons_index)
.push_back(road_boundaries_ptr->at(polygons_index).left_boundary[index]);
}
ADEBUG << "Left road_boundary downsample size = "
<< road_boundaries_ptr->at(polygons_index).left_boundary.size();
temp_cloud->clear();
const LineBoundary& right_boundary = boundary[i]->right_boundary;
const std::vector<apollo::common::PointENU>& right_line_points = right_boundary.line_points;
ADEBUG << "Input right road_boundary size = " << right_line_points.size();
step = (right_line_points.size() > 2) ? hdmap_sample_step_ : 1;
for (unsigned int idx = 0; idx < right_line_points.size(); idx += step) {
PointD pointd;
pointd.x = right_line_points.at(idx).x();
pointd.y = right_line_points.at(idx).y();
pointd.z = right_line_points.at(idx).z();
temp_cloud->push_back(pointd);
}
DownsamplePoints(temp_cloud, &(road_boundaries_ptr->at(polygons_index).right_boundary));
for (unsigned int index = 0; index < road_boundaries_ptr->at(polygons_index).right_boundary.size(); ++index) {
road_polygons_ptr->at(polygons_index)
.push_back(road_boundaries_ptr->at(polygons_index)
.right_boundary
[road_boundaries_ptr->at(polygons_index).right_boundary.size() - 1
- index]);
}
ADEBUG << "Right road_boundary downsample size = "
<< road_boundaries_ptr->at(polygons_index).right_boundary.size();
++polygons_index;
}
// Merge junctions
for (int idx = 0; idx < junctions_size; ++idx) {
const Polygon2d& polygon = junctions[idx]->polygon();
const std::vector<Vec2d>& points = polygon.points();
for (size_t idj = 0; idj < points.size(); ++idj) {
PointD pointd;
pointd.x = points[idj].x();
pointd.y = points[idj].y();
pointd.z = 0.0;
junction_polygons_ptr->at(idx).push_back(pointd);
}
}
}
cpp
如何实现将多边形信息编码到位图
实现流程由hdmap_roi_filter.cc中的filter模块完成。道路和交叉路口的多边形将被转换为polygons_world_进行处理。
// 把道路多边形和交叉口多边形统一当成多边形push到polygons_world_中
for (auto& polygon : road_polygons) {
polygons_world_[i++] = &polygon;
}
for (auto& polygon : junction_polygons) {
polygons_world_[i++] = &polygon;
}
// transform to local
base::PointFCloudPtr cloud_local = base::PointFCloudPool::Instance().Get();
TransformFrame(frame->cloud, frame->lidar2world_pose, polygons_world_, &polygons_local_, &cloud_local);
// 点云过滤的主体实现
bool ret = FilterWithPolygonMask(cloud_local, polygons_local_, &(frame->roi_indices));
cpp
DrawPolygonsMask函数负责将多边形信息编码到位图的实现部分。该功能模块最终将转移到polygon_mask.h文件中进行具体实现。
bool HdmapROIFilter::FilterWithPolygonMask(
const base::PointFCloudPtr& cloud,
const EigenVector<PolygonDType>& map_polygons,
base::PointIndices* roi_indices) {
。。。
return DrawPolygonsMask<double>(raw_polygons, &bitmap_, extend_dist_, no_edge_table_)
&& Bitmap2dFilter(cloud, bitmap_, roi_indices);
}
cppDrawPolygonsMask函数负责完成整个编码流程，在此过程中，
调用poly_scan_cvter进行扫描转换操作得到的分隔对被存储在变量scans_intervals中。
随后，
bitmap->Set(x, valid_y_range.first, valid_y_range.second) 负责处理每个分隔对之间的空间填充操作。
各区域间的空间信息按照预设规则记录到位图中。
bool DrawPolygonMask(
const typename PolygonScanCvter<T>::Polygon& polygon,
Bitmap2D* bitmap,
const double extend_dist,
const bool no_edge_table) {
typedef typename PolygonScanCvter<T>::IntervalIn IntervalIn;
typedef typename PolygonScanCvter<T>::IntervalOut IntervalOut;
typedef typename PolygonScanCvter<T>::DirectionMajor PolyDirMajor;
if (bitmap->Empty()) {
AERROR << "bitmap is empty";
return false;
}
Eigen::Vector2d poly_min_p, poly_max_p;
// 将2个元素初始化为double类型能表示的最大值
poly_min_p.setConstant(std::numeric_limits<double>::max());
poly_max_p = -poly_min_p;
// 求取多边形轴对齐方向xy最大最小范围
for (const auto& pt : polygon) {
poly_min_p.x() = std::min(pt.x(), poly_min_p.x());
poly_min_p.y() = std::min(pt.y(), poly_min_p.y());
poly_max_p.x() = std::max(pt.x(), poly_max_p.x());
poly_max_p.y() = std::max(pt.y(), poly_max_p.y());
}
if (poly_max_p.x() <= poly_min_p.x()) {
AERROR << "Invalid polygon";
return false;
}
if (poly_max_p.y() <= poly_min_p.y()) {
AERROR << "Invalid polygon";
return false;
}
//(-120,-120) (120,120) (0.25,0.25)
const Eigen::Vector2d& bitmap_min_range = bitmap->min_range();
const Eigen::Vector2d& bitmap_max_range = bitmap->max_range();
const Eigen::Vector2d& cell_size = bitmap->cell_size();
const int major_dir = bitmap->dir_major();
const int op_major_dir = bitmap->op_dir_major();
// check major x range
IntervalIn valid_range;
// 假设为x轴为主方向，first：多边形的最小x值，second：多边形的最大x值
valid_range.first = std::max(poly_min_p[major_dir], bitmap_min_range[major_dir]);
valid_range.second = std::min(poly_max_p[major_dir], bitmap_max_range[major_dir]);
// for numerical stability
// 保证数值稳定性，以四舍五入方式，将first转换为可以被cell_size[major_dir]整除的值
valid_range.first
= (static_cast<int>((valid_range.first - bitmap_min_range[major_dir]) / cell_size[major_dir]) + 0.5)
* cell_size[major_dir]
+ bitmap_min_range[major_dir];
if (valid_range.second < valid_range.first + cell_size[major_dir]) {
AWARN << "Invalid range: " << valid_range.first << " " << valid_range.second
<< ". polygon major directory range: " << poly_min_p[major_dir] << " " << poly_max_p[major_dir]
<< ". cell size: " << cell_size[major_dir];
return true;
}
// start calculating intervals of scans
// 创建一个多边形扫描器
PolygonScanCvter<T> poly_scan_cvter;
poly_scan_cvter.Init(polygon);
std::vector<std::vector<IntervalOut>> scans_intervals;
if (no_edge_table) {
size_t scans_size = static_cast<size_t>((valid_range.second - valid_range.first) / cell_size[major_dir]);
scans_intervals.resize(scans_size);
for (size_t i = 0; i < scans_size; ++i) {
double scan_loc = valid_range.first + i * cell_size[major_dir];
poly_scan_cvter.ScanCvt(scan_loc, static_cast<PolyDirMajor>(major_dir), &(scans_intervals[i]));
}
} else {
// 执行该分支
poly_scan_cvter.ScansCvt(
valid_range, static_cast<PolyDirMajor>(major_dir), cell_size[major_dir], &(scans_intervals));
}
// start to draw
// x指的是分割线上主方向的点
double x = valid_range.first;
for (size_t i = 0; i < scans_intervals.size(); x += cell_size[major_dir], ++i) {
// 对每条分割线进行处理
for (auto scan_interval : scans_intervals[i]) {
if (scan_interval.first > scan_interval.second) {
AERROR << "The input polygon is illegal(complex polygon)";
return false;
}
// extend
scan_interval.first -= extend_dist;
scan_interval.second += extend_dist;
IntervalOut valid_y_range;
valid_y_range.first = std::max(bitmap_min_range[op_major_dir], scan_interval.first);
valid_y_range.second = std::min(bitmap_max_range[op_major_dir], scan_interval.second);
if (valid_y_range.first > valid_y_range.second) {
continue;
}
// valid_y_range代表一个分割包低值和高值，分别是两条线段与分割线的交点
// 落在该范围内的点，认为是位于多边形内部
bitmap->Set(x, valid_y_range.first, valid_y_range.second);
}
}
return true;
}
cpp
实现ScansCvt函数的部分中包含两个关键步骤：
首先，在BuildEt()过程中生成一个多边形边界表格时会过滤掉未与任何分隔线相交的边界；
其次，在UpdateAet(&((*scans_intervals)[i]))这一过程同样至关重要,它最终确定所需的所有分割配对,而特别需要注意的是在计算多边形边界与下一个分割面交点时应优先保留有效的边界.
void PolygonScanCvter<T>::ScansCvt(
const IntervalIn& scans_interval,
const DirectionMajor dir_major,
const T& step,
std::vector<std::vector<IntervalOut>>* scans_intervals) {
dir_major_ = dir_major;
op_dir_major_ = OppositeDirection(dir_major_);
CHECK_GT(step, 0.0);
step_ = step;
CHECK_GT(scans_interval.second, scans_interval.first + step);
// first：多边形的最小x值，second：多边形的最大x值
bottom_x_ = scans_interval.first;
double top_x = scans_interval.second;
// 扫描步数，0.25分辨率
scans_size_ = static_cast<size_t>((top_x - bottom_x_) / step_);
top_segments_.clear();
top_segments_.reserve(2);
// 多边形解析，求取线段斜率和标记点的位置关系
ParsePolygon(dir_major, true);
// allocate output data
scans_intervals->clear();
scans_intervals->resize(scans_size_);
// 创建多边形边的table
BuildEt();
// initialization aet
(*scans_intervals)[0].reserve(polygon_.size());
// add et to aet
// 提取第一条分割线对应的边
for (const auto& edge : et_[0]) {
// 斜率有限的，说明与分割线有交点
if (std::isfinite(edge.k)) {
aet_.second.push_back(edge);
} else { // 斜率是无穷大的
// 需要把边的最小的y和最大的y打包成一个分割包
(*scans_intervals)[0].push_back(IntervalOut(edge.y, edge.max_y));
}
}
// sort
// 将线段与分割线的交点的y值按照从小到大左排序
std::sort(aet_.second.begin(), aet_.second.end());
CHECK_EQ(aet_.second.size() & 1, static_cast<size_t>(0));
// add aet to result
// 因为多边形是封闭的，与分割线的角度一定是偶数
for (size_t i = 0; i < aet_.second.size(); i += 2) {
double min_y = aet_.second[i].y;
double max_y = aet_.second[i + 1].y;
// 每2个交点打包成一个分割包
(*scans_intervals)[0].push_back(IntervalOut(min_y, max_y));
}
for (size_t i = 1; i < scans_size_; ++i) {
// 针对剩余的分割线进行分析
UpdateAet(&((*scans_intervals)[i]));
}
if (top_segments_.size()) {
scans_intervals->resize(scans_size_ + 1);
for (auto& seg : top_segments_) {
double y1 = seg.first;
double y2 = seg.second;
double min_y = std::min(y1, y2);
double max_y = std::max(y1, y2);
(*scans_intervals)[scans_size_].push_back(IntervalOut(min_y, max_y));
}
}
}
void PolygonScanCvter<T>::BuildEt() {
const auto& segments = segments_[dir_major_];
// allocate memory
et_.clear();
et_.resize(scans_size_);
for (auto& edge_list : et_) {
// 有多少点，创建多少边
edge_list.reserve(polygon_.size());
}
// convert segments to edges
std::vector<std::pair<int, Edge>> edges;
edges.reserve(segments.size());
for (size_t i = 0; i < segments.size(); ++i) {
std::pair<int, Edge> out_edge;
// 将线段信息转换为边信息，若线段与分割线没有交点，可不考虑此线段
if (ConvertSegment(i, &(out_edge))) {
edges.push_back(out_edge);
}
}
// initial active edge table
aet_.first = 0;
aet_.second.clear();
aet_.second.reserve(segments.size());
for (size_t i = 0; i < edges.size(); ++i) {
// 分割线索引
int x_id = edges[i].first;
const Edge& edge = edges[i].second;
if (x_id >= static_cast<int>(scans_size_)) {
continue;
}
// add x_id == 0 to act
// 用分割线索引和对应的边建立一个边的table
// 注意：分割线索引为大于低点x值最近的分割线
if (x_id >= 0) {
et_[x_id].push_back(edge);
} else {
// check if intersect at x_id = 0
Edge active_edge = edge;
if (active_edge.MoveUp(0.0 - active_edge.x)) {
aet_.second.push_back(active_edge);
}
}
}
}
void PolygonScanCvter<T>::UpdateAet(std::vector<IntervalOut>* scan_intervals) {
size_t x_id = aet_.first + 1;
CHECK_LT(x_id, et_.size());
aet_.first += 1;
scan_intervals->clear();
scan_intervals->reserve(polygon_.size());
// check
// 检查之前的边移动一个步数，查询与现在的分割线是否有交点
size_t valid_edges_num = aet_.second.size();
size_t invalid_edges_num = 0;
for (auto& edge : aet_.second) {
// 如果没有交点，则后面分析时可排除之前没有交点的边，只保留有交点的边
if (!edge.MoveUp(step_)) {
--valid_edges_num;
++invalid_edges_num;
edge.y = s_inf_;
}
}
// add et to aet
size_t new_edges_num = 0;
for (const auto& edge : et_[x_id]) {
// 再次push与本分割线对应的新出来的边
if (std::isfinite(edge.k)) {
++valid_edges_num;
++new_edges_num;
aet_.second.push_back(edge);
} else {
scan_intervals->push_back(IntervalOut(edge.y, edge.max_y));
}
}
CHECK_EQ(valid_edges_num & 1, static_cast<size_t>(0))
<< boost::format(
"valid edges num: %d x: %lf bottom_x: %lf \n vertices num: %d "
"\n")
% valid_edges_num % (x_id * step_ + bottom_x_) % bottom_x_ % polygon_.size()
<< aet_.second[0] << "\n"
<< aet_.second[1] << "\n"
<< aet_.second[2] << "\n"
<< aet_.second[3];
// sort
// 提取有效的边，即所有与此分割线有交点的边
if (invalid_edges_num != 0 || new_edges_num != 0) {
// 因为无效的边的y值都被赋值到s_inf_，sort后，无效边都在后面
std::sort(aet_.second.begin(), aet_.second.end());
// remove invalid edges
aet_.second.resize(valid_edges_num);
}
// 将所有有效边与此分割线的交点打包成一个一个的分割包
AINFO << "autodrv-PolygonScanCvter<T>::UpdateAet-aet_.second.size():" << aet_.second.size();
// 因为多边形是封闭的，与分割线的角度一定是偶数
for (size_t i = 0; i < aet_.second.size(); i += 2) {
// corner case, same point in the pre scan and complex polygon
double min_y = aet_.second[i].y;
double max_y = aet_.second[i + 1].y;
scan_intervals->push_back(IntervalOut(min_y, max_y));
}
}
cpp
bit_map[x] = (valid_y_range.first, valid_y_range.second);该函数的具体实现将被包含在bitmap2d.cc文件中。该实现是地图编码的关键步骤之一，在这一过程中所涉及的min_y和max_y分别代表最低和最高分隔线的位置坐标值。
void Bitmap2D::Set(const double x, const double min_y, const double max_y) {
Eigen::Vector2d real_left, real_right;
real_left[op_dir_major()] = min_y;
real_right[op_dir_major()] = max_y;
real_left[dir_major()] = real_right[dir_major()] = x;
// real_left：分割线交点的低点，real_right：分割线交点的高点
// 将实际位置信息转换成位图的索引
const Vec3ui left_bit_p = RealToBitmap(real_left);
const Vec3ui right_bit_p = RealToBitmap(real_right);
// 对于高点和低点位于位图的同一个字时
if (left_bit_p.y() == right_bit_p.y()) {
// 求取需要索引的字
const int idx = Index(left_bit_p);
// 设置该索引的字从哪一位到哪一位需要全部置1，以标识该区域属于多边形内部
SetRangeBits(right_bit_p.z(), left_bit_p.z(), &bitmap_[idx]);
return;
}
// 下面是针对高点和低点不位于位图的同一个字时
const size_t left_idx = Index(left_bit_p);
const size_t right_idx = Index(right_bit_p);
// 设置一段连续置1的位图空间，以标识该区域属于多边形内部
SetHeadBits(left_bit_p.z(), &bitmap_[left_idx]);
SetTailBits(right_bit_p.z(), &bitmap_[right_idx]);
for (size_t i = left_idx + 1; i < right_idx; ++i) {
bitmap_[i] = static_cast<uint64_t>(-1);
}
}
cpp
效果展示
提取高精地图结构
此页面展示了两个关键图形：第一幅图是由dreamview加载生成的地图界面；第二幅图则显示了代码解析得到的所有多边形顶点位置分布情况。具体而言，在第二幅图中以红色标记标注的是road边界顶点位置信息；以蓝色标记标注的是cross road（交叉路口）顶点位置信息。通过观察发现两幅图的基本结构非常相似
点云过滤效果展示
以蓝色符号表示的点群和以红色符号表示的地图多边形边界上的单个离散点。图1展示了未经筛选的原始数据（蓝圈红框），观察发现，在未经处理的数据中存在大量位于地图范围之外的散乱点。经过筛选处理后得到图2展示的位置数据（蓝圈红框），位于地图范围之外的数据已被剔除。
全部评论 (0)
还没有任何评论哟~