apollo自动驾驶-感知-地图ROI过滤：pointcloud_map_based_roi
功能介绍
该组件是对接收的激光雷达点云进行过滤，目标是只保留地图内的点云，滤除地图外的点云。主要分为2步：1）提取高精地图结构；2）标记哪些点云在地图内。
提取高精地图结构
目标是得到车辆周围指定距离范围内的road（道路）和junction（交叉口）构成的所有多边形。
为此，分为以下几步实现：
- 先读取高精地图文件信息，并做细分处理（创建lane_table_、junction_table_等），同时，创建KD二叉树，方便查找车辆周围的road和junction。
- 再通过二叉树搜索获取车辆一定范围内的road，再获取road的左右边界点；通过二叉树搜索读取了地图中junction的多边形。
- 最后经过对road的左右边界点进行了下采样后，获取每条road边界点构成的多边形；同时，也直接读取地图中junction的多边形。
标记哪些点云在地图内
这里使用了位图的技术，节省了存储空间。其实现思路如下：
- 先将车辆指定范围内的空间分隔为大小为0.25*0.25的小方格，每个小方格在位图中都有对应的位置；
- 地图由road和junction的多边形构成，把多边形信息编码到位图中，被多边形占据的方格置1，否则置0；
- 对于激光点云，若落在方格为1的点则认为在地图内，否则认为在地图外。
这里很关键的一步是如何将多边形信息编码到位图中，现介绍如下：
如下图所示，以其中的一条分隔线为例。
求取分隔线与多边形边的交点，此处共有4个交点，y值从低到高排序为1、2、3、4，可以组成2组分隔对，为(1, 2)，(3, 4)。
从图中可以看出这2组分隔对之间的部分属于多边形内部，每组分隔对之间的部分所占据的方格会对应位图中的连续空间。
实现多边形信息编码到位图的重要任务，就是求取多边形的有效边与每条分隔线的交点，从而得到所有这样的分隔对。因为多边形是封闭空间，所以与分割线的交点一定是偶数。
核心代码解读
若您想了解更多关于此模块的内容，请参考上方与该文章配套的资源，里面含有源码详细的注释和打印，以方便大家理解 。
如何提取高精地图结构
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;
}
该初始化会读取高精地图信息，注意apollo的高精地图是基于OpenDrive规范，推荐大家主动了解一下。会转到hdmap_input.cc读取地图数据。
if (!hdmap_input_->Init()) {
AINFO << "Failed to init hdmap input.";
return false;
}
加载地图文件，会转到hdmap_impl.cc读取地图数据和进行数据处理。
if (hdmap_->LoadMapFromFile(hdmap_file_) != 0) {
AERROR << "Failed to load hadmap file: " << hdmap_file_;
return false;
}
创建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();
。。。
以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));
}
开始创建二叉树。从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);
}
}
2）再通过二叉树搜索获取车辆一定范围内的road，再获取road的左右边界点；通过二叉树搜索读取了地图中junction的多边形。最后经过对road的左右边界点进行了下采样后，获取每条road边界点构成的多边形；同时，也直接读取地图中junction的多边形。
提取地图拓扑结构主体函数，主要是获得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;
}
提取高精地图拓扑结构的主体函数，会转到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.";
}
这个函数就是提取地图拓扑结构的实现部分，其中，
GetRoadBoundaries(point, distance, &road_boundary_vec, &junctions_vec)是通过二叉树搜索获取最终获得每条road的道路左右边界。
GetJunctions(point, distance, &junctions_vec)同样是二叉树搜索获取交叉口多边形。
MergeBoundaryJunction(
road_boundary_vec,
junctions_vec,
&road_boundaries,
&(hdmap_struct_ptr->road_polygons),
&(hdmap_struct_ptr->junction_polygons))函数则得到的是最终所需要的road和junction多边形。
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);
}
}
}
如何实现将多边形信息编码到位图
实现过程位于hdmap_roi_filter.cc中的Filter函数中。将road和junction多边形统一成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));
FilterWithPolygonMask函数中的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);
}
DrawPolygonsMask函数实现了整个编码过程，其中，
poly_scan_cvter.ScansCvt(
valid_range, static_cast(major_dir), cell_size[major_dir], &(scans_intervals)); 得到的是之前介绍的分隔对，存储在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;
}
ScansCvt函数的实现部分，其中，
BuildEt();创建多边形边的table，会剔除掉不予任何分隔线相交的边。
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));
}
}
bitmap->Set(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);
}
}
效果展示
提取高精地图结构
上图为dreamview加载的地图，下图则为代码中提取到的多边形的点，其中红色点为road多边形的点，蓝色为junction多边形的点。可以看出上下图基本是相似的。
点云过滤效果展示
蓝色点为点云，红色点为地图多边形的点。上图为未经过点云过滤的图，可见有很多点云在地图外。下图为过滤后的点云，地图外的点云被过滤。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
