Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
c878c724
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,发现更多精彩内容 >>
提交
c878c724
编写于
7月 27, 2017
作者:
J
Jun Zhu
提交者:
Jiangtao Hu
7月 27, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
[perception] add CNNSegmentation method (#81)
上级
72355b12
变更
22
隐藏空白更改
内联
并排
Showing
22 changed file
with
1995 addition
and
14 deletion
+1995
-14
WORKSPACE.in
WORKSPACE.in
+9
-0
modules/perception/conf/BUILD
modules/perception/conf/BUILD
+5
-0
modules/perception/conf/cnn_segmentation.conf
modules/perception/conf/cnn_segmentation.conf
+18
-0
modules/perception/conf/perception.conf
modules/perception/conf/perception.conf
+6
-1
modules/perception/data/models/cnn_segmentation/.gitattributes
...es/perception/data/models/cnn_segmentation/.gitattributes
+1
-0
modules/perception/data/models/cnn_segmentation/cnnseg.conf
modules/perception/data/models/cnn_segmentation/cnnseg.conf
+36
-0
modules/perception/data/models/cnn_segmentation/deploy.caffemodel
...perception/data/models/cnn_segmentation/deploy.caffemodel
+0
-0
modules/perception/data/models/cnn_segmentation/deploy.prototxt
...s/perception/data/models/cnn_segmentation/deploy.prototxt
+674
-0
modules/perception/obstacle/base/object.h
modules/perception/obstacle/base/object.h
+3
-0
modules/perception/obstacle/common/BUILD
modules/perception/obstacle/common/BUILD
+1
-0
modules/perception/obstacle/common/disjoint_set.h
modules/perception/obstacle/common/disjoint_set.h
+75
-0
modules/perception/obstacle/lidar/interface/base_segmentation.h
...s/perception/obstacle/lidar/interface/base_segmentation.h
+11
-4
modules/perception/obstacle/lidar/segmentation/cnnseg/BUILD
modules/perception/obstacle/lidar/segmentation/cnnseg/BUILD
+54
-0
modules/perception/obstacle/lidar/segmentation/cnnseg/cluster2d.h
...perception/obstacle/lidar/segmentation/cnnseg/cluster2d.h
+339
-0
modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cpp
...n/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cpp
+185
-3
modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.h
...ion/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.h
+89
-6
modules/perception/obstacle/lidar/segmentation/cnnseg/feature_generator.cpp
.../obstacle/lidar/segmentation/cnnseg/feature_generator.cpp
+185
-0
modules/perception/obstacle/lidar/segmentation/cnnseg/feature_generator.h
...on/obstacle/lidar/segmentation/cnnseg/feature_generator.h
+119
-0
modules/perception/obstacle/lidar/segmentation/cnnseg/proto/BUILD
...perception/obstacle/lidar/segmentation/cnnseg/proto/BUILD
+10
-0
modules/perception/obstacle/lidar/segmentation/cnnseg/proto/cnnseg.proto
...ion/obstacle/lidar/segmentation/cnnseg/proto/cnnseg.proto
+68
-0
modules/perception/obstacle/lidar/segmentation/cnnseg/util.h
modules/perception/obstacle/lidar/segmentation/cnnseg/util.h
+80
-0
third_party/caffe.BUILD
third_party/caffe.BUILD
+27
-0
未找到文件。
WORKSPACE.in
浏览文件 @
c878c724
...
@@ -193,6 +193,15 @@ new_local_repository(
...
@@ -193,6 +193,15 @@ new_local_repository(
path = "/usr/include/pcl-1.7",
path = "/usr/include/pcl-1.7",
)
)
# Caffe
# =====
# This requires Caffe being installed in docker image.
new_local_repository(
name = "caffe",
build_file = "third_party/caffe.BUILD",
path = "/third_party/caffe",
)
# YAML-CPP
# YAML-CPP
new_http_archive(
new_http_archive(
name = "yaml_cpp",
name = "yaml_cpp",
...
...
modules/perception/conf/BUILD
浏览文件 @
c878c724
...
@@ -6,3 +6,8 @@ filegroup(
...
@@ -6,3 +6,8 @@ filegroup(
"adapter.conf"
,
"adapter.conf"
,
],
],
)
)
filegroup
(
name
=
"perception_cnn_segmentation_config"
,
srcs
=
[
"cnn_segmentation.conf"
],
)
modules/perception/conf/cnn_segmentation.conf
0 → 100644
浏览文件 @
c878c724
model_configs
{
name
:
"CNNSegmentation"
version
:
"1.0.0"
string_params
{
name
:
"config_file"
value
:
"./data/models/cnn_segmentation/cnnseg.conf"
}
string_params
{
name
:
"proto_file"
value
:
"./data/models/cnn_segmentation/deploy.prototxt"
}
string_params
{
name
:
"weight_file"
value
:
"./data/models/cnn_segmentation/deploy.caffemodel"
}
}
\ No newline at end of file
modules/perception/conf/perception.conf
浏览文件 @
c878c724
...
@@ -21,4 +21,9 @@
...
@@ -21,4 +21,9 @@
# enable hdmap input for roi filter
# enable hdmap input for roi filter
# type: bool
# type: bool
# default: false
# default: false
--
enable_hdmap_input
=
true
--
enable_hdmap_input
=
true
\ No newline at end of file
# Segemntation
# type: string
#default:
--
onboard_segmentation
=
CNNSegmentation
\ No newline at end of file
modules/perception/data/models/cnn_segmentation/.gitattributes
0 → 100644
浏览文件 @
c878c724
*.caffemodel filter=lfs diff=lfs merge=lfs -text
\ No newline at end of file
modules/perception/data/models/cnn_segmentation/cnnseg.conf
0 → 100644
浏览文件 @
c878c724
confidence_thresh
:
0
.
1
height_thresh
:
0
.
5
#return_bg: true
min_pts_num
:
3
use_full_cloud
:
true
gpu_id
:
0
network_param
{
instance_pt_blob
:
"instance_pt"
category_pt_blob
:
"category_score"
confidence_pt_blob
:
"confidence_score"
classify_pt_blob
:
"class_score"
heading_pt_blob
:
"heading_pt"
height_pt_blob
:
"height_pt"
feature_blob
:
"data"
}
feature_param
{
use_max_height
:
true
use_mean_height
:
true
use_log_count
:
true
use_direction
:
true
use_top_intensity
:
true
use_mean_intensity
:
true
use_distance
:
true
use_nonempty
:
true
use_height_filter
:
true
use_dense_feat
:
true
width
:
512
height
:
512
point_cloud_range
:
60
}
modules/perception/data/models/cnn_segmentation/deploy.caffemodel
0 → 100644
浏览文件 @
c878c724
文件已添加
modules/perception/data/models/cnn_segmentation/deploy.prototxt
0 → 100644
浏览文件 @
c878c724
name: "pcd_parsing"
layer {
name: "input"
type: "Input"
top: "data"
input_param {
shape {
dim: 1
dim: 8
dim: 512
dim: 512
}
}
}
layer {
name: "slice_[dump, mask]"
type: "Slice"
bottom: "data"
top: "dump_blob"
top: "mask"
slice_param {
slice_point: 7
axis: 1
}
}
layer {
name: "Silence_[dump_blob]"
type: "Silence"
bottom: "dump_blob"
}
layer {
name: "conv1_1"
type: "Convolution"
bottom: "data"
top: "conv1_1"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 48
pad: 1
kernel_size: 3
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "relu1_1"
type: "ReLU"
bottom: "conv1_1"
top: "conv1_1"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "conv1_2"
type: "Convolution"
bottom: "conv1_1"
top: "conv1_2"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 48
pad: 1
kernel_size: 3
stride: 1
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "relu1_2"
type: "ReLU"
bottom: "conv1_2"
top: "conv1_2"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "conv2_1"
type: "Convolution"
bottom: "conv1_2"
top: "conv2_1"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 64
pad: 1
kernel_size: 3
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "conv2_1_relu"
type: "ReLU"
bottom: "conv2_1"
top: "conv2_1"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "conv2_2"
type: "Convolution"
bottom: "conv2_1"
top: "conv2_2"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 64
pad: 1
kernel_size: 3
stride: 1
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "conv2_2_relu"
type: "ReLU"
bottom: "conv2_2"
top: "conv2_2"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "conv3_1"
type: "Convolution"
bottom: "conv2_2"
top: "conv3_1"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 96
pad: 1
kernel_size: 3
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "conv3_1_relu"
type: "ReLU"
bottom: "conv3_1"
top: "conv3_1"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "conv4_1"
type: "Convolution"
bottom: "conv3_1"
top: "conv4_1"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 128
pad: 1
kernel_size: 3
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "conv4_1_relu"
type: "ReLU"
bottom: "conv4_1"
top: "conv4_1"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "conv5_1"
type: "Convolution"
bottom: "conv4_1"
top: "conv5_1"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 192
pad: 1
kernel_size: 3
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "conv5_1_relu"
type: "ReLU"
bottom: "conv5_1"
top: "conv5_1"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine5_deconv"
type: "Deconvolution"
bottom: "conv5_1"
top: "refine5_deconv"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 128
pad: 1
kernel_size: 4
group: 1
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
}
}
}
layer {
name: "refine5_deconv_relu"
type: "ReLU"
bottom: "refine5_deconv"
top: "refine5_deconv"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine4_concat"
type: "Concat"
bottom: "conv4_1"
bottom: "refine5_deconv"
top: "refine4_concat"
}
layer {
name: "refine4_conv_3x3_2"
type: "Convolution"
bottom: "refine4_concat"
top: "refine4_conv_3x3_2"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 128
pad: 1
kernel_size: 3
stride: 1
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "refine4_conv_3x3_2_relu"
type: "ReLU"
bottom: "refine4_conv_3x3_2"
top: "refine4_conv_3x3_2"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine4_deconv"
type: "Deconvolution"
bottom: "refine4_conv_3x3_2"
top: "refine4_deconv"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 96
pad: 1
kernel_size: 4
group: 1
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
}
}
}
layer {
name: "refine4_deconv_relu"
type: "ReLU"
bottom: "refine4_deconv"
top: "refine4_deconv"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine3_concat"
type: "Concat"
bottom: "conv3_1"
bottom: "refine4_deconv"
top: "refine3_concat"
}
layer {
name: "refine3_conv_3x3_2"
type: "Convolution"
bottom: "refine3_concat"
top: "refine3_conv_3x3_2"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 96
pad: 1
kernel_size: 3
stride: 1
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "refine3_conv_3x3_2_relu"
type: "ReLU"
bottom: "refine3_conv_3x3_2"
top: "refine3_conv_3x3_2"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine3_deconv"
type: "Deconvolution"
bottom: "refine3_conv_3x3_2"
top: "refine3_deconv"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 64
pad: 1
kernel_size: 4
group: 1
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
}
}
}
layer {
name: "refine3_deconv_relu"
type: "ReLU"
bottom: "refine3_deconv"
top: "refine3_deconv"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine2_concat"
type: "Concat"
bottom: "conv2_2"
bottom: "refine3_deconv"
top: "refine2_concat"
}
layer {
name: "refine2_conv_3x3_2"
type: "Convolution"
bottom: "refine2_concat"
top: "refine2_conv_3x3_2"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 64
pad: 1
kernel_size: 3
stride: 1
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "refine2_conv_3x3_2_relu"
type: "ReLU"
bottom: "refine2_conv_3x3_2"
top: "refine2_conv_3x3_2"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine2_deconv"
type: "Deconvolution"
bottom: "refine2_conv_3x3_2"
top: "refine2_deconv"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 48
pad: 1
kernel_size: 4
group: 1
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
}
}
}
layer {
name: "refine2_deconv_relu"
type: "ReLU"
bottom: "refine2_deconv"
top: "refine2_deconv"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "refine1_concat"
type: "Concat"
bottom: "conv1_2"
bottom: "refine2_deconv"
top: "refine1_concat"
}
layer {
name: "refine1_conv_3x3_2"
type: "Convolution"
bottom: "refine1_concat"
top: "refine1_conv_3x3_2"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 48
pad: 1
kernel_size: 3
stride: 1
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
value: 0.0
}
}
}
layer {
name: "refine1_conv_3x3_2_relu"
type: "ReLU"
bottom: "refine1_conv_3x3_2"
top: "refine1_conv_3x3_2"
relu_param {
negative_slope: 0.0
}
}
layer {
name: "predict"
type: "Deconvolution"
bottom: "refine1_conv_3x3_2"
top: "predict"
param {
lr_mult: 1.0
decay_mult: 0.0
}
param {
lr_mult: 1.0
decay_mult: 0.0
}
convolution_param {
num_output: 12
pad: 1
kernel_size: 4
group: 1
stride: 2
weight_filler {
type: "xavier"
}
bias_filler {
type: "constant"
}
}
}
layer {
name: "Slice_[\'category_pt\', \'instance_pt\', \'confidence_pt\', \'classify_pt\', \'heading_pt\', \'height_pt\']"
type: "Slice"
bottom: "predict"
top: "category_pt"
top: "instance_pt"
top: "confidence_pt"
top: "classify_pt"
top: "heading_pt"
top: "height_pt"
slice_param {
slice_point: 1
slice_point: 3
slice_point: 4
slice_point: 9
slice_point: 11
}
}
layer {
name: "all_category_score"
type: "Sigmoid"
bottom: "category_pt"
top: "all_category_score"
propagate_down: false
}
layer {
name: "instance_ignore_layer"
type: "Eltwise"
bottom: "all_category_score"
bottom: "mask"
top: "category_score"
eltwise_param {
operation: PROD
}
}
layer {
name: "confidence_score"
type: "Sigmoid"
bottom: "confidence_pt"
top: "confidence_score"
propagate_down: false
}
layer {
name: "class_score"
type: "Sigmoid"
bottom: "classify_pt"
top: "class_score"
propagate_down: false
}
modules/perception/obstacle/base/object.h
浏览文件 @
c878c724
...
@@ -59,6 +59,9 @@ struct alignas(16) Object {
...
@@ -59,6 +59,9 @@ struct alignas(16) Object {
double
width
=
0.0
;
double
width
=
0.0
;
double
height
=
0.0
;
double
height
=
0.0
;
// foreground score/probability
float
score
=
0.0
;
// Object classification type.
// Object classification type.
ObjectType
type
;
ObjectType
type
;
// Probability of each type, used for track type.
// Probability of each type, used for track type.
...
...
modules/perception/obstacle/common/BUILD
浏览文件 @
c878c724
...
@@ -9,6 +9,7 @@ cc_library(
...
@@ -9,6 +9,7 @@ cc_library(
],
],
hdrs
=
[
hdrs
=
[
"convex_hullxy.h"
,
"convex_hullxy.h"
,
"disjoint_set.h"
,
"geometry_util.h"
,
"geometry_util.h"
,
],
],
deps
=
[
deps
=
[
...
...
modules/perception/obstacle/common/disjoint_set.h
0 → 100644
浏览文件 @
c878c724
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_
#define MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_
namespace
apollo
{
namespace
perception
{
template
<
class
T
>
void
DisjointSetMakeSet
(
T
*
x
)
{
x
->
parent
=
x
;
x
->
node_rank
=
0
;
}
template
<
class
T
>
T
*
DisjointSetFindRecursive
(
T
*
x
)
{
if
(
x
->
parent
!=
x
)
{
x
->
parent
=
DisjointSetFindRecursive
(
x
->
parent
);
}
return
x
->
parent
;
}
template
<
class
T
>
T
*
DisjointSetFind
(
T
*
x
)
{
T
*
y
=
x
->
parent
;
if
(
y
==
x
||
y
->
parent
==
y
)
{
return
y
;
}
T
*
root
=
DisjointSetFindRecursive
(
y
->
parent
);
x
->
parent
=
root
;
y
->
parent
=
root
;
return
root
;
}
template
<
class
T
>
void
DisjointSetMerge
(
T
*
x
,
const
T
*
y
)
{}
template
<
class
T
>
void
DisjointSetUnion
(
T
*
x
,
T
*
y
)
{
x
=
DisjointSetFind
(
x
);
y
=
DisjointSetFind
(
y
);
if
(
x
==
y
)
{
return
;
}
if
(
x
->
node_rank
<
y
->
node_rank
)
{
x
->
parent
=
y
;
DisjointSetMerge
(
y
,
x
);
}
else
if
(
y
->
node_rank
<
x
->
node_rank
)
{
y
->
parent
=
x
;
DisjointSetMerge
(
x
,
y
);
}
else
{
y
->
parent
=
x
;
x
->
node_rank
++
;
DisjointSetMerge
(
x
,
y
);
}
}
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_
modules/perception/obstacle/lidar/interface/base_segmentation.h
浏览文件 @
c878c724
...
@@ -65,7 +65,14 @@
...
@@ -65,7 +65,14 @@
namespace
apollo
{
namespace
apollo
{
namespace
perception
{
namespace
perception
{
struct
SegmentationOptions
{};
struct
SegmentationOptions
{
// original point cloud without ROI filtering
pcl_util
::
PointCloudPtr
origin_cloud
;
// indices of roi-filtered cloud in original cloud if enabled
pcl_util
::
PointIndicesPtr
roi_cloud_indices
;
// indices of non-ground points in original clound if enabled
pcl_util
::
PointIndicesPtr
non_ground_indices
;
};
class
BaseSegmentation
{
class
BaseSegmentation
{
public:
public:
...
@@ -76,11 +83,11 @@ class BaseSegmentation {
...
@@ -76,11 +83,11 @@ class BaseSegmentation {
// @brief: segment the point cloud.
// @brief: segment the point cloud.
// @param [in]: input point cloud.
// @param [in]: input point cloud.
// @param [in]:
non ground points index
.
// @param [in]:
valid indices of points for segmentation
.
// @param [in]: s
ome
options
// @param [in]: s
egmentation
options
// @param [out]: segmented object.
// @param [out]: segmented object.
virtual
bool
Segment
(
const
pcl_util
::
PointCloudPtr
&
cloud
,
virtual
bool
Segment
(
const
pcl_util
::
PointCloudPtr
&
cloud
,
const
pcl_util
::
PointIndices
&
non_groun
d_indices
,
const
pcl_util
::
PointIndices
&
vali
d_indices
,
const
SegmentationOptions
&
options
,
const
SegmentationOptions
&
options
,
std
::
vector
<
ObjectPtr
>
*
objects
)
=
0
;
std
::
vector
<
ObjectPtr
>
*
objects
)
=
0
;
...
...
modules/perception/obstacle/lidar/segmentation/cnnseg/BUILD
0 → 100644
浏览文件 @
c878c724
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"perception_obstacle_lidar_segmention_cnnseg"
,
srcs
=
[
"cnn_segmentation.cpp"
],
hdrs
=
[
"cnn_segmentation.h"
],
deps
=
[
"@caffe//:caffe"
,
"//modules/common:log"
,
"//modules/common/util:util"
,
"//modules/perception/lib/pcl_util:pcl_util"
,
"//modules/perception/obstacle/base:perception_obstacle_base"
,
"//modules/perception/obstacle/lidar/interface:perception_obstacle_lidar_interface"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg/proto:cnnseg_proto"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg:cnnseg_util"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg:cnnseg_feature_generator"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg:cnnseg_cluster2d"
,
"//modules/perception/lib/base:base"
,
"//modules/perception/lib/config_manager:config_manager"
],
)
cc_library
(
name
=
"cnnseg_util"
,
hdrs
=
[
"util.h"
],
deps
=
[
"@opencv2//:core"
],
)
cc_library
(
name
=
"cnnseg_feature_generator"
,
srcs
=
[
"feature_generator.cpp"
],
hdrs
=
[
"feature_generator.h"
],
deps
=
[
"@caffe//:caffe"
,
"//modules/common:log"
,
"//modules/perception/lib/pcl_util:pcl_util"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg/proto:cnnseg_proto"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg:cnnseg_util"
,
],
)
cc_library
(
name
=
"cnnseg_cluster2d"
,
hdrs
=
[
"cluster2d.h"
],
deps
=
[
"@caffe//:caffe"
,
"//modules/perception/lib/pcl_util:pcl_util"
,
"//modules/perception/obstacle/base:perception_obstacle_base"
,
"//modules/perception/obstacle/common:perception_obstacle_common"
,
"//modules/perception/obstacle/lidar/segmentation/cnnseg:cnnseg_util"
,
],
)
\ No newline at end of file
modules/perception/obstacle/lidar/segmentation/cnnseg/cluster2d.h
0 → 100644
浏览文件 @
c878c724
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_CLUSTER2D_H_
#define MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_CLUSTER2D_H_
#include <vector>
#include "caffe/caffe.hpp"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/base/object.h"
#include "modules/perception/obstacle/common/disjoint_set.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/util.h"
namespace
apollo
{
namespace
perception
{
namespace
cnnseg
{
struct
Obstacle
{
std
::
vector
<
int
>
grids
;
apollo
::
perception
::
pcl_util
::
PointCloudPtr
cloud
;
float
score
;
float
height
;
Obstacle
()
{
grids
.
clear
();
cloud
.
reset
(
new
apollo
::
perception
::
pcl_util
::
PointCloud
);
score
=
0.0
;
height
=
0.0
;
}
};
class
Cluster2D
{
public:
void
Init
(
const
int
rows
,
const
int
cols
,
const
float
range
)
{
rows_
=
rows
;
cols_
=
cols
;
grids_
=
rows_
*
cols_
;
range_
=
range
;
scale_
=
0.5
*
static_cast
<
float
>
(
rows_
)
/
range_
;
inv_res_x_
=
0.5
*
static_cast
<
float
>
(
cols_
)
/
range_
;
inv_res_y_
=
0.5
*
static_cast
<
float
>
(
rows_
)
/
range_
;
//point2grid_.clear();
//obstacles_.clear();
//id_img_.assign(rows_ * cols_, -1);
//pc_ptr_ = nullptr;
pc_ptr_
.
reset
();
valid_indices_in_pc_
=
nullptr
;
}
void
Cluster
(
const
caffe
::
Blob
<
float
>&
category_pt_blob
,
const
caffe
::
Blob
<
float
>&
instance_pt_blob
,
const
apollo
::
perception
::
pcl_util
::
PointCloudPtr
&
pc_ptr
,
const
apollo
::
perception
::
pcl_util
::
PointIndices
&
valid_indices
)
{
const
float
*
category_pt_data
=
category_pt_blob
.
cpu_data
();
const
float
*
instance_pt_x_data
=
instance_pt_blob
.
cpu_data
();
const
float
*
instance_pt_y_data
=
instance_pt_blob
.
cpu_data
()
+
instance_pt_blob
.
offset
(
0
,
1
);
pc_ptr_
=
pc_ptr
;
std
::
vector
<
std
::
vector
<
Node
>
>
nodes
(
rows_
,
std
::
vector
<
Node
>
(
cols_
,
Node
()));
// map points into grids
size_t
tot_point_num
=
pc_ptr_
->
size
();
const
std
::
vector
<
int
>&
indices
=
valid_indices
.
indices
;
CHECK_LE
(
indices
.
size
(),
tot_point_num
);
valid_indices_in_pc_
=
&
(
valid_indices
.
indices
);
point2grid_
.
assign
(
indices
.
size
(),
-
1
);
for
(
size_t
i
=
0
;
i
<
indices
.
size
();
++
i
)
{
int
point_id
=
indices
[
i
];
CHECK_GE
(
point_id
,
0
);
CHECK_LT
(
point_id
,
static_cast
<
int
>
(
tot_point_num
));
const
auto
&
point
=
pc_ptr_
->
points
[
point_id
];
// * the coordinates of x and y have been exchanged in feature generation step,
// so we swap them back here.
int
pos_x
=
F2I
(
point
.
y
,
range_
,
inv_res_x_
);
int
pos_y
=
F2I
(
point
.
x
,
range_
,
inv_res_y_
);
//if (pos_x < _cols && pos_x >= 0 && pos_y < _rows && pos_y >= 0) {
if
(
IsValidRowCol
(
pos_y
,
pos_x
))
{
point2grid_
[
i
]
=
RowCol2Grid
(
pos_y
,
pos_x
);
nodes
[
pos_y
][
pos_x
].
point_num
++
;
}
}
// construct graph with center offset prediction and objectness
for
(
int
row
=
0
;
row
<
rows_
;
row
++
)
{
for
(
int
col
=
0
;
col
<
cols_
;
col
++
)
{
//int offset = row * cols_ + col;
int
grid
=
RowCol2Grid
(
row
,
col
);
Node
*
node
=
&
nodes
[
row
][
col
];
apollo
::
perception
::
DisjointSetMakeSet
(
node
);
node
->
is_object
=
(
nodes
[
row
][
col
].
point_num
>
0
)
&&
(
*
(
category_pt_data
+
grid
)
>=
0.5
);
int
center_row
=
std
::
round
(
row
+
instance_pt_x_data
[
grid
]
*
scale_
);
int
center_col
=
std
::
round
(
col
+
instance_pt_y_data
[
grid
]
*
scale_
);
center_row
=
std
::
min
(
std
::
max
(
center_row
,
0
),
rows_
-
1
);
center_col
=
std
::
min
(
std
::
max
(
center_col
,
0
),
cols_
-
1
);
node
->
center_node
=
&
nodes
[
center_row
][
center_col
];
}
}
// traverse nodes
for
(
int
row
=
0
;
row
<
rows_
;
row
++
)
{
for
(
int
col
=
0
;
col
<
cols_
;
col
++
)
{
Node
*
node
=
&
nodes
[
row
][
col
];
if
(
node
->
is_object
&&
node
->
traversed
==
0
)
{
Traverse
(
node
);
}
}
}
for
(
int
row
=
0
;
row
<
rows_
;
row
++
)
{
for
(
int
col
=
0
;
col
<
cols_
;
col
++
)
{
Node
*
node
=
&
nodes
[
row
][
col
];
if
(
!
node
->
is_center
)
{
continue
;
}
for
(
int
row2
=
row
-
1
;
row2
<=
row
+
1
;
row2
++
)
{
for
(
int
col2
=
col
-
1
;
col2
<=
col
+
1
;
col2
++
)
{
/*
if (std::abs(row - row2) + std::abs(col - col2) <= 1 &&
row2 >= 0 && row2 < _rows &&
col2 >= 0 && col2 < _cols) {
Node* node2 = &_nodes[row2][col2];
if (node2->is_center) {
disjoint_set_union(node, node2);
}
}
*/
if
((
row2
==
row
||
col2
==
col
)
&&
IsValidRowCol
(
row2
,
col2
))
{
Node
*
node2
=
&
nodes
[
row2
][
col2
];
if
(
node2
->
is_center
)
{
apollo
::
perception
::
DisjointSetUnion
(
node
,
node2
);
}
}
}
}
}
}
int
count_obstacles
=
0
;
obstacles_
.
clear
();
id_img_
.
assign
(
grids_
,
-
1
);
for
(
int
row
=
0
;
row
<
rows_
;
row
++
)
{
for
(
int
col
=
0
;
col
<
cols_
;
col
++
)
{
Node
*
node
=
&
nodes
[
row
][
col
];
if
(
!
node
->
is_object
)
{
continue
;
}
Node
*
root
=
apollo
::
perception
::
DisjointSetFind
(
node
);
if
(
root
->
obstacle_id
<
0
)
{
root
->
obstacle_id
=
count_obstacles
++
;
obstacles_
.
push_back
(
Obstacle
());
}
//id_img_.at<int>(row, col) = root->obstacle_id;
int
grid
=
RowCol2Grid
(
row
,
col
);
//CHECK_GE(root->obstacle_id, 0);
id_img_
[
grid
]
=
root
->
obstacle_id
;
obstacles_
[
root
->
obstacle_id
].
grids
.
push_back
(
grid
);
}
}
CHECK_EQ
(
static_cast
<
int
>
(
count_obstacles
),
obstacles_
.
size
());
}
void
Filter
(
const
caffe
::
Blob
<
float
>&
confidence_pt_blob
,
const
caffe
::
Blob
<
float
>&
height_pt_blob
)
{
const
float
*
confidence_pt_data
=
confidence_pt_blob
.
cpu_data
();
const
float
*
height_pt_data
=
height_pt_blob
.
cpu_data
();
for
(
size_t
obstacle_id
=
0
;
obstacle_id
<
obstacles_
.
size
();
obstacle_id
++
)
{
Obstacle
*
obs
=
&
obstacles_
[
obstacle_id
];
CHECK_GT
(
obs
->
grids
.
size
(),
0
);
double
score
=
0.0
;
double
height
=
0.0
;
for
(
int
grid
:
obs
->
grids
)
{
//int grid = obs->grids[grid_id];
score
+=
static_cast
<
double
>
(
confidence_pt_data
[
grid
]);
height
+=
static_cast
<
double
>
(
height_pt_data
[
grid
]);
}
obs
->
score
=
score
/
static_cast
<
double
>
(
obs
->
grids
.
size
());
obs
->
height
=
height
/
static_cast
<
double
>
(
obs
->
grids
.
size
());
}
}
void
GetObjects
(
const
float
confidence_thresh
,
const
float
height_thresh
,
const
int
min_pts_num
,
std
::
vector
<
ObjectPtr
>*
objects
)
{
/*
for (const auto point : pc_ptr->points) {
// * the coordinates of x and y have been exchanged in feature generation step,
// so we swap them back here.
int pos_x = F2I(point.y, _range, inv_res_x_);
int pos_y = F2I(point.x, _range, inv_res_y_);
//if (pos_x < _cols && pos_x >= 0 && pos_y < _rows && pos_y >= 0) {
if (IsValidRowCol(pos_y, pos_x)) {
//int obs_id = id_img_.at<int>(pos_y, pos_x);
int obstacle_id = id_img_[RowCol2Grid(pos_y, pos_x)];
if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) {
if (height_thresh < 0 ||
point.z <= obstacles_[obstacle_id].height + height_thresh) {
obstacles_[obstacle_id].cloud->push_back(point);
}
}
}
}
*/
//CHECK(pc_ptr_ != nullptr);
CHECK
(
valid_indices_in_pc_
!=
nullptr
);
for
(
size_t
i
=
0
;
i
<
point2grid_
.
size
();
++
i
)
{
int
grid
=
point2grid_
[
i
];
if
(
grid
<
0
)
{
continue
;
}
CHECK_GE
(
grid
,
0
);
CHECK_LT
(
grid
,
grids_
);
int
obstacle_id
=
id_img_
[
grid
];
int
point_id
=
valid_indices_in_pc_
->
at
(
i
);
CHECK_GE
(
point_id
,
0
);
CHECK_LT
(
point_id
,
static_cast
<
int
>
(
pc_ptr_
->
size
()));
if
(
obstacle_id
>=
0
&&
obstacles_
[
obstacle_id
].
score
>=
confidence_thresh
)
{
if
(
height_thresh
<
0
||
pc_ptr_
->
points
[
point_id
].
z
<=
obstacles_
[
obstacle_id
].
height
+
height_thresh
)
{
obstacles_
[
obstacle_id
].
cloud
->
push_back
(
pc_ptr_
->
points
[
point_id
]);
}
}
}
for
(
size_t
obstacle_id
=
0
;
obstacle_id
<
obstacles_
.
size
();
obstacle_id
++
)
{
Obstacle
*
obs
=
&
obstacles_
[
obstacle_id
];
if
(
static_cast
<
int
>
(
obs
->
cloud
->
size
())
<
min_pts_num
)
{
continue
;
}
apollo
::
perception
::
ObjectPtr
out_obj
(
new
apollo
::
perception
::
Object
);
out_obj
->
cloud
=
obs
->
cloud
;
out_obj
->
score
=
obs
->
score
;
objects
->
push_back
(
out_obj
);
}
}
private:
struct
Node
{
Node
*
center_node
;
Node
*
parent
;
char
node_rank
;
char
traversed
;
bool
is_center
;
bool
is_object
;
int
point_num
;
int
obstacle_id
;
Node
()
{
center_node
=
nullptr
;
parent
=
nullptr
;
node_rank
=
0
;
traversed
=
0
;
is_center
=
false
;
is_object
=
false
;
point_num
=
0
;
obstacle_id
=
-
1
;
}
};
inline
bool
IsValidRowCol
(
int
row
,
int
col
)
const
{
return
IsValidRow
(
row
)
&&
IsValidCol
(
col
);
}
inline
bool
IsValidRow
(
int
row
)
const
{
return
row
>=
0
&&
row
<
rows_
;
}
inline
bool
IsValidCol
(
int
col
)
const
{
return
col
>=
0
&&
col
<
cols_
;
}
inline
int
RowCol2Grid
(
int
row
,
int
col
)
const
{
return
row
*
cols_
+
col
;
}
void
Traverse
(
Node
*
x
)
{
std
::
vector
<
Node
*>
p
;
p
.
clear
();
while
(
x
->
traversed
==
0
)
{
p
.
push_back
(
x
);
x
->
traversed
=
2
;
x
=
x
->
center_node
;
}
if
(
x
->
traversed
==
2
)
{
for
(
int
i
=
static_cast
<
int
>
(
p
.
size
())
-
1
;
i
>=
0
&&
p
[
i
]
!=
x
;
i
--
)
{
p
[
i
]
->
is_center
=
true
;
}
x
->
is_center
=
true
;
}
for
(
size_t
i
=
0
;
i
<
p
.
size
();
i
++
)
{
Node
*
y
=
p
[
i
];
y
->
traversed
=
1
;
y
->
parent
=
x
->
parent
;
}
}
int
rows_
;
int
cols_
;
int
grids_
;
float
range_
;
float
scale_
;
float
inv_res_x_
;
float
inv_res_y_
;
apollo
::
perception
::
pcl_util
::
PointCloudPtr
pc_ptr_
;
const
std
::
vector
<
int
>*
valid_indices_in_pc_
=
nullptr
;
std
::
vector
<
int
>
point2grid_
;
std
::
vector
<
int
>
id_img_
;
std
::
vector
<
Obstacle
>
obstacles_
;
};
}
// namespace cnnseg
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_CLUSTER2D_H_
modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cpp
浏览文件 @
c878c724
//
/******************************************************************************
// Created by zhujun08 on 7/23/17.
* Copyright 2017 The Apollo Authors. All Rights Reserved.
//
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.h"
#include "modules/common/util/file.h"
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lib/base/file_util.h"
#ifndef CPU_ONLY
#define CPU_ONLY
#endif
using
std
::
string
;
using
std
::
vector
;
namespace
apollo
{
namespace
perception
{
bool
CNNSegmentation
::
Init
()
{
string
config_file
;
string
proto_file
;
string
weight_file
;
if
(
!
GetConfigs
(
config_file
,
proto_file
,
weight_file
))
{
return
false
;
}
AINFO
<<
"-- config_file: "
<<
config_file
;
AINFO
<<
"-- proto_file: "
<<
proto_file
;
AINFO
<<
"-- weight_file: "
<<
weight_file
;
//cnnseg::load_text_proto_message_file(config_file, cnnseg_param_);
if
(
!
apollo
::
common
::
util
::
GetProtoFromFile
(
config_file
,
&
cnnseg_param_
))
{
AERROR
<<
"Failed to load config file of CNNSegmentation."
;
}
/// set parameters
apollo
::
perception
::
cnnseg
::
NetworkParam
network_param
=
cnnseg_param_
.
network_param
();
apollo
::
perception
::
cnnseg
::
FeatureParam
feature_param
=
cnnseg_param_
.
feature_param
();
range_
=
static_cast
<
int
>
(
feature_param
.
point_cloud_range
());
width_
=
static_cast
<
int
>
(
feature_param
.
width
());
height_
=
static_cast
<
int
>
(
feature_param
.
height
());
/// Instantiate Caffe net
#ifdef CPU_ONLY
caffe
::
Caffe
::
set_mode
(
caffe
::
Caffe
::
CPU
);
#else
CHECK
(
cnnseg_param_
.
gpu_id
()
>=
0
);
caffe
::
Caffe
::
SetDevice
(
cnnseg_param_
.
gpu_id
);
caffe
::
Caffe
::
set_mode
(
caffe
::
Caffe
::
GPU
);
caffe
::
Caffe
::
DeviceQuery
();
#endif
caffe_net_
.
reset
(
new
caffe
::
Net
<
float
>
(
proto_file
,
caffe
::
TEST
));
caffe_net_
->
CopyTrainedLayersFrom
(
weight_file
);
/// set related Caffe blobs
// center offset prediction
instance_pt_blob_
=
caffe_net_
->
blob_by_name
(
network_param
.
instance_pt_blob
());
CHECK
(
instance_pt_blob_
!=
nullptr
)
<<
"`"
<<
network_param
.
instance_pt_blob
()
<<
"` not exists!"
;
// objectness prediction
category_pt_blob_
=
caffe_net_
->
blob_by_name
(
network_param
.
category_pt_blob
());
CHECK
(
category_pt_blob_
!=
nullptr
)
<<
"`"
<<
network_param
.
category_pt_blob
()
<<
"` not exists!"
;
// positiveness (foreground probability) prediction
confidence_pt_blob_
=
caffe_net_
->
blob_by_name
(
network_param
.
confidence_pt_blob
());
CHECK
(
confidence_pt_blob_
!=
nullptr
)
<<
"`"
<<
network_param
.
confidence_pt_blob
()
<<
"` not exists!"
;
// object height prediction
height_pt_blob_
=
caffe_net_
->
blob_by_name
(
network_param
.
height_pt_blob
());
CHECK
(
height_pt_blob_
!=
nullptr
)
<<
"`"
<<
network_param
.
height_pt_blob
()
<<
"` not exists!"
;
// raw feature data
feature_blob_
=
caffe_net_
->
blob_by_name
(
network_param
.
feature_blob
());
CHECK
(
feature_blob_
!=
nullptr
)
<<
"`"
<<
network_param
.
feature_blob
()
<<
"` not exists!"
;
cluster2d_
->
Init
(
height_
,
width_
,
range_
);
if
(
!
feature_generator_
->
Init
(
feature_param
,
feature_blob_
.
get
()))
{
AERROR
<<
"Fail to init feature generator for CNNSegmentation"
;
return
false
;
}
return
true
;
}
bool
CNNSegmentation
::
Segment
(
const
pcl_util
::
PointCloudPtr
&
pc_ptr
,
const
pcl_util
::
PointIndices
&
valid_indices
,
const
SegmentationOptions
&
options
,
vector
<
ObjectPtr
>*
objects
)
{
objects
->
clear
();
int
num_pts
=
static_cast
<
int
>
(
pc_ptr
->
points
.
size
());
if
(
num_pts
==
0
)
{
AINFO
<<
"None of input points, return directly."
;
return
true
;
}
use_full_cloud_
=
cnnseg_param_
.
use_full_cloud
()
&&
options
.
origin_cloud
;
timer_
.
Tic
();
// generate raw features
if
(
use_full_cloud_
)
{
feature_generator_
->
Generate
(
options
.
origin_cloud
);
}
else
{
feature_generator_
->
Generate
(
pc_ptr
);
}
feat_time_
=
timer_
.
Toc
(
true
);
// network forward process
caffe_net_
->
Forward
();
network_time_
=
timer_
.
Toc
(
true
);
// clutser points and construct segments/objects
cluster2d_
->
Cluster
(
*
category_pt_blob_
,
*
instance_pt_blob_
,
pc_ptr
,
valid_indices
);
clust_time_
=
timer_
.
Toc
(
true
);
cluster2d_
->
Filter
(
*
confidence_pt_blob_
,
*
height_pt_blob_
);
cluster2d_
->
GetObjects
(
cnnseg_param_
.
confidence_thresh
(),
cnnseg_param_
.
height_thresh
(),
cnnseg_param_
.
min_pts_num
(),
objects
);
post_time_
=
timer_
.
Toc
(
true
);
tot_time_
=
feat_time_
+
network_time_
+
clust_time_
+
post_time_
;
AINFO
<<
"Total runtime: "
<<
tot_time_
<<
"ms
\t
"
<<
" Raw feature extraction: "
<<
feat_time_
<<
"ms
\t
"
<<
" CNN forward: "
<<
network_time_
<<
"ms
\t
"
<<
" Clustering: "
<<
clust_time_
<<
"ms
\t
"
<<
" Post-processing: "
<<
post_time_
<<
"ms"
;
return
true
;
}
bool
CNNSegmentation
::
GetConfigs
(
string
&
config_file
,
string
&
proto_file
,
string
&
weight_file
)
{
ConfigManager
*
config_manager
=
Singleton
<
ConfigManager
>::
Get
();
CHECK_NOTNULL
(
config_manager
);
const
ModelConfig
*
model_config
=
nullptr
;
if
(
!
config_manager
->
GetModelConfig
(
"CNNSegmentation"
,
&
model_config
))
{
AERROR
<<
"Failed to get model config for CNNSegmentation"
;
return
false
;
}
const
string
&
work_root
=
config_manager
->
work_root
();
if
(
!
model_config
->
GetValue
(
"config_file"
,
&
config_file
))
{
AERROR
<<
"Failed to get value of config_file."
;
return
false
;
}
config_file
=
FileUtil
::
GetAbsolutePath
(
work_root
,
config_file
);
if
(
!
model_config
->
GetValue
(
"proto_file"
,
&
proto_file
))
{
AERROR
<<
"Failed to get value of proto_file."
;
return
false
;
}
proto_file
=
FileUtil
::
GetAbsolutePath
(
work_root
,
proto_file
);
if
(
!
model_config
->
GetValue
(
"weight_file"
,
&
weight_file
))
{
AERROR
<<
"Failed to get value of weight_file."
;
return
false
;
}
weight_file
=
FileUtil
::
GetAbsolutePath
(
work_root
,
weight_file
);
return
true
;
}
REGISTER_SEGMENTATION
(
CNNSegmentation
);
}
// namespace perception
}
// namespace apollo
modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.h
浏览文件 @
c878c724
//
/******************************************************************************
// Created by zhujun08 on 7/23/17.
* Copyright 2017 The Apollo Authors. All Rights Reserved.
//
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef
APOLLO_CNN_SEGMENTATION_H
#ifndef
MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_CNN_SEGMENTATION_H_
#define
APOLLO_CNN_SEGMENTATION_H
#define
MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_CNN_SEGMENTATION_H_
#endif //APOLLO_CNN_SEGMENTATION_H
#include <memory>
#include <string>
#include "caffe/caffe.hpp"
#include "modules/common/log.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/base/object.h"
#include "modules/perception/obstacle/lidar/interface/base_segmentation.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/proto/cnnseg.pb.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/util.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/feature_generator.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/cluster2d.h"
namespace
apollo
{
namespace
perception
{
class
CNNSegmentation
:
public
BaseSegmentation
{
public:
CNNSegmentation
()
:
BaseSegmentation
()
{
}
~
CNNSegmentation
()
{
}
virtual
bool
Init
()
override
;
virtual
bool
Segment
(
const
pcl_util
::
PointCloudPtr
&
pc_ptr
,
const
pcl_util
::
PointIndices
&
valid_indices
,
const
SegmentationOptions
&
options
,
std
::
vector
<
ObjectPtr
>*
objects
)
override
;
virtual
std
::
string
name
()
const
override
{
return
"CNNSegmentation"
;
}
private:
bool
GetConfigs
(
std
::
string
&
config_file
,
std
::
string
&
proto_file
,
std
::
string
&
weight_file
);
int
range_
;
int
width_
;
int
height_
;
apollo
::
perception
::
cnnseg
::
CNNSegParam
cnnseg_param_
;
std
::
shared_ptr
<
caffe
::
Net
<
float
>
>
caffe_net_
;
std
::
shared_ptr
<
cnnseg
::
FeatureGenerator
<
float
>>
feature_generator_
;
boost
::
shared_ptr
<
caffe
::
Blob
<
float
>
>
instance_pt_blob_
;
// center offset vector
boost
::
shared_ptr
<
caffe
::
Blob
<
float
>
>
category_pt_blob_
;
//
boost
::
shared_ptr
<
caffe
::
Blob
<
float
>
>
confidence_pt_blob_
;
//
boost
::
shared_ptr
<
caffe
::
Blob
<
float
>
>
height_pt_blob_
;
boost
::
shared_ptr
<
caffe
::
Blob
<
float
>
>
feature_blob_
;
bool
use_full_cloud_
;
std
::
shared_ptr
<
cnnseg
::
Cluster2D
>
cluster2d_
;
cnnseg
::
Timer
timer_
;
double
feat_time_
=
0.0
;
double
network_time_
=
0.0
;
double
clust_time_
=
0.0
;
double
post_time_
=
0.0
;
double
tot_time_
=
0.0
;
DISALLOW_COPY_AND_ASSIGN
(
CNNSegmentation
);
};
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_CNN_SEGMENTATION_H_
modules/perception/obstacle/lidar/segmentation/cnnseg/feature_generator.cpp
0 → 100644
浏览文件 @
c878c724
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/feature_generator.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/util.h"
using
std
::
vector
;
namespace
apollo
{
namespace
perception
{
namespace
cnnseg
{
#define CV_PI 3.1415926535897932384626433832795
#define EPS 1e-6
template
<
typename
Dtype
>
bool
FeatureGenerator
<
Dtype
>::
Init
(
const
FeatureParam
&
feature_param
,
caffe
::
Blob
<
Dtype
>*
out_blob
)
{
out_blob_
=
out_blob
;
// raw feature parameters
range_
=
(
int
)
feature_param
.
point_cloud_range
();
width_
=
(
int
)
feature_param
.
width
();
height_
=
(
int
)
feature_param
.
height
();
min_height_
=
feature_param
.
min_height
();
max_height_
=
feature_param
.
max_height
();
CHECK_EQ
(
width_
,
height_
)
<<
"Current implementation version requires input_width == input_height."
;
CHECK
(
feature_param
.
use_max_height
());
CHECK
(
feature_param
.
use_mean_height
());
CHECK
(
feature_param
.
use_log_count
());
CHECK
(
feature_param
.
use_direction
());
CHECK
(
feature_param
.
use_top_intensity
());
CHECK
(
feature_param
.
use_mean_intensity
());
CHECK
(
feature_param
.
use_distance
());
CHECK
(
feature_param
.
use_nonempty
());
data_channel_
=
8
;
CHECK
(
feature_param
.
use_height_filter
());
CHECK
(
feature_param
.
use_dense_feat
());
// set output blob and log lookup table
out_blob_
->
Reshape
(
1
,
data_channel_
,
height_
,
width_
);
log_table_
.
resize
(
256
);
for
(
size_t
i
=
0
;
i
<
log_table_
.
size
();
++
i
)
{
log_table_
[
i
]
=
std
::
log
(
static_cast
<
Dtype
>
(
1
+
i
));
}
Dtype
*
out_blob_data
=
nullptr
;
#ifndef CPU_ONLY
log_table_blob_
.
reset
(
new
caffe
::
Blob
<
Dtype
>
(
1
,
1
,
1
,
log_table_
.
size
()));
Dtype
*
log_table_blob_data
=
log_table_blob_
->
mutable_gpu_data
();
caffe
::
caffe_copy
(
log_table_
.
size
(),
log_table_
.
data
(),
log_table_blob_data
);
out_blob_data
=
out_blob_
->
mutable_gpu_data
();
#else
out_blob_data
=
out_blob_
->
mutable_cpu_data
();
#endif
int
channel_index
=
0
;
max_height_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
mean_height_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
count_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
direction_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
top_intensity_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
mean_intensity_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
distance_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
nonempty_data_
=
out_blob_data
+
out_blob_
->
offset
(
0
,
channel_index
++
);
CHECK_EQ
(
out_blob_
->
offset
(
0
,
channel_index
),
out_blob_
->
count
());
// compute direction and distance features
int
siz
=
height_
*
width_
;
vector
<
Dtype
>
direction_data
(
siz
);
vector
<
Dtype
>
distance_data
(
siz
);
for
(
int
row
=
0
;
row
<
height_
;
++
row
)
{
for
(
int
col
=
0
;
col
<
width_
;
++
col
)
{
int
idx
=
row
*
width_
+
col
;
// * row <-> x, column <-> y
float
center_x
=
Pixel2Pc
(
row
,
height_
,
range_
);
float
center_y
=
Pixel2Pc
(
col
,
width_
,
range_
);
direction_data
[
idx
]
=
static_cast
<
Dtype
>
(
std
::
atan2
(
center_y
,
center_x
)
/
(
2.0
*
CV_PI
));
distance_data
[
idx
]
=
static_cast
<
Dtype
>
(
std
::
hypot
(
center_x
,
center_y
)
/
60.0
-
0.5
);
}
}
caffe
::
caffe_copy
(
siz
,
direction_data
.
data
(),
direction_data_
);
caffe
::
caffe_copy
(
siz
,
distance_data
.
data
(),
distance_data_
);
return
true
;
}
template
<
typename
Dtype
>
void
FeatureGenerator
<
Dtype
>::
GenerateByCpu
(
const
apollo
::
perception
::
pcl_util
::
PointCloudConstPtr
&
pc_ptr
)
{
//Timer timer;
const
auto
&
points
=
pc_ptr
->
points
;
// DO NOT remove this line!!!
// Otherwise, the gpu_data will not be updated for the later frames.
// It marks the head at cpu for blob.
out_blob_
->
mutable_cpu_data
();
int
siz
=
height_
*
width_
;
caffe
::
caffe_set
(
siz
,
Dtype
(
-
5
),
max_height_data_
);
caffe
::
caffe_set
(
siz
,
Dtype
(
0
),
mean_height_data_
);
caffe
::
caffe_set
(
siz
,
Dtype
(
0
),
count_data_
);
caffe
::
caffe_set
(
siz
,
Dtype
(
0
),
top_intensity_data_
);
caffe
::
caffe_set
(
siz
,
Dtype
(
0
),
mean_intensity_data_
);
caffe
::
caffe_set
(
siz
,
Dtype
(
0
),
nonempty_data_
);
//AINFO << "\t** feature time: " << timer.toc(true) << "ms";
map_idx_
.
resize
(
points
.
size
());
float
inv_res_x
=
0.5
*
static_cast
<
float
>
(
width_
)
/
static_cast
<
float
>
(
range_
);
float
inv_res_y
=
0.5
*
static_cast
<
float
>
(
height_
)
/
static_cast
<
float
>
(
range_
);
for
(
size_t
i
=
0
;
i
<
points
.
size
();
++
i
)
{
if
(
points
[
i
].
z
<=
min_height_
||
points
[
i
].
z
>=
max_height_
)
{
map_idx_
[
i
]
=
-
1
;
continue
;
}
// * the coordinates of x and y are exchanged here (* row <-> x, column <-> y)
int
pos_x
=
F2I
(
points
[
i
].
y
,
range_
,
inv_res_x
);
int
pos_y
=
F2I
(
points
[
i
].
x
,
range_
,
inv_res_y
);
if
(
pos_x
>=
width_
||
pos_x
<
0
||
pos_y
>=
height_
||
pos_y
<
0
)
{
map_idx_
[
i
]
=
-
1
;
continue
;
}
map_idx_
[
i
]
=
pos_y
*
width_
+
pos_x
;
int
idx
=
map_idx_
[
i
];
float
pz
=
points
[
i
].
z
;
float
pi
=
points
[
i
].
intensity
/
255.0
;
if
(
max_height_data_
[
idx
]
<
pz
)
{
max_height_data_
[
idx
]
=
pz
;
top_intensity_data_
[
idx
]
=
pi
;
}
mean_height_data_
[
idx
]
+=
static_cast
<
Dtype
>
(
pz
);
mean_intensity_data_
[
idx
]
+=
static_cast
<
Dtype
>
(
pi
);
count_data_
[
idx
]
+=
Dtype
(
1
);
}
//AINFO << "\t** feature time: " << timer.toc(true) << "ms";
for
(
int
i
=
0
;
i
<
siz
;
++
i
)
{
if
(
count_data_
[
i
]
<
EPS
)
{
max_height_data_
[
i
]
=
Dtype
(
0
);
}
else
{
mean_height_data_
[
i
]
/=
count_data_
[
i
];
mean_intensity_data_
[
i
]
/=
count_data_
[
i
];
nonempty_data_
[
i
]
=
Dtype
(
1
);
}
count_data_
[
i
]
=
LogCount
(
static_cast
<
int
>
(
count_data_
[
i
]));
}
//AINFO << "\t** feature time: " << timer.toc() << "ms";
}
template
bool
FeatureGenerator
<
float
>
::
Init
(
const
FeatureParam
&
feature_param
,
caffe
::
Blob
<
float
>*
blob
);
template
void
FeatureGenerator
<
float
>
::
GenerateByCpu
(
const
apollo
::
perception
::
pcl_util
::
PointCloudConstPtr
&
pc_ptr
);
template
bool
FeatureGenerator
<
double
>
::
Init
(
const
FeatureParam
&
feature_param
,
caffe
::
Blob
<
double
>*
blob
);
template
void
FeatureGenerator
<
double
>
::
GenerateByCpu
(
const
apollo
::
perception
::
pcl_util
::
PointCloudConstPtr
&
pc_ptr
);
}
// namespace cnnseg
}
// namespace perception
}
// namespace apollo
modules/perception/obstacle/lidar/segmentation/cnnseg/feature_generator.h
0 → 100644
浏览文件 @
c878c724
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_FEATURE_GENERATOR_H_
#define MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_FEATURE_GENERATOR_H
#ifndef CPU_ONLY
#define CPU_ONLY
#endif
#include <string>
#include <cmath>
#include <caffe/caffe.hpp>
#include "modules/common/log.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/proto/cnnseg.pb.h"
namespace
apollo
{
namespace
perception
{
namespace
cnnseg
{
template
<
typename
Dtype
>
class
FeatureGenerator
{
public:
FeatureGenerator
()
{}
~
FeatureGenerator
()
{
#ifndef CPU_ONLY
release_gpu_mem
();
#endif
}
bool
Init
(
const
FeatureParam
&
feature_param
,
caffe
::
Blob
<
Dtype
>*
out_blob
);
inline
void
Generate
(
const
apollo
::
perception
::
pcl_util
::
PointCloudConstPtr
&
pc_ptr
)
{
#ifdef CPU_ONLY
AINFO
<<
"Generate raw features with CPU for CNNSegmentation."
;
GenerateByCpu
(
pc_ptr
);
#else
AINFO
<<
"Generate raw features with GPU for CNNSegmentation."
;
GenerateByGpu
(
pc_ptr
);
#endif
}
#ifdef CPU_ONLY
void
GenerateByCpu
(
const
apollo
::
perception
::
pcl_util
::
PointCloudConstPtr
&
pc_ptr
);
#else
void
GenerateByGpu
(
const
apollo
::
perception
::
pcl_util
::
PointCloudConstPtr
&
pc_ptr
);
#endif
inline
std
::
string
name
()
const
{
return
"FeatureGenerator"
;
}
private:
Dtype
LogCount
(
int
count
)
{
if
(
count
<
static_cast
<
int
>
(
log_table_
.
size
()))
{
return
log_table_
[
count
];
}
return
std
::
log
(
static_cast
<
Dtype
>
(
1
+
count
));
}
std
::
vector
<
Dtype
>
log_table_
;
int
width_
;
int
height_
;
int
data_channel_
;
int
range_
;
float
min_height_
;
float
max_height_
;
// raw feature data
Dtype
*
max_height_data_
=
nullptr
;
Dtype
*
mean_height_data_
=
nullptr
;
Dtype
*
count_data_
=
nullptr
;
Dtype
*
direction_data_
=
nullptr
;
Dtype
*
top_intensity_data_
=
nullptr
;
Dtype
*
mean_intensity_data_
=
nullptr
;
Dtype
*
distance_data_
=
nullptr
;
Dtype
*
nonempty_data_
=
nullptr
;
// point index in feature map
std
::
vector
<
int
>
map_idx_
;
// output Caffe blob
caffe
::
Blob
<
Dtype
>*
out_blob_
=
nullptr
;
#ifndef CPU_ONLY
apollo
::
perception
::
pcl_util
::
Point
*
pc_gpu_
=
nullptr
;
int
pc_gpu_size_
=
0
;
std
::
shared_ptr
<
caffe
::
Blob
<
Dtype
>>
log_table_blob_
;
#endif
//DISALLOW_COPY_AND_ASSIGN(FeatureGenerator);
};
typedef
FeatureGenerator
<
float
>
FP32FeatureGenerator
;
typedef
FeatureGenerator
<
double
>
FP64FeatureGenerator
;
}
// namespace cnnseg
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_FEATURE_GENERATOR_H
modules/perception/obstacle/lidar/segmentation/cnnseg/proto/BUILD
0 → 100644
浏览文件 @
c878c724
package
(
default_visibility
=
[
"//visibility:public"
])
load
(
"@org_pubref_rules_protobuf//cpp:rules.bzl"
,
"cc_proto_library"
)
cc_proto_library
(
name
=
"cnnseg_proto"
,
protos
=
[
"cnnseg.proto"
]
)
\ No newline at end of file
modules/perception/obstacle/lidar/segmentation/cnnseg/proto/cnnseg.proto
0 → 100644
浏览文件 @
c878c724
syntax
=
"proto2"
;
package
apollo
.
perception.cnnseg
;
message
CNNSegParam
{
required
NetworkParam
network_param
=
1
;
required
FeatureParam
feature_param
=
2
;
//optional bool use_ground = 22 [default = true];
//optional bool trust_ground = 23 [default = true];
//optional float thresh = 24 [default = 0.5];
optional
float
confidence_thresh
=
25
[
default
=
0
];
optional
float
height_thresh
=
26
[
default
=
0.5
];
//optional bool use_spp_engine = 27 [default = false];
//optional bool return_bg = 32 [default = false];
optional
uint32
min_pts_num
=
33
[
default
=
1
];
//optional string ground_detector = 41 [default = "MultiScaleGroundDetector"];
//optional float velodyne_ground_position = 42 [default = -1.7];
//optional bool do_classification = 51 [default = false];
//optional string classification_strategy = 52 [default = "voting"];
//optional bool do_heading = 53 [default = false];
optional
bool
use_full_cloud
=
61
[
default
=
false
];
//optional bool get_cnn_feature = 62 [default = false];
optional
uint32
gpu_id
=
71
[
default
=
0
];
}
message
NetworkParam
{
optional
string
instance_pt_blob
=
1
[
default
=
"instance_refine"
];
optional
string
category_pt_blob
=
2
[
default
=
"category_score"
];
optional
string
confidence_pt_blob
=
3
[
default
=
"confidence_score"
];
optional
string
classify_pt_blob
=
4
[
default
=
"classify_pt"
];
optional
string
heading_pt_blob
=
5
[
default
=
"heading_pt"
];
optional
string
height_pt_blob
=
6
[
default
=
"height_pt"
];
optional
string
feature_blob
=
7
[
default
=
"data"
];
//optional string feature_map_blob = 8 [default = ""];
}
message
FeatureParam
{
//optional string feature_type = 1 [default = "BVReferenceFP32FeatureGenerator"];
optional
bool
use_max_height
=
11
[
default
=
false
];
optional
bool
use_mean_height
=
12
[
default
=
false
];
optional
bool
use_log_count
=
13
[
default
=
false
];
optional
bool
use_direction
=
14
[
default
=
false
];
optional
bool
use_top_intensity
=
15
[
default
=
false
];
optional
bool
use_mean_intensity
=
16
[
default
=
false
];
optional
bool
use_distance
=
17
[
default
=
false
];
optional
bool
use_nonempty
=
18
[
default
=
false
];
//optional bool use_first_order = 19 [default = false];
//optional bool use_second_order = 20 [default = false];
optional
bool
use_height_filter
=
24
[
default
=
false
];
optional
bool
use_dense_feat
=
25
[
default
=
false
];
optional
uint32
point_cloud_range
=
26
[
default
=
60
];
optional
uint32
width
=
27
[
default
=
512
];
optional
uint32
height
=
28
[
default
=
512
];
optional
float
min_height
=
31
[
default
=
-
5.0
];
optional
float
max_height
=
32
[
default
=
5.0
];
}
\ No newline at end of file
modules/perception/obstacle/lidar/segmentation/cnnseg/util.h
0 → 100644
浏览文件 @
c878c724
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_
#define MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_
#include <string>
#include <opencv2/opencv.hpp>
#include "modules/perception/obstacle/lidar/segmentation/cnnseg/proto/cnnseg.pb.h"
namespace
apollo
{
namespace
perception
{
namespace
cnnseg
{
class
Timer
{
public:
Timer
()
{
Tic
();
scale_
=
1.0
/
(
static_cast
<
double
>
(
cvGetTickFrequency
())
*
1000.
);
}
void
Tic
()
{
start_
=
static_cast
<
double
>
(
cv
::
getTickCount
());
}
double
Toc
(
bool
reset
=
false
)
{
double
time
=
(
static_cast
<
double
>
(
cvGetTickCount
())
-
start_
)
*
scale_
;
if
(
reset
)
{
Tic
();
}
return
time
;
}
private:
double
start_
;
double
scale_
;
};
inline
int
F2I
(
float
val
,
float
ori
,
float
scale
)
{
return
static_cast
<
int
>
(
std
::
floor
((
ori
-
val
)
*
scale
));
}
inline
int
Pc2Pixel
(
float
in_pc
,
float
in_range
,
float
out_size
)
{
float
inv_res
=
0.5
*
out_size
/
in_range
;
return
static_cast
<
int
>
(
std
::
floor
((
in_range
-
in_pc
)
*
inv_res
));
}
inline
float
Pixel2Pc
(
int
in_pixel
,
float
in_size
,
float
out_range
)
{
float
res
=
2.0
*
out_range
/
in_size
;
return
out_range
-
(
static_cast
<
float
>
(
in_pixel
)
+
0.5
f
)
*
res
;
}
/*
void load_text_proto_message_file(const std::string& path,
google::protobuf::Message& msg) {
int fd = open(path.c_str(), O_RDONLY);
PCHECK(fd >= 0) << "path[" << path << "]";
google::protobuf::io::FileInputStream file_in(fd);
CHECK(google::protobuf::TextFormat::Parse(&file_in, &msg)) << "path[" << path << "]";
PCHECK(0 == close(fd));
}
*/
}
// namespace cnnseg
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_
third_party/caffe.BUILD
0 → 100644
浏览文件 @
c878c724
package(default_visibility = ["//visibility:public"])
licenses(["notice"])
cc_library(
name = "caffe",
srcs = [
"build/lib/libcaffe.so",
"build/lib/libproto.a",
#"build/include/caffe/proto/caffe.ph.cc",
],
hdrs = glob([
"include/caffe/*.hpp",
"build/include/caffe/proto/*.ph.h",
]),
defines = [
"CPU_ONLY"
],
includes = [
"include",
"build/include"
],
#linkopts = [
# "-lrt",
# "-lboost_system",
#],
)
\ No newline at end of file
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录