Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
94c4f61c
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,发现更多精彩内容 >>
提交
94c4f61c
编写于
12月 11, 2019
作者:
C
chenguang09
提交者:
Xiangquan Xiao
12月 13, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
[Tools]: add support to lidar calibration with localization pose
上级
88050759
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
68 addition
and
29 deletion
+68
-29
modules/tools/sensor_calibration/extract_data.py
modules/tools/sensor_calibration/extract_data.py
+10
-10
modules/tools/sensor_calibration/extract_static_data.py
modules/tools/sensor_calibration/extract_static_data.py
+18
-14
modules/tools/sensor_calibration/sensor_msg_extractor.py
modules/tools/sensor_calibration/sensor_msg_extractor.py
+40
-5
未找到文件。
modules/tools/sensor_calibration/extract_data.py
浏览文件 @
94c4f61c
...
...
@@ -36,7 +36,7 @@ from cyber.proto import record_pb2
from
configuration_yaml_generator
import
ConfigYaml
from
extract_static_data
import
get_subfolder_list
,
select_static_image_pcd
from
modules.tools.sensor_calibration.proto
import
extractor_config_pb2
from
sensor_msg_extractor
import
GpsParser
,
ImageParser
,
PointCloudParser
from
sensor_msg_extractor
import
GpsParser
,
ImageParser
,
PointCloudParser
,
PoseParser
#from scripts.record_bag import SMALL_TOPICS
...
...
@@ -108,7 +108,7 @@ def get_sensor_channel_list(record_file):
"""Get the channel list of sensors for calibration."""
record_reader
=
RecordReader
(
record_file
)
return
set
(
channel_name
for
channel_name
in
record_reader
.
get_channellist
()
if
'sensor'
in
channel_name
)
if
'sensor'
in
channel_name
or
'/localization/pose'
in
channel_name
)
def
validate_channel_list
(
channels
,
dictionary
):
ret
=
True
...
...
@@ -131,6 +131,8 @@ def build_parser(channel, output_path):
parser
=
PointCloudParser
(
output_path
=
output_path
,
instance_saving
=
True
)
elif
channel
.
endswith
(
"/gnss/odometry"
):
parser
=
GpsParser
(
output_path
=
output_path
,
instance_saving
=
False
)
elif
channel
.
endswith
(
"/localization/pose"
):
parser
=
PoseParser
(
output_path
=
output_path
,
instance_saving
=
False
)
else
:
raise
ValueError
(
"Not Support this channel type: %s"
%
channel
)
...
...
@@ -368,8 +370,8 @@ def reorganize_extracted_data(tmp_data_path, task_name, remove_input_data_cache=
if
task_name
==
'lidar_to_gnss'
:
print
(
get_subfolder_list
(
tmp_data_path
))
subfolders
=
[
x
for
x
in
get_subfolder_list
(
tmp_data_path
)
if
'_apollo_sensor_'
in
x
]
odometry_subfolders
=
[
x
for
x
in
subfolders
if
'_odometry
'
in
x
]
if
'_apollo_sensor_'
in
x
or
'_localization_pose'
]
odometry_subfolders
=
[
x
for
x
in
subfolders
if
'_odometry'
in
x
or
'_pose
'
in
x
]
lidar_subfolders
=
[
x
for
x
in
subfolders
if
'_PointCloud2'
in
x
]
print
(
lidar_subfolders
)
print
(
odometry_subfolders
)
...
...
@@ -405,11 +407,11 @@ def reorganize_extracted_data(tmp_data_path, task_name, remove_input_data_cache=
# data selection.
pair_data_folder_name
=
'camera-lidar-pairs'
cameras
,
lidar
=
select_static_image_pcd
(
path
=
tmp_data_path
,
min_distance
=
5
,
stop_times
=
5
,
min_distance
=
5
,
stop_times
=
4
,
wait_time
=
3
,
check_range
=
50
,
image_static_diff_threshold
=
0.005
,
image_suffix
=
'.png'
,
pcd_suffix
=
'.pcd'
,
output_folder_name
=
pair_data_folder_name
)
output_folder_name
=
pair_data_folder_name
,
image_suffix
=
'.jpg'
,
pcd_suffix
=
'.pcd'
)
lidar_name
=
get_substring
(
str
=
lidar
,
prefix
=
'_apollo_sensor_'
,
suffix
=
'_PointCloud2'
)
for
camera
in
cameras
:
camera_name
=
get_substring
(
str
=
camera
,
prefix
=
'_apollo_sensor_'
,
suffix
=
'_image'
)
...
...
@@ -512,9 +514,7 @@ def main():
ret
=
extract_data
(
valid_record_list
,
output_abs_path
,
channels
,
start_timestamp
,
end_timestamp
,
extraction_rates
)
if
not
ret
:
raise
ValueError
(
'Failed to extract data!'
)
# output_abs_path='/apollo/data/extracted_data/CoolHigh-2019-09-20/lidar_to_gnss-2019-12-10-15-56/tmp'
reorganize_extracted_data
(
tmp_data_path
=
output_abs_path
,
task_name
=
config
.
io_config
.
task_name
)
# generate_compressed_file(input_path=config.io_config.output_path,
...
...
modules/tools/sensor_calibration/extract_static_data.py
浏览文件 @
94c4f61c
...
...
@@ -40,7 +40,8 @@ from modules.tools.sensor_calibration.proto import extractor_config_pb2
CYBER_PATH
=
os
.
environ
[
'CYBER_PATH'
]
CYBER_RECORD_HEADER_LENGTH
=
2048
Z_FILL_LEN
=
5
Z_FILL_LEN
=
4
Z_DEFAULT_LEN
=
5
def
mkdir_p
(
path
):
if
not
os
.
path
.
isdir
(
path
):
...
...
@@ -139,7 +140,7 @@ def get_difference_score_between_images(path, file_indexes,
image_diff
=
np
.
zeros
(
len
(
file_indexes
),
dtype
=
np
.
float32
)
image_thumbnails
=
[]
for
c
,
idx
in
enumerate
(
file_indexes
):
image_file
=
os
.
path
.
join
(
path
,
str
(
int
(
idx
)).
zfill
(
Z_
FILL
_LEN
)
+
suffix
)
image_file
=
os
.
path
.
join
(
path
,
str
(
int
(
idx
)).
zfill
(
Z_
DEFAULT
_LEN
)
+
suffix
)
image
=
cv2
.
imread
(
image_file
)
image_thumbnails
.
append
(
cv2
.
resize
(
image
,
(
thumbnail_size
,
thumbnail_size
),
interpolation
=
cv2
.
INTER_AREA
))
...
...
@@ -164,7 +165,7 @@ def check_static_by_odometry(data, index, check_range=40,
start_idx
=
np
.
maximum
(
index
-
check_range
,
0
)
end_idx
=
np
.
minimum
(
index
+
check_range
,
data
.
shape
[
0
]
-
1
)
# skip if start and end index are too nearby.
if
end_idx
-
start_idx
<
=
check_range
:
if
end_idx
-
start_idx
<
2
*
check_range
:
return
False
# calculate x-y plane distance
distance
=
get_distance_by_odometry
(
data
,
start_idx
,
end_idx
)
...
...
@@ -174,12 +175,15 @@ def check_static_by_odometry(data, index, check_range=40,
def
select_static_image_pcd
(
path
,
min_distance
=
5
,
stop_times
=
5
,
wait_time
=
3
,
check_range
=
50
,
image_static_diff_threshold
=
0.005
,
image_suffix
=
'.png'
,
pcd_suffix
=
'.pcd'
,
output_folder_name
=
'camera-lidar-pairs'
):
"""select pairs of images and pcds"""
output_folder_name
=
'camera-lidar-pairs'
,
image_suffix
=
'.jpg'
,
pcd_suffix
=
'.pcd'
):
"""
select pairs of images and pcds, odometry information may
come from /apollolocalization/pose as well
"""
subfolders
=
[
x
for
x
in
get_subfolder_list
(
path
)
if
'_apollo_sensor_'
in
x
]
lidar_subfolder
=
[
x
for
x
in
subfolders
if
'_PointCloud2'
in
x
]
odometry_subfolder
=
[
x
for
x
in
subfolders
if
'_odometry'
in
x
]
odometry_subfolder
=
[
x
for
x
in
subfolders
if
'_odometry'
in
x
or
'_pose'
in
x
]
camera_subfolders
=
[
x
for
x
in
subfolders
if
'_image'
in
x
]
if
len
(
lidar_subfolder
)
is
not
1
or
\
len
(
odometry_subfolder
)
is
not
1
:
...
...
@@ -223,7 +227,7 @@ def select_static_image_pcd(path, min_distance=5, stop_times=5,
camera_folder_path
=
os
.
path
.
join
(
path
,
camera
)
print
(
'foder: {}'
.
format
(
camera_folder_path
))
camera_diff
=
get_difference_score_between_images
(
camera_folder_path
,
timestamp_dict
[
camera
][:,
0
])
camera_folder_path
,
timestamp_dict
[
camera
][:,
0
]
,
suffix
=
image_suffix
)
valid_image_indexes
=
[
x
for
x
,
v
in
enumerate
(
camera_diff
)
if
v
<=
image_static_diff_threshold
]
valid_images
=
(
timestamp_dict
[
camera
][
valid_image_indexes
,
0
]).
astype
(
int
)
...
...
@@ -242,6 +246,7 @@ def select_static_image_pcd(path, min_distance=5, stop_times=5,
if
last_idx
is
-
1
:
last_idx
=
i
last_odometry_idx
=
odometry_idx
candidate_idx
.
append
(
i
)
continue
time_interval
=
timestamp_dict
[
camera
][
i
,
1
]
-
timestamp_dict
[
camera
][
last_idx
,
1
]
odomerty_interval
=
\
...
...
@@ -276,13 +281,13 @@ def select_static_image_pcd(path, min_distance=5, stop_times=5,
mkdir_p
(
output_path
)
for
count
,
i
in
enumerate
(
image_idx
):
# save images
in_file
=
os
.
path
.
join
(
camera_folder_path
,
str
(
int
(
i
)).
zfill
(
Z_
FILL
_LEN
)
+
image_suffix
)
out_file
=
os
.
path
.
join
(
output_path
,
str
(
int
(
count
)).
zfill
(
Z_FILL_LEN
)
+
image_suffix
)
in_file
=
os
.
path
.
join
(
camera_folder_path
,
str
(
int
(
i
)).
zfill
(
Z_
DEFAULT
_LEN
)
+
image_suffix
)
out_file
=
os
.
path
.
join
(
output_path
,
str
(
int
(
count
)).
zfill
(
Z_FILL_LEN
)
+
'_00'
+
image_suffix
)
copyfile
(
in_file
,
out_file
)
j
=
camera_lidar_nearest_pairs
[
i
]
# save pcd
in_file
=
os
.
path
.
join
(
path
,
lidar_subfolder
,
str
(
int
(
j
)).
zfill
(
Z_
FILL
_LEN
)
+
pcd_suffix
)
out_file
=
os
.
path
.
join
(
output_path
,
str
(
int
(
count
)).
zfill
(
Z_FILL_LEN
)
+
pcd_suffix
)
in_file
=
os
.
path
.
join
(
path
,
lidar_subfolder
,
str
(
int
(
j
)).
zfill
(
Z_
DEFAULT
_LEN
)
+
pcd_suffix
)
out_file
=
os
.
path
.
join
(
output_path
,
str
(
int
(
count
)).
zfill
(
Z_FILL_LEN
)
+
'_00'
+
pcd_suffix
)
copyfile
(
in_file
,
out_file
)
print
(
"generate image-lidar-pair:[%d, %d]"
%
(
i
,
j
))
return
camera_subfolders
,
lidar_subfolder
...
...
@@ -324,8 +329,7 @@ def main():
select_static_image_pcd
(
path
=
args
.
workspace
,
min_distance
=
5
,
stop_times
=
5
,
wait_time
=
3
,
check_range
=
50
,
image_static_diff_threshold
=
0.005
,
image_suffix
=
'.png'
,
pcd_suffix
=
'.pcd'
)
image_static_diff_threshold
=
0.005
)
if
__name__
==
'__main__'
:
main
()
modules/tools/sensor_calibration/sensor_msg_extractor.py
浏览文件 @
94c4f61c
...
...
@@ -29,6 +29,7 @@ import struct
from
modules.drivers.proto
import
sensor_image_pb2
from
modules.drivers.proto
import
pointcloud_pb2
from
modules.localization.proto
import
gps_pb2
from
modules.localization.proto
import
localization_pb2
from
data_file_object
import
TimestampFileObject
,
OdometryFileObject
...
...
@@ -121,13 +122,47 @@ class GpsParser(SensorMessageParser):
odometry_file_obj
.
save_to_file
(
self
.
_parsed_data
)
return
True
class
PoseParser
(
GpsParser
):
"""
inherit similar data saver and data structure from GpsParser
save the ego-localization information same as odometry
"""
def
_init_parser
(
self
):
self
.
_msg_parser
=
localization_pb2
.
LocalizationEstimate
()
def
parse_sensor_message
(
self
,
msg
):
""" parse localization information from localization estimate channel"""
loc_est
=
self
.
_msg_parser
loc_est
.
ParseFromString
(
msg
.
message
)
# all double, except point_type is int32
ts
=
loc_est
.
header
.
timestamp_sec
self
.
_timestamps
.
append
(
ts
)
point_type
=
56
qw
=
loc_est
.
pose
.
orientation
.
qw
qx
=
loc_est
.
pose
.
orientation
.
qx
qy
=
loc_est
.
pose
.
orientation
.
qy
qz
=
loc_est
.
pose
.
orientation
.
qz
x
=
loc_est
.
pose
.
position
.
x
y
=
loc_est
.
pose
.
position
.
y
z
=
loc_est
.
pose
.
position
.
z
# save 9 values as a tuple, for eaisier struct packing during storage
if
self
.
_instance_saving
:
raise
ValueError
(
"localization--pseudo odometry-- should be saved in a file"
)
else
:
self
.
_parsed_data
.
append
((
ts
,
point_type
,
qw
,
qx
,
qy
,
qz
,
x
,
y
,
z
))
return
True
class
PointCloudParser
(
SensorMessageParser
):
"""
class to parse apollo/$(lidar)/PointCloud2 channels.
saving separately each parsed msg
"""
def
__init__
(
self
,
output_path
,
instance_saving
=
True
):
def
__init__
(
self
,
output_path
,
instance_saving
=
True
,
suffix
=
'.pcd'
):
super
(
PointCloudParser
,
self
).
__init__
(
output_path
,
instance_saving
)
self
.
_suffix
=
suffix
def
convert_xyzit_pb_to_array
(
self
,
xyz_i_t
,
data_type
):
arr
=
np
.
zeros
(
len
(
xyz_i_t
),
dtype
=
data_type
)
...
...
@@ -189,7 +224,7 @@ class PointCloudParser(SensorMessageParser):
self
.
_parsed_data
=
self
.
make_xyzit_point_cloud
(
pointcloud
.
point
)
if
self
.
_instance_saving
:
file_name
=
"%05d
.pcd"
%
self
.
get_msg_count
()
file_name
=
"%05d
"
%
self
.
get_msg_count
()
+
self
.
_suffix
output_file
=
os
.
path
.
join
(
self
.
_output_path
,
file_name
)
self
.
save_pointcloud_meta_to_file
(
pc_meta
=
self
.
_parsed_data
,
pcd_file
=
output_file
)
else
:
...
...
@@ -203,9 +238,9 @@ class ImageParser(SensorMessageParser):
class to parse apollo/$(camera)/image channels.
saving separately each parsed msg
"""
def
__init__
(
self
,
output_path
,
instance_saving
=
True
):
def
__init__
(
self
,
output_path
,
instance_saving
=
True
,
suffix
=
'.jpg'
):
super
(
ImageParser
,
self
).
__init__
(
output_path
,
instance_saving
)
self
.
_suffix
=
suffix
def
_init_parser
(
self
):
self
.
_msg_parser
=
sensor_image_pb2
.
Image
()
...
...
@@ -242,7 +277,7 @@ class ImageParser(SensorMessageParser):
(
image
.
height
,
image
.
width
,
channel_num
))
if
self
.
_instance_saving
:
file_name
=
"%05d
.png"
%
self
.
get_msg_count
()
file_name
=
"%05d
"
%
self
.
get_msg_count
()
+
self
.
_suffix
output_file
=
os
.
path
.
join
(
self
.
_output_path
,
file_name
)
self
.
save_image_mat_to_file
(
image_file
=
output_file
)
else
:
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录