Apollo 6.0 perception 瞎学笔记（四）激光雷达lidar结合opendrive地图数据roi滤波单元测试
目录
前面我们瞎学了apollo感知部分bitmap2d和hdmap_roi_filter，现在我们瞎学一下apollo关于《通过高精地图信息来对激光雷达数据roi滤波》的单元测试部分。代码位于apollo-r6.0.0/modules/perception/lidar/lib/scene_manager/scene_manager_test.cc
1.MockData(LidarFrame* frame) {}函数
此文件中前面是关于ground_service_test部分，我们不在此部分瞎学。直接从打开激光雷达生成数据开始，看到MockData(LidarFrame* frame)函数
void MockData(LidarFrame* frame) {
std::string pcd =
"/apollo/modules/perception/testdata/lidar/lib/scene_manager/data/"
"pcd/1532063882.176900.pcd";
std::string pose =
"/apollo/modules/perception/testdata/lidar/lib/scene_manager/data/"
"pose/1532063882.176900.pose";
// a. load pcd
frame->cloud = base::PointFCloudPool::Instance().Get();
EXPECT_TRUE(LoadPCLPCD(pcd, frame->cloud.get()));
// b. load pose
int idt = 0;
double timestamp = 0.0;
EXPECT_TRUE(
common::ReadPoseFile(pose, &frame->lidar2world_pose, &idt, ×tamp));
// c.update hdmap struct;
base::PointD point;
point.x = frame->lidar2world_pose.translation()(0);
point.y = frame->lidar2world_pose.translation()(1);
point.z = frame->lidar2world_pose.translation()(2);
frame->hdmap_struct.reset(new base::HdmapStruct);
ACHECK(map::HDMapInput::Instance()->GetRoiHDMapStruct(point, 120.0,
frame->hdmap_struct));
// d. trans points
frame->world_cloud = base::PointDCloudPool::Instance().Get();
frame->lidar2world_pose.translation();
for (size_t i = 0; i < frame->cloud->size(); ++i) {
auto& local_pt = frame->cloud->at(i);
Eigen::Vector3d trans_pt(local_pt.x, local_pt.y, local_pt.z);
trans_pt = frame->lidar2world_pose * trans_pt;
base::PointD world_pt;
world_pt.x = trans_pt(0);
world_pt.y = trans_pt(1);
world_pt.z = trans_pt(2);
frame->world_cloud->push_back(world_pt);
}
}
- 声明获取测试点云pcd的路径
- 声明获取此时激光雷达的位姿
- 初始化赋值一个空指针给frame->cloud
- 通过调用pcl的库来加载前面路径下的点云，赋值给frame->cloud
- 通过common::ReadPoseFile方法来将位置和时间戳信息传给frame
- 将雷达的位置信息赋值给base::PointD类型的点point
- 调用HDMapInput中GetRoiHDMapStruct方法，根据函数的名字，我们猜测是根据当前点的位置和搜索半径，返回hdmap_struct。我们打开此函数:
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);
if (hdmap_->GetRoadBoundaries(point, distance, &road_boundary_vec,
&junctions_vec) != 0) {
AERROR << "Failed to get road boundary, 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
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
GetRoadBoundaryFilteredByJunctions(road_boundaries,
hdmap_struct_ptr->junction_polygons,
&(hdmap_struct_ptr->road_boundary));
return true;
}
- 开启线程锁，判断hdmap_类内变量地图数据是否为空
- 将传入点的坐标值传入到apollo::common::PointENU类型的点point
- 判断hdmap_是否可以获得道路边界，我们打开GetRoadBoundaries()方法
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;
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()) {
JunctionInfoConstPtr junction_ptr =
GetJunctionById(road_ptr->junction_id());
if (junction_id_set.find(junction_ptr->id().id()) ==
junction_id_set.end()) {
junctions->push_back(junction_ptr);
junction_id_set.insert(junction_ptr->id().id());
}
} else {
RoadRoiPtr road_boundary_ptr(new RoadRoi());
const std::vector<apollo::hdmap::RoadBoundary>& temp_road_boundaries =
road_ptr->GetBoundaries();
road_boundary_ptr->id = road_ptr->id();
for (const auto& temp_road_boundary : temp_road_boundaries) {
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()) {
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);
}
}
}
}
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_boundaries->push_back(road_boundary_ptr);
}
}
return 0;
}
- 先判断指针的合法性
- 清空road_boundaries，junctions
- 通过GetRoads方法来获取该点半径内的roads，打开GetRoads()函数
int HDMapImpl::GetRoads(const Vec2d& point, double distance,
std::vector<RoadInfoConstPtr>* roads) const {
std::vector<LaneInfoConstPtr> lanes;
if (GetLanes(point, distance, &lanes) != 0) {
return -1;
}
std::unordered_set<std::string> road_ids;
for (auto& lane : lanes) {
if (!lane->road_id().id().empty()) {
road_ids.insert(lane->road_id().id());
}
}
for (auto& road_id : road_ids) {
RoadInfoConstPtr road = GetRoadById(CreateHDMapId(road_id));
CHECK_NOTNULL(road);
roads->push_back(road);
}
return 0;
}
上述GetRoads方法中，首先调用GetLanes方法获取附近的所有车道，然后遍历所有lanes，根据lane取出所在road的id。然后再次遍历所有road，根据GetRoadById方法来拿到所有的road信息，因此下面打开GetLanes()和GetRoadById方法：
int HDMapImpl::GetLanes(const Vec2d& point, double distance,
std::vector<LaneInfoConstPtr>* lanes) const {
if (lanes == nullptr || lane_segment_kdtree_ == nullptr) {
return -1;
}
lanes->clear();
std::vector<std::string> ids;
const int status =
SearchObjects(point, distance, *lane_segment_kdtree_, &ids);
if (status < 0) {
return status;
}
for (const auto& id : ids) {
lanes->emplace_back(GetLaneById(CreateHDMapId(id)));
}
return 0;
}
- 判断指针合法性
- 通过SearchObjects方法来搜索附近一定范围内lane
- 根据id，将lane压入到lanes中
打开SearchObjects方法
template <class KDTree>
int HDMapImpl::SearchObjects(const Vec2d& center, const double radius,
const KDTree& kdtree,
std::vector<std::string>* const results) {
static std::mutex mutex_search_object;
UNIQUE_LOCK_MULTITHREAD(mutex_search_object);
if (results == nullptr) {
return -1;
}
auto objects = kdtree.GetObjects(center, radius);
std::unordered_set<std::string> result_ids;
result_ids.reserve(objects.size());
for (const auto* object_ptr : objects) {
result_ids.insert(object_ptr->object()->id().id());
}
results->reserve(result_ids.size());
results->assign(result_ids.begin(), result_ids.end());
return 0;
}
这里其实就是用的kdtree的结构形式，加速搜索的速度，快速搜索出一定半径内的objects，其实autoware在找寻地图信息的时候，也是这样使用的。
下面是GetRoadById方法，这里就是遍历road_table_，根据id找到对应的RoadInfoConstPtr
RoadInfoConstPtr HDMapImpl::GetRoadById(const Id& id) const {
RoadTable::const_iterator it = road_table_.find(id.id());
return it != road_table_.end() ? it->second : nullptr;
}
再回到前文中的GetRoadBoundaries方法，拿到该点一定半径内的roads后：
- 遍历所有的roads
- 如果此条road拥有junction, 根据id拿到此条junction的信息，判断原来的junction集合中是否有此条junction信息，没有则插入到集合中
- 如果此条road没有junction，根据GetBoundaries()方法拿到temp_road_boundaries
- 遍历temp_road_boundary，拿到outer_polygon()，即边界的外围
- 遍历每一个 temp_road_boundary的边界的每一条边
- 根据edge的类型判断，如果是左边或者右边，将边界的每一条线段上的点压入到对应边界的line_points中
- 如果temp_road_boundary中hole_size不为0，即存在hole，将hole的那些点存入到hole_boundary，再存入到road_boundaries
- 遍历每一个 temp_road_boundary的边界的每一条边
- 遍历temp_road_boundary，拿到outer_polygon()，即边界的外围
在看完GetRoadBoundaries后，再回到GetRoiHDMapStruct方法中，拿到边界信息后，清空hdmap_struct_ptr里面的参数信息。
打开MergeBoundaryJunction方法：
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();
temp_cloud->push_back(pointd);
}
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_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);
}
}
}
- 遍历polygon_size
- 拿到左边界和右边界的数据，如果边界点的数目大于2，step=hdmap_sample_step_，否则step=1。
- 遍历边界点，隔一定的点数，将边界点坐标传到temp_cloud中
- 调用DownsamplePoints方法，我们打开此方法
void HDMapInput::DownsamplePoints(const base::PointDCloudPtr& raw_cloud_ptr,
base::PointCloud<base::PointD>* polygon_ptr,
size_t min_points_num_for_sample) const {
constexpr double kDoubleEpsilon = std::numeric_limits<double>::epsilon();
const PointDCloud& raw_cloud = *raw_cloud_ptr;
unsigned int spt = 0;
double acos_theta = 0.0;
const double radian_to_degree = 57.29577951308232;
const size_t raw_cloud_size = raw_cloud.size();
if (raw_cloud_size <= min_points_num_for_sample) {
for (size_t i = 0; i < raw_cloud_size; ++i) {
polygon_ptr->push_back(raw_cloud[i]);
}
return;
}
// The first point
polygon_ptr->push_back(raw_cloud[0]);
for (size_t idx = 2; idx < raw_cloud_size; ++idx) {
const PointD& point_0 = raw_cloud[spt];
const PointD& point_1 = raw_cloud[idx - 1];
const PointD& point_2 = raw_cloud[idx];
Eigen::Vector2d v1(point_1.x - point_0.x, point_1.y - point_0.y);
Eigen::Vector2d v2(point_2.x - point_1.x, point_2.y - point_1.y);
double vector_dist =
sqrt(v1.cwiseProduct(v1).sum()) * sqrt(v2.cwiseProduct(v2).sum());
// Judge duplicate points
if (vector_dist < kDoubleEpsilon) {
continue;
}
double cos_theta = (v1.cwiseProduct(v2)).sum() / vector_dist;
if (cos_theta > 1.0) {
cos_theta = 1.0;
} else if (cos_theta < -1.0) {
cos_theta = -1.0;
}
double angle = (acos(cos_theta) * radian_to_degree);
acos_theta += angle;
if ((acos_theta - 1.0) > kDoubleEpsilon) {
polygon_ptr->push_back(point_1);
spt = static_cast<unsigned int>(idx - 1);
acos_theta = 0.0;
}
}
// The last point
polygon_ptr->push_back(raw_cloud[raw_cloud_size - 1]);
AINFO << "Downsample road boundary points from " << raw_cloud_size << " to "
<< polygon_ptr->size();
}
- 如果输入原始点云的数量少于最小用于sample的数目，则直接将点压入到polygon_ptr中，即将原始点云直接赋给polygon_ptr。
- 如果大于sample的数量，将原始点云的第一个点压入到polygon_ptr中。
- 遍历剩下的原始点云的点
- 取出第一个点，当前点和前一个点
- 算出第一个点和当前点的距离 乘以 第二个点和第一个点的距离
- 如果乘积过小，则跳过
- 通过向量算法算出两者的角度
- 限定cos_theta的范围到-1，1内
- 转换为角度angle
- acos_theta加上此角度，
- 如果角度大于1，即不是一个几乎可以忽略的小角度，将此点存入到polygon_ptr中，同时更新spt参数值，变成前一个点而不是第一个点。累计的角度也置为0
- 最后把最后一个点也赋给polygon_ptr中
以上便是对边界点进行的一个简单的采样，滤除一些直道上非必要的点。
再回到MergeBoundaryJunction方法中，在对左边或者右边的边界点进行剪裁过后，将边界点的值都存入到road_polygons_ptr->at(polygons_index)中。最后再将junction中的边界点都存入到junction_polygons_ptr->at(idx)中。
MergeBoundaryJunction方法结束后，再回到GetRoiHDMapStruct方法中，下面便是GetRoadBoundaryFilteredByJunctions方法：
bool HDMapInput::GetRoadBoundaryFilteredByJunctions(
const EigenVector<base::RoadBoundary>& road_boundaries,
const EigenVector<base::PointCloud<base::PointD>>& junctions,
EigenVector<base::RoadBoundary>* flt_road_boundaries_ptr) {
for (size_t n_rd = 0; n_rd < road_boundaries.size(); ++n_rd) {
const base::RoadBoundary& temp_road_boundary = road_boundaries[n_rd];
EigenVector<base::PointCloud<base::PointD>> temp_left_boundary_vec;
EigenVector<base::PointCloud<base::PointD>> temp_right_boundary_vec;
// Filter left boundary points
this->SplitBoundary(temp_road_boundary.left_boundary, junctions,
&temp_left_boundary_vec);
// Filter right boundary points
this->SplitBoundary(temp_road_boundary.right_boundary, junctions,
&temp_right_boundary_vec);
auto n_temp_road_boundary =
std::max(temp_left_boundary_vec.size(), temp_right_boundary_vec.size());
for (size_t i = 0; i < n_temp_road_boundary; ++i) {
base::RoadBoundary temp_road_boundary;
if (i < temp_left_boundary_vec.size()) {
temp_road_boundary.left_boundary = temp_left_boundary_vec[i];
}
if (i < temp_right_boundary_vec.size()) {
temp_road_boundary.right_boundary = temp_right_boundary_vec[i];
}
flt_road_boundaries_ptr->push_back(temp_road_boundary);
}
}
return true;
}
- 遍历road_boundaries
- 分别得到左边右边的边界
- 遍历所有的边界，如果左边有则压入，如果右边有则压入，最后得到flt_road_boundaries_ptr
这里面有一个SplitBoundary方法
void HDMapInput::SplitBoundary(
const base::PointCloud<base::PointD>& boundary_line,
const EigenVector<base::PointCloud<base::PointD>>& junctions,
EigenVector<base::PointCloud<base::PointD>>* boundary_line_vec_ptr) {
std::vector<bool> boundary_flag(boundary_line.size());
for (size_t npt = 0; npt < boundary_line.size(); ++npt) {
const PointD& pointd = boundary_line[npt];
boundary_flag[npt] = false;
for (size_t n_rj = 0; n_rj < junctions.size(); ++n_rj) {
if (common::IsPointXYInPolygon2DXY(pointd, junctions[n_rj])) {
boundary_flag[npt] = true;
break;
}
}
}
std::vector<int> line_index;
base::PolygonDType temp_line;
for (size_t i = 1; i < boundary_flag.size(); ++i) {
if (!boundary_flag[i - 1] || !boundary_flag[i]) {
line_index.push_back(static_cast<int>(i - 1));
line_index.push_back(static_cast<int>(i));
} else if (line_index.size() > 1) {
auto pos = std::unique(line_index.begin(), line_index.end());
line_index.erase(pos, line_index.end());
for (size_t j = 0; j < line_index.size(); ++j) {
const PointD& pointd = boundary_line[line_index[j]];
temp_line.push_back(pointd);
}
boundary_line_vec_ptr->push_back(temp_line);
line_index.clear();
temp_line.clear();
}
}
// In case the last several unjunctioned points in the "boundary_line"
if (line_index.size() > 1) {
auto pos = std::unique(line_index.begin(), line_index.end());
line_index.erase(pos, line_index.end());
for (size_t j = 0; j < line_index.size(); ++j) {
const PointD& pointd = boundary_line[line_index[j]];
temp_line.push_back(pointd);
}
boundary_line_vec_ptr->push_back(temp_line);
}
}
- 遍历输入boundary_line的点
- 遍历传入的junction，如果该点在此junction内，boundary_flag置为true，退出junction循环
- 遍历boundary_flag
- 如果此点和上一个点都不是在junction内，则压入到line_index内
- 如果line_index内有点，则去除重复的点，再遍历剩下的不重复的点，把点存入到temp_line内，然后存入到boundary_line_vec_ptr内
- 如果最后有些点不属于junction内，也是将点存入到temp_line内，然后存入到boundary_line_vec_ptr内
因此上述函数会分离出边界部分的点，而不会存入polygon部分的点。
再回到仿制雷达数据函数MockData内：
frame->world_cloud = base::PointDCloudPool::Instance().Get();
frame->lidar2world_pose.translation();
for (size_t i = 0; i < frame->cloud->size(); ++i) {
auto& local_pt = frame->cloud->at(i);
Eigen::Vector3d trans_pt(local_pt.x, local_pt.y, local_pt.z);
trans_pt = frame->lidar2world_pose * trans_pt;
base::PointD world_pt;
world_pt.x = trans_pt(0);
world_pt.y = trans_pt(1);
world_pt.z = trans_pt(2);
frame->world_cloud->push_back(world_pt);
}
将雷达的每一点坐标值乘以当时雷达在世界中的位姿，得到点云在世界坐标系下的位置，将其存入到world_cloud中，因此，以上便是MockData的函数部分了。
2.TEST_F函数
TEST_F(LidarLibSceneManagerTest, lidar_lib_roi_service_test) {
// FIXME(perception): fix the missing data files.
return;
ROIServiceContent content, content1, content2;
content.GetCopy(nullptr);
EXPECT_EQ(content.Name(), "ROIServiceContent");
content1.range_ = 100;
content1.cell_size_ = 0.1;
content1.transform_ = Eigen::Vector3d(0, 0, 0);
EXPECT_FALSE(content.Check(Eigen::Vector3d(0, 0, 0)));
content.SetContent(content1);
EXPECT_EQ(content.range_, content1.range_);
EXPECT_EQ(content.cell_size_, content1.cell_size_);
EXPECT_FALSE(content.Check(Eigen::Vector3d(1000.0, 0, 0)));
EXPECT_FALSE(content.Check(Eigen::Vector3d(-1000.0, 0, 0)));
EXPECT_FALSE(content.Check(Eigen::Vector3d(0, 1000.0, 0)));
EXPECT_FALSE(content.Check(Eigen::Vector3d(0, -1000.0, 0)));
content.GetCopy(&content2);
EXPECT_EQ(content.range_, content2.range_);
EXPECT_EQ(content.cell_size_, content2.cell_size_);
EXPECT_TRUE(SceneManager::Instance().Init());
LidarFrame frame;
MockData(&frame);
HdmapROIFilter roi_filter;
ACHECK(roi_filter.Init(ROIFilterInitOptions()));
ACHECK(roi_filter.Filter(ROIFilterOptions(), &frame));
auto roi_service = std::dynamic_pointer_cast<ROIService>(
SceneManager::Instance().Service("ROIService"));
ACHECK(roi_service);
EXPECT_EQ(roi_service->Name(), "ROIService");
base::PointIndices filter_indices;
for (size_t i = 0; i < frame.world_cloud->size(); ++i) {
auto& pt = frame.world_cloud->at(i);
Eigen::Vector3d world_pt(pt.x, pt.y, pt.z);
if (roi_service->QueryIsPointInROI(world_pt)) {
filter_indices.indices.push_back(static_cast<int>(i));
}
}
EXPECT_EQ(filter_indices.indices.size(), frame.roi_indices.indices.size());
for (size_t i = 0; i < filter_indices.indices.size(); ++i) {
EXPECT_EQ(filter_indices.indices[i], frame.roi_indices.indices[i]);
}
}
前面的check数据部分我们直接跳过，来到HdmapROIFilter，首先初始化传入ROIFilterInitOptions()，然后调用前面篇章所讲的Filter方法滤除roi区域外的点云部分。最后再遍历world_cloud点云内的所有点，检查是否属于roi区域内，如果在则压入到filter_indices.indices内，最后比较两者点的数量和索引。
总结
这里单元测试相当于是告诉我们如何结合地图数据和激光雷达点云数据使用这个hdmap_roi_filter模块。
461
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
