Apollo 6.0 perception 瞎学笔记(二)hdmap_roi_filter
目录
上篇博客作者瞎学了bitmap2d.cc,有那种它要干什么的感觉,但是并不清楚每一步的操作含义是什么,暂时先将那篇博文放着。我们继续来看关于地图bitmap2d的上一层部分:hdmap_roi_filter
hdmap_roi_filter.cc
打开/apollo-r6.0.0/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc
初始化Init方法
bool HdmapROIFilter::Init(const ROIFilterInitOptions& options) {
// load model config
auto config_manager = lib::ConfigManager::Instance();
const lib::ModelConfig* model_config = nullptr;
ACHECK(config_manager->GetModelConfig(Name(), &model_config));
const std::string work_root = config_manager->work_root();
std::string config_file;
std::string root_path;
ACHECK(model_config->get_value("root_path", &root_path));
config_file = GetAbsolutePath(work_root, root_path);
config_file = GetAbsolutePath(config_file, "hdmap_roi_filter.conf");
HDMapRoiFilterConfig config;
ACHECK(apollo::cyber::common::GetProtoFromFile(config_file, &config));
range_ = config.range();
cell_size_ = config.cell_size();
extend_dist_ = config.extend_dist();
no_edge_table_ = config.no_edge_table();
set_roi_service_ = config.set_roi_service();
// reserve mem
const size_t KPolygonMaxNum = 100;
polygons_world_.reserve(KPolygonMaxNum);
polygons_local_.reserve(KPolygonMaxNum);
// init bitmap
Eigen::Vector2d min_range(-range_, -range_);
Eigen::Vector2d max_range(range_, range_);
Eigen::Vector2d cell_size(cell_size_, cell_size_);
bitmap_.Init(min_range, max_range, cell_size);
// output input parameters
AINFO << " HDMap Roi Filter Parameters: "
<< " range: " << range_ << " cell_size: " << cell_size_
<< " extend_dist: " << extend_dist_
<< " no_edge_table: " << no_edge_table_
<< " set_roi_service: " << set_roi_service_;
return true;
}
函数传入ROIFilterInitOptions类型的options,但是函数内并没有关于options的操作。
- 实例化一个ConfigManager类型的对象
- 声明一个ModelConfig类型的空指针
- ACheck检查模型文件
- 通过config_manager获取根路径
- ACheck检查根路径
- 通过GetAbsolutePath获取路径
- 加上配置文件的名称hdmap_roi_filter.conf来获得具体路径
- 声明一个HDMapRoiFilterConfig类型的变量
- ACheck检测能否获得模型文件
- 将配置文件的参数都赋值给全局变量
- 将两个全局变量vector分别调整为100大小
- 设定好地图的尺寸参数,调用bitmap_.Init()方法来初始化地图
- 记录log
Filter方法
bool HdmapROIFilter::Filter(const ROIFilterOptions& options,
LidarFrame* frame) {
if (frame->hdmap_struct == nullptr || frame->cloud == nullptr) {
AERROR << " Input frame data error !";
return false;
}
// get map polygon of roi
auto& road_polygons = frame->hdmap_struct->road_polygons;
auto& junction_polygons = frame->hdmap_struct->junction_polygons;
size_t polygons_world_size = road_polygons.size() + junction_polygons.size();
if (0 == polygons_world_size) {
AINFO << " Polygon Empty.";
return false;
}
polygons_world_.clear();
polygons_world_.resize(polygons_world_size, nullptr);
size_t i = 0;
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));
// set roi points label
if (ret) {
for (auto index : frame->roi_indices.indices) {
frame->cloud->mutable_points_label()->at(index) =
static_cast<uint8_t>(LidarPointLabel::ROI);
frame->world_cloud->mutable_points_label()->at(index) =
static_cast<uint8_t>(LidarPointLabel::ROI);
}
}
// set roi service
if (set_roi_service_) {
auto roi_service = SceneManager::Instance().Service("ROIService");
if (roi_service != nullptr) {
roi_service_content_.range_ = range_;
roi_service_content_.cell_size_ = cell_size_;
roi_service_content_.map_size_ = bitmap_.map_size();
roi_service_content_.bitmap_ = bitmap_.bitmap();
roi_service_content_.major_dir_ =
static_cast<ROIServiceContent::DirectionMajor>(bitmap_.dir_major());
roi_service_content_.transform_ = frame->lidar2world_pose.translation();
if (!ret) {
std::fill(roi_service_content_.bitmap_.begin(),
roi_service_content_.bitmap_.end(), -1);
}
roi_service->UpdateServiceContent(roi_service_content_);
} else {
AINFO << "Failed to find roi service and cannot update.";
}
}
return ret;
}
这里入口参数,有一个LidarFrame类型的变量,我们下一讲细致看下
- 如果雷达和地图输入都是空,直接返回false
- 拿到road_polygons 和 junction_polygons 判断两者数量是不是大于0,即拿到了地图边界
- 将polygon_world_清空,然后将road_polygons和junction_polygons都存入到polygons_world_里面
- 拿到点云数据,通过雷达在世界中的位姿将雷达数据转换到世界坐标系
- 通过FilterWithPolygonMask方法来得到Roi_indices,即roi区域内的索引
- 如果可以拿到索引的话,分别改变点云和世界点云的标签为roi点云
- 下面的部分是apollo重写rosservice的形式,将bitmap的相关参数传出去
FilterWithPolygonMask方法
bool HdmapROIFilter::FilterWithPolygonMask(
const base::PointFCloudPtr& cloud,
const std::vector<PolygonDType>& map_polygons,
base::PointIndices* roi_indices) {
std::vector<Polygon<double>> raw_polygons;
// convert and obtain the major direction
raw_polygons.resize(map_polygons.size());
double min_x = range_;
double max_x = -min_x;
double min_y = min_x;
double max_y = max_x;
for (size_t i = 0; i < map_polygons.size(); ++i) {
auto& raw_polygon = raw_polygons[i];
const auto& map_polygon = map_polygons[i];
raw_polygon.resize(map_polygon.size());
for (size_t j = 0; j < map_polygon.size(); ++j) {
raw_polygon[j].x() = map_polygon[j].x;
raw_polygon[j].y() = map_polygon[j].y;
min_x = std::min(raw_polygon[j].x(), min_x);
max_x = std::max(raw_polygon[j].x(), max_x);
min_y = std::min(raw_polygon[j].y(), min_y);
max_y = std::max(raw_polygon[j].y(), max_y);
}
}
min_x = std::max(min_x, -range_);
max_x = std::min(max_x, range_);
min_y = std::max(min_y, -range_);
max_y = std::min(max_y, range_);
DirectionMajor major_dir = DirectionMajor::XMAJOR;
if ((max_y - min_y) < (max_x - min_x)) {
major_dir = DirectionMajor::YMAJOR;
}
bitmap_.SetUp(major_dir);
return DrawPolygonsMask<double>(raw_polygons, &bitmap_, extend_dist_,
no_edge_table_) &&
Bitmap2dFilter(cloud, bitmap_, roi_indices);
}
上面Filter方法在其中调用了FilterWithPolygonMask方法,我们来看一下:
- 声明一个vector,类型是Polygon(double)
- 调整raw_polygons和map_polygons为一样的尺寸
- 声明并且初始化了局部最大最小值
- 遍历map_polygons
- 拿到原始的raw_polygon 和 map_polygon元素
- 调整raw_polygon的大小,是的raw_polygon的尺寸和map_polygons里面每个元素里的尺寸一样大小
- 遍历map_polygon里面的点
- 将map_polygon里面的顶点x,y传给raw_polygon,比较并更新最小最大的x,y值
- 将最大和最小值和边界值比较,取得较内的边界值
- XMAJOR值赋值给major_dir
- 如果Y方向变化比X方向变化值大,则Y方向为主方向。并且调用Setup方法设立为主方向
- 最后调用DrawPolygonsMask 和 Bitmap2dFilter方法,如果两者都是成功,返回true
TransformFrame方法
这里是transformFrame方法,将世界坐标系下的点云和polygon根据雷达在世界坐标系下的位姿转换到本地
void HdmapROIFilter::TransformFrame(
const base::PointFCloudPtr& cloud, const Eigen::Affine3d& vel_pose,
const std::vector<PolygonDType*>& polygons_world,
std::vector<PolygonDType>* polygons_local,
base::PointFCloudPtr* cloud_local) {
Eigen::Vector3d vel_location = vel_pose.translation();
Eigen::Matrix3d vel_rot = vel_pose.linear();
Eigen::Vector3d x_axis = vel_rot.row(0);
Eigen::Vector3d y_axis = vel_rot.row(1);
// transform polygons
polygons_local->clear();
polygons_local->resize(polygons_world.size());
for (size_t i = 0; i < polygons_local->size(); ++i) {
const auto& polygon_world = *(polygons_world[i]);
auto& polygon_local = (*polygons_local)[i];
polygon_local.resize(polygon_world.size());
for (size_t j = 0; j < polygon_local.size(); ++j) {
polygon_local[j].x = polygon_world[j].x - vel_location.x();
polygon_local[j].y = polygon_world[j].y - vel_location.y();
}
}
// transform cloud
(*cloud_local)->clear();
(*cloud_local)->resize(cloud->size());
for (size_t i = 0; i < (*cloud_local)->size(); ++i) {
const auto& pt = cloud->at(i);
auto& local_pt = (*cloud_local)->at(i);
Eigen::Vector3d e_pt(pt.x, pt.y, pt.z);
local_pt.x = static_cast<float>(x_axis.dot(e_pt));
local_pt.y = static_cast<float>(y_axis.dot(e_pt));
}
}
- 获得vel_pose的translation部分,赋值给vel_location
- 获取vel_pose的linear部分即旋转矩阵,赋值给vel_rot
- 第0行数据赋值给x_axis
- 第1行数据赋值给y_axis
- 将polygons_local清空,并且调整polygons_local大小和polygons_world一样
- 遍历polygons_local里的元素
- 将polygon_world里面每一个点减去相应的x()和y(),得到局部的x()和y(),赋值给polygon_local,因此世界和局部之间的polygon就转换好了
- 将cloud_local清空
- 遍历每一个局部点云的坐标
- 将函数传入的cloud的每一个点的坐标直接乘以相应的变换,得到局部坐标值
Bitmap2dFilter方法
输入点云和bitmap,输出roi点云的索引
bool HdmapROIFilter::Bitmap2dFilter(const base::PointFCloudPtr& in_cloud,
const Bitmap2D& bitmap,
base::PointIndices* roi_indices) {
if (!bitmap.Check(Eigen::Vector2d(0.0, 0.0))) {
AWARN << " Car is not in roi!!.";
return false;
}
roi_indices->indices.clear();
roi_indices->indices.reserve(in_cloud->size());
for (size_t i = 0; i < in_cloud->size(); ++i) {
const auto& pt = in_cloud->at(i);
Eigen::Vector2d e_pt(pt.x, pt.y);
if (!bitmap.IsExists(e_pt)) {
continue;
}
if (bitmap.Check(e_pt)) {
roi_indices->indices.push_back(static_cast<int>(i));
}
}
return true;
}
- 如果check零点返回false,则认为车不在roi内,这里有点让人匪夷所思
- 清空roi_indices索引,调整roi_indices尺寸和输入量一样
- 遍历输入点云
- 判断点是不是在地图内,如果不在则跳过
- 如果此张地图可以Check该点,将该点的索引压入到索引内
因此这里主要还是调用的bitmap的底层接口来判断是否在roi区域内。
总结
这里主要是写好了Filter接口,用于滤除找到ROI区域内的点云索引点。