Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
d9b9a09d
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,发现更多精彩内容 >>
提交
d9b9a09d
编写于
8月 04, 2017
作者:
W
Weide Zhang
提交者:
Jiangtao Hu
8月 04, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
refactor visualizer
上级
960659dc
变更
17
隐藏空白更改
内联
并排
Showing
17 changed file
with
425 addition
and
543 deletion
+425
-543
docker/dev.x86_64.dockerfile
docker/dev.x86_64.dockerfile
+2
-0
docker/scripts/dev_start.sh
docker/scripts/dev_start.sh
+1
-1
modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cc
...on/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cc
+2
-1
modules/perception/obstacle/lidar/visualizer/BUILD
modules/perception/obstacle/lidar/visualizer/BUILD
+0
-28
modules/perception/obstacle/lidar/visualizer/opengl_visualizer/BUILD
...ception/obstacle/lidar/visualizer/opengl_visualizer/BUILD
+2
-0
modules/perception/obstacle/lidar/visualizer/opengl_visualizer/glfw_viewer.cc
...bstacle/lidar/visualizer/opengl_visualizer/glfw_viewer.cc
+5
-2
modules/perception/obstacle/lidar/visualizer/opengl_visualizer/pcl_vis_util.cc
...stacle/lidar/visualizer/opengl_visualizer/pcl_vis_util.cc
+1
-1
modules/perception/obstacle/lidar/visualizer/opengl_visualizer/pcl_vis_util.h
...bstacle/lidar/visualizer/opengl_visualizer/pcl_vis_util.h
+0
-0
modules/perception/obstacle/lidar/visualizer/pcl_obstacle_visualizer.cc
...tion/obstacle/lidar/visualizer/pcl_obstacle_visualizer.cc
+0
-390
modules/perception/obstacle/lidar/visualizer/pcl_obstacle_visualizer.h
...ption/obstacle/lidar/visualizer/pcl_obstacle_visualizer.h
+0
-119
modules/perception/obstacle/onboard/lidar_process.cc
modules/perception/obstacle/onboard/lidar_process.cc
+1
-0
modules/perception/tool/offline_lidar_perception_tool.cc
modules/perception/tool/offline_lidar_perception_tool.cc
+1
-1
modules/perception/tool/offline_visualizer_tool/BUILD
modules/perception/tool/offline_visualizer_tool/BUILD
+30
-0
modules/perception/tool/offline_visualizer_tool/conf/BUILD
modules/perception/tool/offline_visualizer_tool/conf/BUILD
+0
-0
modules/perception/tool/offline_visualizer_tool/conf/config_manager.config
...n/tool/offline_visualizer_tool/conf/config_manager.config
+0
-0
modules/perception/tool/offline_visualizer_tool/conf/offline_lidar_perception_test.flag
...e_visualizer_tool/conf/offline_lidar_perception_test.flag
+0
-0
modules/perception/tool/offline_visualizer_tool/offline_lidar_visualizer_tool.cc
.../offline_visualizer_tool/offline_lidar_visualizer_tool.cc
+380
-0
未找到文件。
docker/dev.x86_64.dockerfile
浏览文件 @
d9b9a09d
...
...
@@ -157,3 +157,5 @@ RUN wget https://github.com/nigels-com/glew/releases/download/glew-2.0.0/glew-2.
RUN
unzip glew-2.0.0.zip
WORKDIR
/tmp/glew-2.0.0
RUN
make
&&
make
install
RUN
ln
-s
/usr/lib64/libGLEW.so /usr/lib/libGLEW.so
RUN
ln
-s
/usr/lib64/libGLEW.so.2.0 /usr/lib/libGLEW.so.2.0
docker/scripts/dev_start.sh
浏览文件 @
d9b9a09d
...
...
@@ -18,7 +18,7 @@
VERSION
=
""
ARCH
=
$(
uname
-m
)
VERSION_X86_64
=
"dev-x86_64-2017080
3_13
05"
VERSION_X86_64
=
"dev-x86_64-2017080
4_12
05"
VERSION_AARCH64
=
"dev-aarch64-20170712_1533"
if
[[
$#
==
1
]]
;
then
VERSION
=
$1
...
...
modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cc
浏览文件 @
d9b9a09d
...
...
@@ -126,7 +126,8 @@ bool CNNSegmentation::Segment(const pcl_util::PointCloudPtr& pc_ptr,
// network forward process
caffe_net_
->
Forward
();
network_time_
=
timer_
.
Toc
(
true
);
AERROR
<<
"objectness threshold is "
<<
cnnseg_param_
.
objectness_thresh
();
AERROR
<<
"confidence threshold is "
<<
cnnseg_param_
.
confidence_thresh
();
// clutser points and construct segments/objects
cluster2d_
->
Cluster
(
*
category_pt_blob_
,
*
instance_pt_blob_
,
pc_ptr
,
valid_indices
,
...
...
modules/perception/obstacle/lidar/visualizer/BUILD
已删除
100644 → 0
浏览文件 @
960659dc
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"perception_obstacle_lidar_visualizer"
,
srcs
=
[
"pcl_obstacle_visualizer.cc"
,
"pcl_vis_util.cc"
,
],
hdrs
=
[
"pcl_obstacle_visualizer.h"
,
"pcl_vis_util.h"
,
],
deps
=
[
"//modules/perception/lib/pcl_util:pcl_util"
,
"//modules/perception/obstacle/base:perception_obstacle_base"
,
"//modules/perception/lib/base:base"
,
"//modules/perception/obstacle/common:perception_obstacle_common"
,
"@eigen//:eigen"
,
"@pcl//:pcl"
,
"@vtk//:vtk"
,
],
)
cpplint
()
modules/perception/obstacle/lidar/visualizer/opengl_visualizer/BUILD
浏览文件 @
d9b9a09d
...
...
@@ -10,6 +10,7 @@ cc_library(
"glfw_viewer.cc"
,
"opengl_visualizer.cc"
,
"frame_content.cc"
,
"pcl_vis_util.cc"
,
],
hdrs
=
[
"frame.h"
,
...
...
@@ -17,6 +18,7 @@ cc_library(
"glfw_viewer.h"
,
"opengl_visualizer.h"
,
"frame_content.h"
,
"pcl_vis_util.h"
,
],
deps
=
[
"//modules/perception/lib/pcl_util:pcl_util"
,
...
...
modules/perception/obstacle/lidar/visualizer/opengl_visualizer/glfw_viewer.cc
浏览文件 @
d9b9a09d
...
...
@@ -116,8 +116,6 @@ bool GLFWViewer::initialize(){
show_velocity
=
1
;
show_polygon
=
0
;
show_text
=
1
;
return
true
;
}
...
...
@@ -438,12 +436,15 @@ bool GLFWViewer::draw_objects(FrameContent* content , bool draw_cube, bool draw_
tracked_objects
=
content
->
get_tracked_objects
();
std
::
cout
<<
"tracked objects size "
<<
tracked_objects
.
size
()
<<
std
::
endl
;
for
(
size_t
i
=
0
;
i
<
tracked_objects
.
size
();
++
i
)
{
objects
.
push_back
(
tracked_objects
[
i
]);
}
float
rgb
[
3
];
if
(
draw_cube
){
std
::
cout
<<
"draw cube enabled with objects size"
<<
objects
.
size
()
<<
std
::
endl
;
float
verts
[
8
][
3
];
int
i
=
0
;
vec3
center
;
//x,y,z
...
...
@@ -462,6 +463,8 @@ bool GLFWViewer::draw_objects(FrameContent* content , bool draw_cube, bool draw_
size
.
y
=
objects
[
i
]
->
width
;
size
.
z
=
objects
[
i
]
->
height
;
std
::
cout
<<
"objects info "
<<
objects
[
i
]
->
ToString
()
<<
std
::
endl
;
float
x1
=
size
.
x
/
2
;
float
x2
=
0
-
x1
;
float
y1
=
size
.
y
/
2
;
...
...
modules/perception/obstacle/lidar/visualizer/pcl_vis_util.cc
→
modules/perception/obstacle/lidar/visualizer/
opengl_visualizer/
pcl_vis_util.cc
浏览文件 @
d9b9a09d
...
...
@@ -14,7 +14,7 @@
* limitations under the License.
*****************************************************************************/
#include "modules/perception/obstacle/lidar/visualizer/pcl_vis_util.h"
#include "modules/perception/obstacle/lidar/visualizer/
opengl_visualizer/
pcl_vis_util.h"
namespace
apollo
{
namespace
perception
{
...
...
modules/perception/obstacle/lidar/visualizer/pcl_vis_util.h
→
modules/perception/obstacle/lidar/visualizer/
opengl_visualizer/
pcl_vis_util.h
浏览文件 @
d9b9a09d
文件已移动
modules/perception/obstacle/lidar/visualizer/pcl_obstacle_visualizer.cc
已删除
100644 → 0
浏览文件 @
960659dc
/******************************************************************************
* 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/visualizer/pcl_obstacle_visualizer.h"
#include <gflags/gflags.h>
namespace
apollo
{
namespace
perception
{
DEFINE_int32
(
visual_spin_rate
,
1
,
"visual_spin_rate"
);
int
ObstacleTrackData
::
_s_max_queue_size
=
300
;
ObstacleTrackData
::
ObstacleTrackData
()
{
}
ObstacleTrackData
::~
ObstacleTrackData
()
{
}
void
ObstacleTrackData
::
add_obstacle
(
const
ObjectPtr
&
obj
)
{
if
(
_history_data
.
size
()
>=
(
size_t
)
_s_max_queue_size
)
{
_history_data
.
pop_front
();
}
ObjectPtr
new_obj
(
new
Object
());
new_obj
->
clone
(
*
obj
);
_history_data
.
push_back
(
new_obj
);
_consecutive_invisible_count
=
0
;
_updated
=
true
;
}
PCLObstacleVisualizer
::
PCLObstacleVisualizer
()
:
_name
(
"ObsVisualizer"
),
_object_color_type
(
OBJECT_COLOR_TRACK
),
_capture_screen
(
false
),
_output_directory
(
""
)
{
initialize_pcl_visualizer
();
}
PCLObstacleVisualizer
::
PCLObstacleVisualizer
(
const
std
::
string
&
name
,
const
OBJECT_COLOR_TYPE
color_type
,
const
bool
capture_screen
,
const
std
::
string
&
output_directory
)
:
_name
(
name
),
_object_color_type
(
color_type
),
_capture_screen
(
capture_screen
),
_output_directory
(
output_directory
)
{
initialize_pcl_visualizer
();
}
PCLObstacleVisualizer
::~
PCLObstacleVisualizer
()
{
}
//default pcl visualizer initialization
void
PCLObstacleVisualizer
::
initialize_pcl_visualizer
()
{
_pcl_vs
=
boost
::
shared_ptr
<
pcl
::
visualization
::
PCLVisualizer
>
(
new
pcl
::
visualization
::
PCLVisualizer
(
_name
));
_view_port
=
0
;
_pcl_vs
->
addCoordinateSystem
(
1.0
,
_view_port
);
set_size
(
2000
,
1400
);
set_background_color
(
0.05
,
0.05
,
0.05
,
0.0
f
);
set_velodyne_height
(
2.5
);
set_car_points
();
set_camera_position
();
}
void
PCLObstacleVisualizer
::
set_size
(
int
w
,
int
h
)
{
_width
=
w
;
_height
=
h
;
_pcl_vs
->
setSize
(
w
,
h
);
}
void
PCLObstacleVisualizer
::
set_background_color
(
float
r
,
float
g
,
float
b
,
float
a
)
{
_pcl_vs
->
setBackgroundColor
(
r
,
g
,
b
,
a
);
}
void
PCLObstacleVisualizer
::
set_velodyne_height
(
float
h
)
{
_velodyne_height
=
h
;
}
void
PCLObstacleVisualizer
::
set_camera_position
()
{
_up
.
x
=
0
;
_up
.
y
=
1
;
_up
.
z
=
0
;
_view_point
.
x
=
0
;
_view_point
.
y
=
0
;
_view_point
.
z
=
0
;
_camera_center
.
x
=
0
;
_camera_center
.
y
=
0
;
_camera_center
.
z
=
150
;
// _camera_center = _main_car_points_local.at(7);
}
void
PCLObstacleVisualizer
::
set_car_points
()
{
_main_car_points_local
.
resize
(
9
);
_main_car_points_local
.
at
(
0
).
x
=
0
;
_main_car_points_local
.
at
(
0
).
y
=
0
;
_main_car_points_local
.
at
(
0
).
z
=
0
;
_main_car_points_local
.
at
(
1
).
x
=
0
;
_main_car_points_local
.
at
(
1
).
y
=
0
;
_main_car_points_local
.
at
(
1
).
z
=
-
_velodyne_height
;
_main_car_points_local
.
at
(
2
).
x
=
3
;
_main_car_points_local
.
at
(
2
).
y
=
0
;
_main_car_points_local
.
at
(
2
).
z
=
-
_velodyne_height
;
_main_car_points_local
.
at
(
3
).
x
=
2.5
;
_main_car_points_local
.
at
(
3
).
y
=
1.0
;
_main_car_points_local
.
at
(
3
).
z
=
-
_velodyne_height
;
_main_car_points_local
.
at
(
4
).
x
=
2.5
;
_main_car_points_local
.
at
(
4
).
y
=
-
1.0
;
_main_car_points_local
.
at
(
4
).
z
=
-
_velodyne_height
;
_main_car_points_local
.
at
(
5
).
x
=
-
2.5
;
_main_car_points_local
.
at
(
5
).
y
=
-
1.0
;
_main_car_points_local
.
at
(
5
).
z
=
-
_velodyne_height
;
_main_car_points_local
.
at
(
6
).
x
=
-
2.5
;
_main_car_points_local
.
at
(
6
).
y
=
1.0
;
_main_car_points_local
.
at
(
6
).
z
=
-
_velodyne_height
;
_main_car_points_local
.
at
(
7
).
x
=
0
;
_main_car_points_local
.
at
(
7
).
y
=
0
;
_main_car_points_local
.
at
(
7
).
z
=
160
;
_main_car_points_local
.
at
(
8
).
x
=
-
40
;
_main_car_points_local
.
at
(
8
).
y
=
0
;
_main_car_points_local
.
at
(
8
).
z
=
50
;
}
Eigen
::
Vector3f
PCLObstacleVisualizer
::
get_cls_color
(
int
cls
)
{
Eigen
::
Vector3f
rgb
(
255
,
255
,
255
);
switch
(
cls
)
{
case
0
:
rgb
<<
128
,
0
,
255
;
//紫
break
;
case
1
:
rgb
<<
0
,
255
,
255
;
//青
break
;
case
2
:
rgb
<<
255
,
255
,
0
;
//黄
break
;
case
3
:
rgb
<<
255
,
128
,
128
;
//赤
break
;
case
4
:
rgb
<<
0
,
0
,
255
;
//蓝
break
;
case
5
:
rgb
<<
0
,
255
,
0
;
//绿
break
;
case
6
:
rgb
<<
255
,
128
,
0
;
//橙
break
;
case
7
:
rgb
<<
255
,
0
,
0
;
//赤
break
;
}
return
rgb
;
}
Eigen
::
Vector3f
PCLObstacleVisualizer
::
get_track_color
(
int
track_id
)
{
int
cid
=
track_id
%
124
+
1
;
int
r
=
cid
%
5
;
int
g
=
((
cid
-
r
)
/
5
)
%
5
;
int
b
=
(
cid
-
r
-
5
*
g
)
/
25
;
float
id2intesity
[
5
]
=
{
0
,
128
,
255
,
64
,
192
};
return
Eigen
::
Vector3f
(
id2intesity
[
r
],
id2intesity
[
g
],
id2intesity
[
b
]);
}
void
PCLObstacleVisualizer
::
add_object
(
ObjectPtr
&
obj
,
std
::
string
id
)
{
Eigen
::
Vector3f
coord_dir
(
1.0
,
0.0
,
0.0
);
Eigen
::
Vector3f
size_dir
=
obj
->
direction
.
cast
<
float
>
();
Eigen
::
Matrix3f
rot
=
vector_rot_mat_2d_xy
\
<
Eigen
::
Vector3f
,
Eigen
::
Matrix3f
>
(
size_dir
,
coord_dir
);
Eigen
::
Vector3f
cnt
=
obj
->
center
.
cast
<
float
>
();
Eigen
::
Vector3f
size
=
Eigen
::
Vector3f
(
obj
->
length
,
obj
->
width
,
obj
->
height
);
Eigen
::
Vector3f
rgb
=
get_cls_color
(
obj
->
type
)
/
255
;
add_bbox_3d
<
pcl_util
::
Point
>
(
cnt
,
size
,
rot
,
rgb
,
_pcl_vs
.
get
(),
_view_port
,
id
);
}
void
PCLObstacleVisualizer
::
visualize_detection
(
pcl_util
::
PointCloudPtr
ground
,
std
::
vector
<
ObjectPtr
>&
objects
,
int
frame_id
)
{
add_pointcloud
<
pcl_util
::
Point
>
(
_pcl_vs
.
get
(),
ground
,
180
,
180
,
180
,
1
,
"ground"
,
_view_port
);
add_pointcloud
<
pcl_util
::
Point
>
(
_pcl_vs
.
get
(),
\
_main_car_points_local
.
makeShared
(),
255
,
255
,
255
,
5
,
"car_points"
,
_view_port
);
for
(
size_t
i
=
0
;
i
<
objects
.
size
();
++
i
)
{
std
::
ostringstream
oss
;
oss
<<
"obj_"
<<
i
;
Eigen
::
Vector3f
rgb
(
1
,
0
,
0
);
add_pointcloud
<
pcl_util
::
Point
>
(
_pcl_vs
.
get
(),
objects
[
i
]
->
cloud
,
255
,
0
,
0
,
3
,
oss
.
str
(),
_view_port
);
add_object
(
objects
[
i
],
oss
.
str
());
}
_pcl_vs
.
get
()
->
setCameraPosition
(
_camera_center
.
x
,
_camera_center
.
y
,
_camera_center
.
z
,
_view_point
.
x
,
_view_point
.
y
,
_view_point
.
z
,
_up
.
x
,
_up
.
y
,
_up
.
z
);
_pcl_vs
.
get
()
->
spinOnce
();
std
::
ostringstream
oss_prefix
;
oss_prefix
<<
std
::
setfill
(
'0'
)
<<
std
::
setw
(
6
)
<<
frame_id
;
std
::
string
file_name
=
_output_directory
+
"/detection_"
+
oss_prefix
.
str
()
+
".png"
;
std
::
cout
<<
file_name
<<
"
\n
"
;
_pcl_vs
.
get
()
->
saveScreenshot
(
file_name
);
_pcl_vs
.
get
()
->
removeAllPointClouds
();
_pcl_vs
.
get
()
->
removeAllShapes
();
}
void
PCLObstacleVisualizer
::
visualize_tracking
(
pcl_util
::
PointDCloudPtr
cloud_world
,
std
::
vector
<
ObjectPtr
>&
objects_world
,
const
Eigen
::
Matrix4d
&
pose_velo2w
,
int
frame_id
,
const
pcl_util
::
PointIndices
&
roi_indices
)
{
Eigen
::
Matrix4d
pose
=
pose_velo2w
;
if
(
_track_data
.
empty
())
{
_global_offset
=
Eigen
::
Vector3d
(
-
pose_velo2w
(
0
,
3
),
-
pose_velo2w
(
1
,
3
),
-
pose_velo2w
(
2
,
3
));
}
pose
(
0
,
3
)
+=
_global_offset
[
0
];
pose
(
1
,
3
)
+=
_global_offset
[
1
];
pose
(
2
,
3
)
+=
_global_offset
[
2
];
for
(
size_t
i
=
0
;
i
<
cloud_world
->
size
();
i
++
)
{
cloud_world
->
points
[
i
].
x
+=
_global_offset
[
0
];
cloud_world
->
points
[
i
].
y
+=
_global_offset
[
1
];
cloud_world
->
points
[
i
].
z
+=
_global_offset
[
2
];
}
for
(
size_t
i
=
0
;
i
<
objects_world
.
size
();
i
++
)
{
objects_world
[
i
]
->
center
+=
_global_offset
;
for
(
size_t
j
=
0
;
j
<
objects_world
[
i
]
->
cloud
->
size
();
j
++
)
{
objects_world
[
i
]
->
cloud
->
points
[
j
].
x
+=
_global_offset
[
0
];
objects_world
[
i
]
->
cloud
->
points
[
j
].
y
+=
_global_offset
[
1
];
objects_world
[
i
]
->
cloud
->
points
[
j
].
z
+=
_global_offset
[
2
];
}
for
(
size_t
j
=
0
;
j
<
objects_world
[
i
]
->
polygon
.
size
();
j
++
)
{
objects_world
[
i
]
->
polygon
.
points
[
j
].
x
+=
_global_offset
[
0
];
objects_world
[
i
]
->
polygon
.
points
[
j
].
y
+=
_global_offset
[
1
];
objects_world
[
i
]
->
polygon
.
points
[
j
].
z
+=
_global_offset
[
2
];
}
}
/// transform camera center to world
pcl_util
::
Point
camera_center_w
;
transform_point_cloud
<
pcl_util
::
Point
>
(
_camera_center
,
camera_center_w
,
pose
);
/// transform camera view point to world
pcl_util
::
Point
camera_vpoint_w
;
transform_point_cloud
<
pcl_util
::
Point
>
(
_view_point
,
camera_vpoint_w
,
pose
);
/// transform host car points to world
pcl_util
::
PointCloudPtr
w_car_points
(
new
pcl_util
::
PointCloud
);
transform_point_cloud
<
pcl_util
::
Point
>
(
_main_car_points_local
,
*
w_car_points
,
pose
);
/// show and save tracking result
Eigen
::
Vector4d
up_w
(
_up
.
x
,
_up
.
y
,
_up
.
z
,
0
);
up_w
=
pose
*
up_w
;
std
::
ostringstream
oss_prefix
;
oss_prefix
<<
std
::
setfill
(
'0'
)
<<
std
::
setw
(
6
)
<<
frame_id
;
std
::
string
file_name
=
_output_directory
+
"/tracking_"
+
oss_prefix
.
str
()
+
".png"
;
pcl_util
::
PointDCloudPtr
roi_cloud
(
new
pcl_util
::
PointDCloud
);
pcl
::
copyPointCloud
(
*
cloud_world
,
roi_indices
,
*
roi_cloud
);
add_pointcloud
<
pcl_util
::
PointD
>
(
_pcl_vs
.
get
(),
\
cloud_world
,
180
,
180
,
180
,
1
,
"ground"
,
_view_port
);
add_pointcloud
<
pcl_util
::
Point
>
(
_pcl_vs
.
get
(),
\
w_car_points
,
255
,
255
,
255
,
5
,
"car_points"
,
_view_port
);
add_pointcloud
<
pcl_util
::
PointD
>
(
_pcl_vs
.
get
(),
\
roi_cloud
,
0
,
0
,
205
,
1
,
"roi"
,
_view_port
);
// MediumBlue
for
(
std
::
map
<
int
,
ObstacleTrackData
>::
iterator
it
=
_track_data
.
begin
();
it
!=
_track_data
.
end
();
++
it
)
{
it
->
second
.
_updated
=
false
;
}
for
(
size_t
i
=
0
;
i
<
objects_world
.
size
();
i
++
)
{
int
track_id
=
objects_world
[
i
]
->
track_id
;
std
::
map
<
int
,
ObstacleTrackData
>::
iterator
it
=
_track_data
.
find
(
track_id
);
if
(
it
==
_track_data
.
end
())
{
ObstacleTrackData
&
data
=
_track_data
[
track_id
];
data
.
add_obstacle
(
objects_world
[
i
]);
}
else
{
it
->
second
.
add_obstacle
(
objects_world
[
i
]);
}
}
for
(
std
::
map
<
int
,
ObstacleTrackData
>::
iterator
it
=
_track_data
.
begin
();
it
!=
_track_data
.
end
();)
{
if
(
it
->
second
.
_updated
==
false
)
{
it
->
second
.
_consecutive_invisible_count
++
;
if
(
it
->
second
.
_consecutive_invisible_count
>
1
)
{
it
=
_track_data
.
erase
(
it
);
}
else
{
++
it
;
}
}
else
{
++
it
;
}
}
for
(
std
::
map
<
int
,
ObstacleTrackData
>::
iterator
it
=
_track_data
.
begin
();
it
!=
_track_data
.
end
();
++
it
)
{
if
(
it
->
second
.
_consecutive_invisible_count
>
0
)
{
continue
;
}
std
::
ostringstream
oss
;
oss
<<
"track_"
<<
it
->
first
;
std
::
string
id
=
oss
.
str
()
+
"_cloud"
;
ObjectPtr
recent_obj
=
it
->
second
.
_history_data
.
back
();
add_pointcloud
<
pcl_util
::
Point
>
(
_pcl_vs
.
get
(),
recent_obj
->
cloud
,
255
,
0
,
3
,
1
,
id
,
_view_port
);
add_object
(
recent_obj
,
oss
.
str
());
pcl_util
::
Point
src
;
pcl_util
::
Point
dst
;
src
.
x
=
recent_obj
->
center
[
0
];
src
.
y
=
recent_obj
->
center
[
1
];
src
.
z
=
recent_obj
->
center
[
2
];
dst
.
x
=
src
.
x
+
recent_obj
->
velocity
[
0
];
dst
.
y
=
src
.
y
+
recent_obj
->
velocity
[
1
];
dst
.
z
=
src
.
z
+
recent_obj
->
velocity
[
2
];
id
=
oss
.
str
()
+
"_velocity"
;
_pcl_vs
.
get
()
->
addLine
(
src
,
dst
,
1
,
1
,
1
,
id
);
std
::
vector
<
Eigen
::
Vector3d
>
history_centers
;
std
::
deque
<
ObjectPtr
>::
iterator
dq_it
=
it
->
second
.
_history_data
.
begin
();
for
(;
dq_it
!=
it
->
second
.
_history_data
.
end
();
dq_it
++
)
{
history_centers
.
push_back
((
*
dq_it
)
->
center
);
}
Eigen
::
Vector3f
rgb
(
1
,
1
,
0
);
int
history_length
=
history_centers
.
size
();
for
(
int
j
=
0
;
j
<
history_length
-
1
;
j
++
)
{
std
::
ostringstream
id_oss
;
id_oss
<<
"_edge_"
<<
j
;
std
::
string
tag
=
oss
.
str
()
+
id_oss
.
str
();
src
.
x
=
history_centers
[
j
][
0
];
src
.
y
=
history_centers
[
j
][
1
];
src
.
z
=
history_centers
[
j
][
2
];
dst
.
x
=
history_centers
[
j
+
1
][
0
];
dst
.
y
=
history_centers
[
j
+
1
][
1
];
dst
.
z
=
history_centers
[
j
+
1
][
2
];
_pcl_vs
.
get
()
->
addLine
(
src
,
dst
,
rgb
[
0
],
rgb
[
1
],
rgb
[
2
],
tag
);
}
pcl_util
::
Point
loc
;
loc
.
x
=
recent_obj
->
center
[
0
];
loc
.
y
=
recent_obj
->
center
[
1
];
loc
.
z
=
recent_obj
->
center
[
2
];
std
::
string
text_id
=
oss
.
str
();
std
::
ostringstream
text_oss
;
text_oss
<<
recent_obj
->
track_id
;
std
::
string
disp_text
=
text_oss
.
str
();
_pcl_vs
.
get
()
->
addText3D
(
disp_text
,
loc
,
0.5
,
1
,
1
,
1
,
text_id
+
"_text"
,
_view_port
);
}
_pcl_vs
.
get
()
->
setCameraPosition
(
camera_center_w
.
x
,
camera_center_w
.
y
,
camera_center_w
.
z
,
camera_vpoint_w
.
x
,
camera_vpoint_w
.
y
,
camera_vpoint_w
.
z
,
up_w
[
0
],
up_w
[
1
],
up_w
[
2
]);
_pcl_vs
.
get
()
->
spinOnce
(
FLAGS_visual_spin_rate
);
_pcl_vs
.
get
()
->
saveScreenshot
(
file_name
);
_pcl_vs
.
get
()
->
removeAllPointClouds
();
_pcl_vs
.
get
()
->
removeAllShapes
();
}
}
// namespace perception
}
// namespace apollo
modules/perception/obstacle/lidar/visualizer/pcl_obstacle_visualizer.h
已删除
100644 → 0
浏览文件 @
960659dc
/******************************************************************************
* 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_VISUALIZATION_PCL_OBSTACLE_VISUALIZER_H
#define MODULES_PERCEPTION_VISUALIZATION_PCL_OBSTACLE_VISUALIZER_H
#include <deque>
#include <iomanip>
#include <string>
#include <sstream>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/base/object.h"
#include "modules/perception/obstacle/lidar/visualizer/pcl_vis_util.h"
namespace
apollo
{
namespace
perception
{
enum
OBJECT_COLOR_TYPE
{
OBJECT_COLOR_CLASSIFICATION
=
0
,
OBJECT_COLOR_TRACK
};
class
ObstacleTrackData
{
public:
ObstacleTrackData
();
~
ObstacleTrackData
();
void
add_obstacle
(
const
ObjectPtr
&
obj
);
public:
std
::
deque
<
ObjectPtr
>
_history_data
;
int
_consecutive_invisible_count
;
bool
_updated
;
private:
static
int
_s_max_queue_size
;
};
class
PCLObstacleVisualizer
{
public:
PCLObstacleVisualizer
();
explicit
PCLObstacleVisualizer
(
const
std
::
string
&
name
,
const
OBJECT_COLOR_TYPE
color_type
,
const
bool
capture_screen
,
const
std
::
string
&
output_directory
);
~
PCLObstacleVisualizer
();
void
set_size
(
int
w
,
int
h
);
void
set_background_color
(
float
r
,
float
g
,
float
b
,
float
a
=
1.0
f
);
void
set_velodyne_height
(
float
height
);
void
set_car_points
();
void
set_camera_position
();
void
set_output_dir
(
const
std
::
string
&
outptut_dir
)
{
_output_directory
=
outptut_dir
;
}
void
visualize_detection
(
pcl_util
::
PointCloudPtr
ground
,
std
::
vector
<
ObjectPtr
>&
objects
,
int
frame_id
=
-
1
);
void
visualize_tracking
(
pcl_util
::
PointDCloudPtr
cloud_world
,
std
::
vector
<
ObjectPtr
>&
objects_world
,
const
Eigen
::
Matrix4d
&
pose_velo2w
,
int
frame_id
,
const
pcl_util
::
PointIndices
&
roi_indices
);
protected:
void
initialize_pcl_visualizer
();
Eigen
::
Vector3f
get_cls_color
(
int
cls
);
Eigen
::
Vector3f
get_track_color
(
int
track_id
);
void
add_object
(
ObjectPtr
&
obj
,
std
::
string
id
);
private:
boost
::
shared_ptr
<
pcl
::
visualization
::
PCLVisualizer
>
_pcl_vs
;
std
::
map
<
int
,
ObstacleTrackData
>
_track_data
;
std
::
string
_name
;
int
_view_port
;
pcl_util
::
Point
_camera_center
;
pcl_util
::
Point
_view_point
;
pcl_util
::
Point
_up
;
float
_velodyne_height
;
pcl_util
::
PointCloud
_main_car_points_local
;
int
_width
;
int
_height
;
OBJECT_COLOR_TYPE
_object_color_type
;
bool
_capture_screen
;
std
::
string
_output_directory
;
Eigen
::
Vector3d
_global_offset
;
};
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_VISUALIZATION_PCL_OBSTACLE_VISUALIZER_H
modules/perception/obstacle/onboard/lidar_process.cc
浏览文件 @
d9b9a09d
...
...
@@ -169,6 +169,7 @@ bool LidarProcess::Process(double timestamp, PointCloudPtr point_cloud,
}
AERROR
<<
"call object_builder succ."
;
PERF_BLOCK_END
(
"lidar_object_builder"
);
//objects_ = objects;
/// call tracker
if
(
tracker_
!=
nullptr
)
{
...
...
modules/perception/tool/offline_lidar_perception_tool.cc
浏览文件 @
d9b9a09d
...
...
@@ -201,8 +201,8 @@ public:
pcl_util
::
PointIndices
roi_indices_1
;
FrameContent
content
;
content
.
set_lidar_cloud
(
cloud
);
content
.
set_lidar_pose
(
pose
);
content
.
set_lidar_cloud
(
cloud
);
content
.
set_tracked_objects
(
result_objects
);
visualizer_
->
update_camera_system
(
&
content
);
visualizer_
->
render
(
content
);
...
...
modules/perception/tool/offline_visualizer_tool/BUILD
0 → 100644
浏览文件 @
d9b9a09d
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_binary
(
name
=
"offline_lidar_visualizer_tool"
,
srcs
=
[
"offline_lidar_visualizer_tool.cc"
],
data
=
[
"//modules/perception/tool/offline_visualizer_tool/conf:perception_tool_config"
,
"//modules/perception:perception_model"
,
],
linkstatic
=
0
,
deps
=
[
"//modules/common"
,
"//modules/common:log"
,
"//modules/perception/common:perception_common"
,
"//modules/perception/lib/base"
,
"//modules/perception/lib/config_manager"
,
"//modules/perception/lib/pcl_util"
,
"//modules/perception/obstacle/common:perception_obstacle_common"
,
"//modules/perception/obstacle/lidar/visualizer/opengl_visualizer:perception_obstacle_lidar_opengl_visualizer"
,
"//modules/perception/obstacle/onboard:perception_obstacle_lidar_process"
,
"@eigen//:eigen"
,
"@glew//:glew"
,
"@glfw//:glfw"
,
"@opengl//:opengl"
,
],
)
cpplint
()
modules/perception/tool/conf/BUILD
→
modules/perception/tool/
offline_visualizer_tool/
conf/BUILD
浏览文件 @
d9b9a09d
文件已移动
modules/perception/tool/conf/config_manager.config
→
modules/perception/tool/
offline_visualizer_tool/
conf/config_manager.config
浏览文件 @
d9b9a09d
文件已移动
modules/perception/tool/conf/offline_lidar_perception_test.flag
→
modules/perception/tool/
offline_visualizer_tool/
conf/offline_lidar_perception_test.flag
浏览文件 @
d9b9a09d
文件已移动
modules/perception/tool/offline_visualizer_tool/offline_lidar_visualizer_tool.cc
0 → 100644
浏览文件 @
d9b9a09d
/******************************************************************************
* 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 <fstream>
#include <iomanip>
#include <sstream>
#include <vector>
#include <string>
#include <pcl/io/pcd_io.h>
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/common/file_system_util.h"
#include "modules/perception/obstacle/onboard/lidar_process.h"
#include "modules/perception/obstacle/lidar/visualizer/opengl_visualizer/opengl_visualizer.h"
#include "modules/perception/obstacle/lidar/visualizer/opengl_visualizer/frame_content.h"
DECLARE_string
(
flagfile
);
DECLARE_string
(
config_manager_path
);
DEFINE_string
(
pcd_path
,
"./pcd/"
,
"pcd path"
);
DEFINE_string
(
pose_path
,
"./pose/"
,
"pose path"
);
DEFINE_bool
(
enable_visualization
,
true
,
"enable visualization"
);
namespace
apollo
{
namespace
perception
{
DEFINE_string
(
output_path
,
"./output/"
,
"output path"
);
DEFINE_int32
(
start_frame
,
1000
,
"start frame"
);
template
<
typename
T
>
void
quaternion_to_rotation_matrix
(
const
T
*
quat
,
T
*
R
)
{
T
x2
=
quat
[
0
]
*
quat
[
0
];
T
xy
=
quat
[
0
]
*
quat
[
1
];
T
rx
=
quat
[
3
]
*
quat
[
0
];
T
y2
=
quat
[
1
]
*
quat
[
1
];
T
yz
=
quat
[
1
]
*
quat
[
2
];
T
ry
=
quat
[
3
]
*
quat
[
1
];
T
z2
=
quat
[
2
]
*
quat
[
2
];
T
zx
=
quat
[
2
]
*
quat
[
0
];
T
rz
=
quat
[
3
]
*
quat
[
2
];
T
r2
=
quat
[
3
]
*
quat
[
3
];
R
[
0
]
=
r2
+
x2
-
y2
-
z2
;
// fill diagonal terms
R
[
4
]
=
r2
-
x2
+
y2
-
z2
;
R
[
8
]
=
r2
-
x2
-
y2
+
z2
;
R
[
3
]
=
2
*
(
xy
+
rz
);
// fill off diagonal terms
R
[
6
]
=
2
*
(
zx
-
ry
);
R
[
7
]
=
2
*
(
yz
+
rx
);
R
[
1
]
=
2
*
(
xy
-
rz
);
R
[
2
]
=
2
*
(
zx
+
ry
);
R
[
5
]
=
2
*
(
yz
-
rx
);
}
bool
read_pose_file
(
const
std
::
string
&
filename
,
Eigen
::
Matrix4d
&
pose
,
int
&
frame_id
,
double
&
time_stamp
)
{
std
::
ifstream
ifs
(
filename
.
c_str
());
if
(
!
ifs
.
is_open
())
{
std
::
cerr
<<
"Failed to open file "
<<
filename
<<
std
::
endl
;
return
false
;
}
char
buffer
[
1024
];
ifs
.
getline
(
buffer
,
1024
);
int
id
=
0
;
double
time_samp
=
0
;
double
quat
[
4
];
double
matrix3x3
[
9
];
sscanf
(
buffer
,
"%d %lf %lf %lf %lf %lf %lf %lf %lf"
,
&
id
,
&
(
time_samp
),
&
(
pose
(
0
,
3
)),
&
(
pose
(
1
,
3
)),
&
(
pose
(
2
,
3
)),
&
(
quat
[
0
]),
&
(
quat
[
1
]),
&
(
quat
[
2
]),
&
(
quat
[
3
]));
quaternion_to_rotation_matrix
<
double
>
(
quat
,
matrix3x3
);
for
(
int
i
=
0
;
i
<
3
;
++
i
)
{
for
(
int
j
=
0
;
j
<
3
;
++
j
)
{
pose
(
i
,
j
)
=
matrix3x3
[
i
*
3
+
j
];
}
}
frame_id
=
id
;
time_stamp
=
time_samp
;
return
true
;
}
bool
read_pose_file_mat12
(
const
std
::
string
&
filename
,
Eigen
::
Matrix4d
&
pose
,
int
&
frame_id
,
double
&
time_stamp
)
{
std
::
ifstream
ifs
(
filename
.
c_str
());
if
(
!
ifs
.
is_open
())
{
std
::
cerr
<<
"Failed to open file "
<<
filename
<<
std
::
endl
;
return
false
;
}
pose
=
Eigen
::
Matrix4d
::
Identity
();
ifs
>>
frame_id
>>
time_stamp
;
for
(
int
i
=
0
;
i
<
3
;
i
++
)
{
for
(
int
j
=
0
;
j
<
4
;
j
++
)
{
ifs
>>
pose
(
i
,
j
);
}
}
return
true
;
}
class
OfflineLidarPerceptionTool
{
public:
OfflineLidarPerceptionTool
()
{
config_manager_
=
NULL
;
};
~
OfflineLidarPerceptionTool
()
=
default
;
bool
Init
(
bool
use_visualization
=
false
)
{
FLAGS_config_manager_path
=
"tool/offline_visualizer_tool/conf/config_manager.config"
;
config_manager_
=
Singleton
<
ConfigManager
>::
Get
();
if
(
config_manager_
==
NULL
||
!
config_manager_
->
Init
())
{
AERROR
<<
"failed to init ConfigManager"
;
return
false
;
}
lidar_process_
.
reset
(
new
LidarProcess
());
if
(
!
lidar_process_
->
Init
())
{
AERROR
<<
"failed to init lidar_process."
;
return
false
;
}
output_dir_
=
FLAGS_output_path
;
if
(
use_visualization
)
{
//visualizer_.reset(new PCLObstacleVisualizer("obstaclevisualizer_",
// OBJECT_COLOR_TRACK, false, output_dir_));
visualizer_
.
reset
(
new
OpenglVisualizer
());
if
(
!
visualizer_
->
init
())
{
AERROR
<<
"init visialuzer failed"
<<
std
::
endl
;
};
}
return
true
;
}
void
Run
(
const
std
::
string
&
pcd_path
,
const
std
::
string
pose_path
)
{
std
::
string
pcd_folder
=
pcd_path
;
std
::
string
pose_folder
=
pose_path
;
std
::
vector
<
std
::
string
>
pcd_file_names
;
std
::
vector
<
std
::
string
>
pose_file_names
;
AERROR
<<
"starting to run"
;
get_file_names_in_folder
(
pose_folder
,
".pose"
,
pose_file_names
);
get_file_names_in_folder
(
pcd_folder
,
".pcd"
,
pcd_file_names
);
AERROR
<<
" pose size "
<<
pose_file_names
.
size
();
AERROR
<<
" pcd size "
<<
pcd_file_names
.
size
();
if
(
pose_file_names
.
size
()
!=
pcd_file_names
.
size
())
{
AERROR
<<
"pcd file number does not match pose file number"
;
return
;
}
double
time_stamp
=
0.0
;
int
start_frame
=
FLAGS_start_frame
;
AERROR
<<
"starting frame is "
<<
start_frame
;
sleep
(
1
);
for
(
size_t
i
=
start_frame
;
i
<
pcd_file_names
.
size
();
i
++
)
{
AERROR
<<
"***************** Frame "
<<
i
<<
" ******************"
;
std
::
ostringstream
oss
;
pcl_util
::
PointCloudPtr
cloud
(
new
pcl_util
::
PointCloud
);
AERROR
<<
"load pcd file from file path"
<<
pcd_folder
+
pcd_file_names
[
i
];
pcl
::
io
::
loadPCDFile
<
pcl_util
::
Point
>
(
pcd_folder
+
pcd_file_names
[
i
],
*
cloud
);
AERROR
<<
"read point cloud from "
<<
pcd_file_names
[
i
]
<<
" with cloud size: "
<<
cloud
->
points
.
size
();
//read pose
Eigen
::
Matrix4d
pose
=
Eigen
::
Matrix4d
::
Identity
();
int
frame_id
=
-
1
;
if
(
!
read_pose_file
(
pose_folder
+
pose_file_names
[
i
],
pose
,
frame_id
,
time_stamp
))
{
std
::
cout
<<
"Failed to read file "
<<
pose_file_names
[
i
]
<<
"
\n
"
;
return
;
}
AERROR
<<
"read pose file "
<<
pose_file_names
[
i
]
<<
" "
<<
pose
;
std
::
shared_ptr
<
Eigen
::
Matrix4d
>
velodyne_trans
=
std
::
make_shared
<
Eigen
::
Matrix4d
>
(
pose
);
lidar_process_
->
Process
(
time_stamp
,
cloud
,
velodyne_trans
);
std
::
vector
<
ObjectPtr
>
result_objects
=
lidar_process_
->
GetObjects
();
const
pcl_util
::
PointIndicesPtr
roi_indices
=
lidar_process_
->
GetROIIndices
();
AERROR
<<
"finish processing point cloud with num objects"
<<
result_objects
.
size
();
if
(
visualizer_
)
{
pcl_util
::
PointDCloudPtr
transformed_cloud
(
new
pcl_util
::
PointDCloud
);
transform_perception_cloud
(
cloud
,
pose
,
transformed_cloud
);
AERROR
<<
"transformed cloud size is "
<<
transformed_cloud
->
size
();
pcl_util
::
PointIndices
roi_indices_1
;
FrameContent
content
;
content
.
set_lidar_pose
(
pose
);
content
.
set_lidar_cloud
(
cloud
);
content
.
set_tracked_objects
(
result_objects
);
visualizer_
->
update_camera_system
(
&
content
);
visualizer_
->
render
(
content
);
}
AERROR
<<
"finish pc"
;
//oss << std::setfill('0') << std::setw(6) << i;
//std::string filename = FLAGS_output_path + oss.str() + ".txt";
//save_obstacle_informaton(result_objects, filename);
}
}
void
save_obstacle_informaton
(
std
::
vector
<
ObjectPtr
>&
objects
,
std
::
string
file_name
)
{
std
::
cout
<<
"save "
<<
objects
.
size
()
<<
" objects to "
<<
file_name
<<
std
::
endl
;
std
::
fstream
test_file
;
test_file
.
open
(
file_name
.
c_str
(),
std
::
fstream
::
out
);
std
::
cout
<<
"sort each object
\n
"
;
for
(
size_t
i
=
0
;
i
<
objects
.
size
();
++
i
)
{
std
::
cout
<<
i
<<
" "
;
ObjectPtr
obs
=
objects
[
i
];
PolygonDType
&
cloud
=
obs
->
polygon
;
if
(
cloud
.
size
()
==
0
)
{
continue
;
}
for
(
size_t
j
=
0
;
j
<
cloud
.
points
.
size
()
-
1
;
++
j
)
{
for
(
size_t
k
=
j
+
1
;
k
<
cloud
.
points
.
size
();
++
k
)
{
pcl_util
::
PointD
p1
=
cloud
.
points
[
j
];
pcl_util
::
PointD
p2
=
cloud
.
points
[
k
];
bool
need_change
=
false
;
if
(
static_cast
<
long
long
>
(
10000
*
p1
.
x
)
==
static_cast
<
long
long
>
(
10000
*
p2
.
x
))
{
if
(
static_cast
<
long
long
>
(
10000
*
p1
.
y
)
==
static_cast
<
long
long
>
(
10000
*
p2
.
y
))
{
need_change
=
static_cast
<
long
long
>
(
10000
*
p1
.
z
)
>
static_cast
<
long
long
>
(
10000
*
p2
.
z
);
}
else
{
need_change
=
static_cast
<
long
long
>
(
10000
*
p1
.
y
)
>
static_cast
<
long
long
>
(
10000
*
p2
.
y
);
}
}
else
{
need_change
=
static_cast
<
long
long
>
(
10000
*
p1
.
x
)
>
static_cast
<
long
long
>
(
10000
*
p2
.
x
);
}
if
(
need_change
)
{
pcl_util
::
PointD
tmp
=
cloud
.
points
[
j
];
cloud
.
points
[
j
]
=
cloud
.
points
[
k
];
cloud
.
points
[
k
]
=
tmp
;
}
}
}
}
std
::
cout
<<
"
\n
sort all objects
\n
"
;
for
(
int
i
=
0
;
i
<
(
int
)
objects
.
size
()
-
1
;
++
i
)
{
std
::
cout
<<
"
\r
"
<<
i
;
for
(
size_t
j
=
i
+
1
;
j
<
objects
.
size
();
++
j
)
{
bool
need_change
=
false
;
ObjectPtr
obs_i
=
objects
[
i
];
ObjectPtr
obs_j
=
objects
[
j
];
PolygonDType
&
cloud_i
=
obs_i
->
polygon
;
PolygonDType
&
cloud_j
=
obs_j
->
polygon
;
if
(
cloud_i
.
points
.
size
()
==
cloud_j
.
points
.
size
())
{
if
(
cloud_i
.
points
.
size
()
==
0
)
{
continue
;
}
pcl_util
::
PointD
p1
=
cloud_i
.
points
[
0
];
pcl_util
::
PointD
p2
=
cloud_j
.
points
[
0
];
if
(
static_cast
<
long
long
>
(
10000
*
p1
.
x
)
==
static_cast
<
long
long
>
(
10000
*
p2
.
x
))
{
if
(
static_cast
<
long
long
>
(
10000
*
p1
.
y
)
==
static_cast
<
long
long
>
(
10000
*
p2
.
y
))
{
need_change
=
static_cast
<
long
long
>
(
10000
*
p1
.
z
)
>
static_cast
<
long
long
>
(
10000
*
p2
.
z
);
}
else
{
need_change
=
static_cast
<
long
long
>
(
10000
*
p1
.
y
)
>
static_cast
<
long
long
>
(
10000
*
p2
.
y
);
}
}
else
{
need_change
=
static_cast
<
long
long
>
(
10000
*
p1
.
x
)
>
static_cast
<
long
long
>
(
10000
*
p2
.
x
);
}
}
else
{
need_change
=
cloud_i
.
points
.
size
()
>
cloud_j
.
points
.
size
();
}
if
(
need_change
)
{
ObjectPtr
obj_tmp
=
objects
[
i
];
objects
[
i
]
=
objects
[
j
];
objects
[
j
]
=
obj_tmp
;
}
}
}
std
::
cout
<<
"
\n
start to save objects
\n
"
;
for
(
size_t
i
=
0
;
i
<
objects
.
size
();
++
i
)
{
ObjectPtr
obs
=
objects
[
i
];
if
(
obs
->
polygon
.
size
()
<=
0
)
{
std
::
cout
<<
"Find object with empty convex hull "
<<
obs
->
cloud
->
size
()
<<
"
\n
"
;
continue
;
}
// id
test_file
<<
obs
->
track_id
<<
" "
;
// test_file << obs.get_idx() << " ";
// position
test_file
<<
std
::
setprecision
(
4
)
<<
std
::
fixed
<<
obs
->
center
[
0
]
<<
" "
<<
std
::
setprecision
(
4
)
<<
std
::
fixed
<<
obs
->
center
[
1
]
<<
" "
<<
std
::
setprecision
(
4
)
<<
std
::
fixed
<<
obs
->
center
[
2
]
<<
" "
;
// theta
double
theta
=
0
;
Eigen
::
Vector3d
dir
=
obs
->
direction
;
if
(
fabs
(
dir
[
0
])
<
DBL_MIN
)
{
if
(
dir
[
1
]
>
0
)
{
theta
=
M_PI
/
2.0
;
}
else
{
theta
=
-
M_PI
/
2.0
;
}
}
else
{
theta
=
atan
(
dir
[
1
]
/
dir
[
0
]);
}
test_file
<<
std
::
setprecision
(
4
)
<<
theta
<<
std
::
fixed
<<
" "
;
// velocity
test_file
<<
std
::
setprecision
(
4
)
<<
std
::
fixed
<<
obs
->
velocity
[
0
]
<<
std
::
fixed
<<
" "
<<
std
::
setprecision
(
4
)
<<
obs
->
velocity
[
1
]
<<
std
::
fixed
<<
" "
<<
std
::
setprecision
(
4
)
<<
obs
->
velocity
[
2
]
<<
std
::
fixed
<<
" "
;
// size
test_file
<<
std
::
setprecision
(
4
)
<<
std
::
fixed
<<
obs
->
length
<<
std
::
fixed
<<
" "
<<
std
::
setprecision
(
4
)
<<
obs
->
width
<<
std
::
fixed
<<
" "
<<
std
::
setprecision
(
4
)
<<
obs
->
height
<<
std
::
fixed
<<
" "
;
// dir
// test_file << std::setprecision(4) << obs.get_dir()[0] << fixed << " " << std::setprecision(4) << obs.get_dir()[1] << fixed << " ";
// polygon
PolygonDType
&
cloud
=
obs
->
polygon
;
test_file
<<
cloud
.
size
()
<<
" "
;
for
(
size_t
j
=
0
;
j
<
cloud
.
size
();
++
j
)
{
test_file
<<
std
::
setprecision
(
4
)
<<
std
::
fixed
<<
cloud
.
points
[
j
].
x
<<
std
::
fixed
<<
" "
<<
std
::
setprecision
(
4
)
<<
cloud
.
points
[
j
].
y
<<
std
::
fixed
<<
" "
<<
std
::
setprecision
(
4
)
<<
cloud
.
points
[
j
].
z
<<
std
::
fixed
<<
" "
;
}
// type
std
::
string
type
=
"unknow"
;
if
(
obs
->
type
==
UNKNOWN
)
{
type
=
"unknow"
;
}
else
if
(
obs
->
type
==
PEDESTRIAN
)
{
type
=
"pedestrian"
;
}
else
if
(
obs
->
type
==
VEHICLE
)
{
type
=
"car"
;
}
test_file
<<
type
<<
" "
;
test_file
<<
std
::
endl
;
}
}
protected:
ConfigManager
*
config_manager_
;
std
::
unique_ptr
<
LidarProcess
>
lidar_process_
;
//std::unique_ptr<PCLObstacleVisualizer> visualizer_;
std
::
unique_ptr
<
OpenglVisualizer
>
visualizer_
;
std
::
string
output_dir_
;
std
::
string
pose_dir_
;
std
::
string
label_file_
;
};
}
// namespace perception
}
// namespace apollo
int
main
(
int
argc
,
char
*
argv
[])
{
FLAGS_flagfile
=
"./modules/perception/tool/offline_visualizer_tool/conf/offline_lidar_perception_test.flag"
;
gflags
::
ParseCommandLineFlags
(
&
argc
,
&
argv
,
true
);
apollo
::
perception
::
OfflineLidarPerceptionTool
tool
;
tool
.
Init
(
FLAGS_enable_visualization
);
tool
.
Run
(
FLAGS_pcd_path
,
FLAGS_pose_path
);
return
0
;
}
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录