Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
9f86132a
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
9f86132a
编写于
7月 10, 2020
作者:
J
Jiahao Chen
提交者:
Calvin Miao
7月 10, 2020
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Perception: down sample point cloud through filtering beams
上级
cacf540b
变更
7
隐藏空白更改
内联
并排
Showing
7 changed file
with
62 addition
and
9 deletion
+62
-9
modules/perception/base/point_cloud.h
modules/perception/base/point_cloud.h
+3
-0
modules/perception/base/point_cloud_util.cc
modules/perception/base/point_cloud_util.cc
+19
-0
modules/perception/base/point_cloud_util.h
modules/perception/base/point_cloud_util.h
+4
-0
modules/perception/common/perception_gflags.cc
modules/perception/common/perception_gflags.cc
+5
-1
modules/perception/common/perception_gflags.h
modules/perception/common/perception_gflags.h
+2
-0
modules/perception/lidar/lib/detection/lidar_point_pillars/point_pillars_detection.cc
.../detection/lidar_point_pillars/point_pillars_detection.cc
+22
-5
modules/perception/production/conf/perception/perception_common.flag
...ception/production/conf/perception/perception_common.flag
+7
-3
未找到文件。
modules/perception/base/point_cloud.h
浏览文件 @
9f86132a
...
...
@@ -451,6 +451,9 @@ class AttributePointCloud : public PointCloud<PointT> {
const
std
::
vector
<
int32_t
>&
points_beam_id
()
const
{
return
points_beam_id_
;
}
std
::
vector
<
int32_t
>*
mutable_points_beam_id
()
{
return
&
points_beam_id_
;
}
int32_t
&
points_beam_id
(
size_t
i
)
{
return
points_beam_id_
[
i
];
}
const
int32_t
&
points_beam_id
(
size_t
i
)
const
{
return
points_beam_id_
[
i
];
}
const
std
::
vector
<
uint8_t
>&
points_label
()
const
{
return
points_label_
;
}
std
::
vector
<
uint8_t
>*
mutable_points_label
()
{
return
&
points_label_
;
}
...
...
modules/perception/base/point_cloud_util.cc
浏览文件 @
9f86132a
...
...
@@ -24,6 +24,25 @@ namespace apollo {
namespace
perception
{
namespace
base
{
bool
DownSamplePointCloudBeams
(
base
::
PointFCloudPtr
cloud_ptr
,
base
::
PointFCloudPtr
out_cloud_ptr
,
int
downsample_factor
)
{
if
(
downsample_factor
<=
0
)
{
return
false
;
}
for
(
size_t
i
=
0
;
i
<
cloud_ptr
->
size
();
++
i
)
{
int32_t
beam_id
=
cloud_ptr
->
points_beam_id
(
i
);
if
(
beam_id
%
downsample_factor
==
0
)
{
base
::
PointF
point
=
cloud_ptr
->
at
(
i
);
double
timestamp
=
cloud_ptr
->
points_timestamp
(
i
);
float
height
=
cloud_ptr
->
points_height
(
i
);
uint8_t
label
=
cloud_ptr
->
points_label
(
i
);
out_cloud_ptr
->
push_back
(
point
,
timestamp
,
height
,
beam_id
,
label
);
}
}
return
true
;
}
void
GetPointCloudCentroid
(
const
PointFCloud
&
cloud
,
PointF
*
centroid
)
{
for
(
size_t
i
=
0
;
i
<
cloud
.
size
();
++
i
)
{
centroid
->
x
+=
cloud
[
i
].
x
;
...
...
modules/perception/base/point_cloud_util.h
浏览文件 @
9f86132a
...
...
@@ -43,6 +43,10 @@ struct BoundingCube {
float
trans_z
;
// center of points
};
bool
DownSamplePointCloudBeams
(
base
::
PointFCloudPtr
cloud_ptr
,
base
::
PointFCloudPtr
out_cloud_ptr
,
int
downsample_factor
);
bool
GetPointCloudMinareaBbox
(
const
base
::
PointFCloud
&
pc
,
BoundingCube
*
box
,
const
int
&
min_num_points
=
5
,
const
bool
&
verbose
=
false
);
...
...
modules/perception/common/perception_gflags.cc
浏览文件 @
9f86132a
...
...
@@ -52,7 +52,11 @@ DEFINE_double(normalizing_factor, 255,
DEFINE_int32
(
num_point_feature
,
5
,
"Length of raw point feature. Features include x, y, z,"
"intensity and delta of time."
);
DEFINE_bool
(
enable_downsample_pointcloud
,
true
,
DEFINE_bool
(
enable_downsample_beams
,
false
,
"Enable down sampling point cloud beams with a factor."
);
DEFINE_int32
(
downsample_beams_factor
,
4
,
"Down sample point cloud beams with this factor."
);
DEFINE_bool
(
enable_downsample_pointcloud
,
false
,
"Enable down sampling point cloud into centroids of voxel grid."
);
DEFINE_double
(
downsample_voxel_size_x
,
0.01
,
"X-axis size of voxels used for down sampling point cloud."
);
...
...
modules/perception/common/perception_gflags.h
浏览文件 @
9f86132a
...
...
@@ -37,6 +37,8 @@ DECLARE_string(pfe_onnx_file);
DECLARE_string
(
rpn_onnx_file
);
DECLARE_double
(
normalizing_factor
);
DECLARE_int32
(
num_point_feature
);
DECLARE_bool
(
enable_downsample_beams
);
DECLARE_int32
(
downsample_beams_factor
);
DECLARE_bool
(
enable_downsample_pointcloud
);
DECLARE_double
(
downsample_voxel_size_x
);
DECLARE_double
(
downsample_voxel_size_y
);
...
...
modules/perception/lidar/lib/detection/lidar_point_pillars/point_pillars_detection.cc
浏览文件 @
9f86132a
...
...
@@ -24,6 +24,7 @@
#include "cyber/common/log.h"
#include "modules/perception/base/object_pool_types.h"
#include "modules/perception/base/point_cloud_util.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/lidar/common/lidar_timer.h"
#include "modules/perception/lidar/common/pcl_util.h"
...
...
@@ -77,7 +78,19 @@ bool PointPillarsDetection::Detect(const DetectionOptions& options,
Timer
timer
;
int
num_points
;
cur_cloud_ptr_
=
std
::
make_shared
<
base
::
AttributePointCloud
<
base
::
PointF
>>
();
cur_cloud_ptr_
=
std
::
make_shared
<
base
::
PointFCloud
>
(
*
original_cloud_
);
// down sample the point cloud through filtering beams
if
(
FLAGS_enable_downsample_beams
)
{
base
::
PointFCloudPtr
downsample_beams_cloud_ptr
(
new
base
::
PointFCloud
());
if
(
DownSamplePointCloudBeams
(
original_cloud_
,
downsample_beams_cloud_ptr
,
FLAGS_downsample_beams_factor
))
{
cur_cloud_ptr_
=
downsample_beams_cloud_ptr
;
}
else
{
AWARN
<<
"Down sample beams factor must be >= 1. Cancel down sampling."
" Current factor: "
<<
FLAGS_downsample_beams_factor
;
}
}
// down sample the point cloud through filtering voxel grid
if
(
FLAGS_enable_downsample_pointcloud
)
{
...
...
@@ -85,21 +98,25 @@ bool PointPillarsDetection::Detect(const DetectionOptions& options,
new
pcl
::
PointCloud
<
pcl
::
PointXYZI
>
());
pcl
::
PointCloud
<
pcl
::
PointXYZI
>::
Ptr
filtered_cloud_ptr
(
new
pcl
::
PointCloud
<
pcl
::
PointXYZI
>
());
TransformToPCLXYZI
(
*
original_cloud
_
,
pcl_cloud_ptr
);
TransformToPCLXYZI
(
*
cur_cloud_ptr
_
,
pcl_cloud_ptr
);
DownSampleCloudByVoxelGrid
(
pcl_cloud_ptr
,
filtered_cloud_ptr
,
FLAGS_downsample_voxel_size_x
,
FLAGS_downsample_voxel_size_y
,
FLAGS_downsample_voxel_size_z
);
// transform pcl point cloud to apollo point cloud
TransformFromPCLXYZI
(
filtered_cloud_ptr
,
cur_cloud_ptr_
);
base
::
PointFCloudPtr
downsample_voxel_cloud_ptr
(
new
base
::
PointFCloud
());
TransformFromPCLXYZI
(
filtered_cloud_ptr
,
downsample_voxel_cloud_ptr
);
cur_cloud_ptr_
=
downsample_voxel_cloud_ptr
;
}
downsample_time_
=
timer
.
toc
(
true
);
num_points
=
cur_cloud_ptr_
->
size
();
A
DEBUG
<<
"num points before fusing: "
<<
num_points
;
A
INFO
<<
"num points before fusing: "
<<
num_points
;
// fuse clouds of preceding frames with current cloud
cur_cloud_ptr_
->
mutable_points_timestamp
()
->
assign
(
cur_cloud_ptr_
->
size
(),
0.0
);
if
(
FLAGS_enable_fuse_frames
&&
FLAGS_num_fuse_frames
>
1
)
{
// before fusing
while
(
!
prev_world_clouds_
.
empty
()
&&
...
...
@@ -136,7 +153,7 @@ bool PointPillarsDetection::Detect(const DetectionOptions& options,
}
prev_world_clouds_
.
emplace_back
(
cur_world_cloud_ptr
);
}
A
DEBUG
<<
"num points after fusing: "
<<
num_points
;
A
INFO
<<
"num points after fusing: "
<<
num_points
;
fuse_time_
=
timer
.
toc
(
true
);
// shuffle points and cut off
...
...
modules/perception/production/conf/perception/perception_common.flag
浏览文件 @
9f86132a
...
...
@@ -86,10 +86,14 @@
--config_manager_path=./
--start_visualizer=false
# configs for lidar point pillars
--score_threshold=0.7
###########################################################################
# Flags from lidar_point_pillars
--score_threshold=0.5
--enable_downsample_beams=false
--downsample_beams_factor=4
--enable_downsample_pointcloud=false
--downsample_voxel_size_x=0.1
--downsample_voxel_size_y=0.1
--downsample_voxel_size_z=0.1
--enable_fuse_frames=false
--num_fuse_frames=5
\ No newline at end of file
--num_fuse_frames=5
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录