学习笔记之——数据集的生成
Posted gwpscut
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了学习笔记之——数据集的生成相关的知识,希望对你有一定的参考价值。
本博文记录本人收集数据集的过程,本博文仅仅为本人的学习记录。
目录
传感器的驱动
Event Camera
关于事件相机的驱动安装可以参考博文《》
DAVIS346
Dvxplorer
Frame-based Camera
RealSense D435i
T265
LiDAR
Ouster LiDAR (3D)
RPLiDAR (2D)
GPS
IMU
硬件架构设计
传感器标定
其他补充
2D LiDAR数据集
此处再补充一下一些公开的2D lidar的数据集
Public Data — Cartographer ROS documentation
SLAM Benchmarking Datasets (需要编写把clf文件转化为rosbag文件的python脚本)
Pre-2014 Robotics 2D-Laser Datasets – StachnissLab
注意,有部分是log文件或者clf文件的,需要通过python转换成rosbag
里面采用的CARMEN log files 来记录传感器数据
CARMEN log files是什么?
Convert CARMEN log file to rosbag - ROS Answers: Open Source Q&A Forum
官方给出的github转换(更好可以转换的包) GitHub - artivis/carmen_publisher: [python][ROS] convert CARMEN log files to ROS msgs
GitHub - artivis/carmen_publisher at legacy/python
GitHub - ooalyokhina/ConvertCarmenToRosbag at convert
.log .clf文件转化为 .bag文件源码
文件的含义
# message_name [message contents] ipc_timestamp ipc_hostname logger_timestamp # message formats defined: PARAM SYNC ODOM FLASER RLASER TRUEPOS # PARAM param_name param_value # SYNC tagname # ODOM x y theta tv rv accel # FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta # RLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta # TRUEPOS true_x true_y true_theta odom_x odom_y odom_theta # NMEA-GGA utc latitude lat_orient longitude long_orient gps_quality num_sattelites hdop sea_level alitude geo_sea_level geo_sep data_age
自己修改的代码如下
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Software License Agreement (BSD License)
#
# Copyright (c) 2016, Martin Guenther
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
'''This is a converter for the Intel Research Lab SLAM dataset
( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
to rosbag'''
import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
def make_tf_msg(x, y, z,theta, t,parentid,childid):
trans = TransformStamped()
trans.header.stamp = t
trans.header.frame_id = parentid
trans.child_frame_id = childid
trans.transform.translation.x = x
trans.transform.translation.y = y
trans.transform.translation.z = z
q = tf.transformations.quaternion_from_euler(0, 0, theta)
trans.transform.rotation.x = q[0]
trans.transform.rotation.y = q[1]
trans.transform.rotation.z = q[2]
trans.transform.rotation.w = q[3]
msg = TFMessage()
msg.transforms.append(trans)
return msg
with open('aces.clf') as dataset:
with rosbag.Bag('MIT_Infinite_Corridor_Dataset.bag', 'w') as bag:
for line in dataset.readlines():
line = line.strip()
tokens = line.split(' ')
if len(tokens) <= 2:
continue
if tokens[0] == 'FLASER': #FLASER这是原始文件中原来标注激光数据的,一般不需要更改
msg = LaserScan()
num_scans = int(tokens[1])
if num_scans != 180 or len(tokens) < num_scans + 9:
rospy.logwarn("unsupported scan format")
continue
msg.header.frame_id = 'base_laser' #这是激光坐标系id,根据需要更改,在ROS中读取数据时会使用到,一般也不需要更改。
t = rospy.Time(float(tokens[(num_scans + 8)])) #获取时间
msg.header.stamp = t
msg.angle_min = -90.0 / 180.0 * pi
msg.angle_max = 90.0 / 180.0 * pi
msg.angle_increment = pi / num_scans
msg.time_increment = 0.2 / 360.0
msg.scan_time = 0.2
msg.range_min = 0.001
msg.range_max = 50.0
msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
bag.write('scan', msg, t) #这是激光数据发布的话题,根据节点程序订阅的具体话题更改。
odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
tf_msg = make_tf_msg(0.1, 0, 0.2, 0, t,'base_link','base_laser')
bag.write('tf', tf_msg, t)
elif tokens[0] == 'ODOM': #标志里程计数据
odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
t = rospy.Time(float(tokens[7]))
msg=Odometry()
msg.header.stamp=t
msg.header.frame_id='odom'
msg.child_frame_id='base_link'
msg.pose.pose.position.x=odom_x
msg.pose.pose.position.y=odom_y
msg.pose.pose.position.z=0
q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
msg.pose.pose.orientation.x = q[0]
msg.pose.pose.orientation.y = q[1]
msg.pose.pose.orientation.z = q[2]
msg.pose.pose.orientation.w = q[3]
bag.write('odom', msg, t)
# tf_msg = make_tf_msg(odom_x, odom_y,0 , odom_theta, t,'odom','base_link')
# bag.write('tf', tf_msg, t)
GitHub - Robots90/clf2bag: Transfer clf file to bag file for ROS
.log .clf文件转化为 .bag文件 github_Robots.的博客-CSDN博客
用数据集跑激光slam,gmapping/karto_范坤3371的博客-CSDN博客
参考下面图片对corrected-log的数据进行进一步处理
获取odom数据
获取lidar数据可以参考:
carmen_to_rosbag/old_laser_scan.py at master · art32fil/carmen_to_rosbag · GitHub
最终修改版本如下:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Software License Agreement (BSD License)
#
# Copyright (c) 2016, Martin Guenther
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
'''This is a converter for the Intel Research Lab SLAM dataset
( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
to rosbag'''
import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
def make_tf_msg(x, y, z,theta, t,parentid,childid):
trans = TransformStamped()
trans.header.stamp = t
trans.header.frame_id = parentid
trans.child_frame_id = childid
trans.transform.translation.x = x
trans.transform.translation.y = y
trans.transform.translation.z = z
q = tf.transformations.quaternion_from_euler(0, 0, theta)
trans.transform.rotation.x = q[0]
trans.transform.rotation.y = q[1]
trans.transform.rotation.z = q[2]
trans.transform.rotation.w = q[3]
msg = TFMessage()
msg.transforms.append(trans)
return msg
# 0 | 1 | 2 - 1+num_readings | 2+num_readings | 3+num_readings |
# xLASER | num_readings | [range_readings] | x | y |
#-------------------------------------------------------------------------------
# 4+num_readings | 5+num_readings | 6+num_readings | 7+num_readings |
# theta | odom_x | odom_y | odom_theta |
#-------------------------------------------------------------------------------
# 8+num_readings | 9+num_readings | 10+num_readings
# ipc_timestamp | ipc_hostname |logger_timestamp
with open('mit-killian.clf') as dataset:
with rosbag.Bag('MIT.bag', 'w') as bag:
for line in dataset.readlines():
line = line.strip()
tokens = line.split(' ')
if len(tokens) <= 2:
continue
if tokens[0] == 'FLASER': #FLASER这是原始文件中原来标注激光数据的,一般不需要更改
msg = LaserScan()
num_scans = int(tokens[1])
if num_scans != 180 or len(tokens) < num_scans + 9:
rospy.logwarn("unsupported scan format")
continue
msg.header.frame_id = 'base_laser' #这是激光坐标系id,根据需要更改,在ROS中读取数据时会使用到,一般也不需要更改。
t = rospy.Time(float(tokens[(num_scans + 8)])) #获取时间
msg.header.stamp = t
msg.angle_min = -90.0 / 180.0 * pi
msg.angle_max = 90.0 / 180.0 * pi
msg.angle_increment = pi / num_scans
msg.time_increment = 0.2 / 360.0
msg.scan_time = 0.2
msg.range_min = 0.001
msg.range_max = 50.0
msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
bag.write('scan', msg, t) #这是激光数据发布的话题,根据节点程序订阅的具体话题更改。
tf_msg = make_tf_msg(0.1, 0, 0.2, 0, t,'base_link','base_laser')
bag.write('tf', tf_msg, t)
# odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
# odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 5):(num_scans + 8)]]
# msg1=Odometry()
# msg1.header.stamp=t
# msg1.header.frame_id='odom'
# msg1.child_frame_id='base_link'
# msg1.pose.pose.position.x=odom_x
# msg1.pose.pose.position.y=odom_y
# msg1.pose.pose.position.z=0
# q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
# msg1.pose.pose.orientation.x = q[0]
# msg1.pose.pose.orientation.y = q[1]
# msg1.pose.pose.orientation.z = q[2]
# msg1.pose.pose.orientation.w = q[3]
# bag.write('odom', msg1, t)
# 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9
# ODOM| x | y |theta| tv| rv|accel|ipc_timestamp|ipc_hostname|logger_timestamp
elif tokens[0] == 'ODOM': #标志里程计数据
odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
t = rospy.Time(float(tokens[7]))
msg=Odometry()
msg.header.stamp=t
msg.header.frame_id='odom'
msg.child_frame_id='base_link'
msg.pose.pose.position.x=odom_x
msg.pose.pose.position.y=odom_y
msg.pose.pose.position.z=0
q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
msg.pose.pose.orientation.x = q[0]
msg.pose.pose.orientation.y = q[1]
msg.pose.pose.orientation.z = q[2]
msg.pose.pose.orientation.w = q[3]
bag.write('odom', msg, t)
# tf_msg = make_tf_msg(odom_x, odom_y,0 , odom_theta, t,'odom','base_link')
# bag.write('tf', tf_msg, t)
跑MIT Infinite Corridor Dataset的效果如下:
所用的参数文档如下
-- Copyright 2016 The Cartographer Authors
--
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options =
map_builder = MAP_BUILDER, -- map_builder.lua的配置信息
trajectory_builder = TRAJECTORY_BUILDER, -- trajectory_builder.lua的配置信息
map_frame = "map", -- 地图坐标系的名字
tracking_frame = "base_laser", -- 将所有传感器数据转换到这个坐标系下
published_frame = "base_link", -- tf: map -> published_frame
odom_frame = "odom", -- 里程计的坐标系名字
provide_odom_frame = true, -- 是否提供odom的tf, 如果为true,则tf树为map->odom->published_frame
-- 如果为false tf树为map->published_frame
publish_frame_projected_to_2d = false, -- 是否将坐标系投影到平面上
use_pose_extrapolator = true, -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿
use_odometry = true, -- 是否使用里程计,如果使用要求一定要有odom的tf
use_nav_sat = false, -- 是否使用gps
use_landmarks = false, -- 是否使用landmark
num_laser_scans = 1, -- 是否使用单线激光数据
num_multi_echo_laser_scans = 0, -- 是否使用multi_echo_laser_scans数据
num_subdivisions_per_laser_scan = 1, -- 1帧数据被分成几次处理,一般为1
num_point_clouds = 0, -- 是否使用点云数据
lookup_transform_timeout_sec = 0.2, -- 查找tf时的超时时间
submap_publish_period_sec = 0.3, -- 发布数据的时间间隔
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1., -- 传感器数据的采样频率
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.min_range = 0.001
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false --不采用imu数据
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
-- -- ####################################################################################################
-- MAP_BUILDER.use_trajectory_builder_2d = true
-- MAP_BUILDER.num_background_threads =6 --建议优化至每个机器的CPU线程数量
-- TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 --积累几帧激光数据作为一个标准单位scan
-- TRAJECTORY_BUILDER_2D.min_range = 0.01 --激光的最近有效距离
-- TRAJECTORY_BUILDER_2D.max_range = 26. --激光最远的有效距离(可以接受的调整参数范围在30 - 15之间,推荐20-25误差较小。)
-- -- TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. --无效激光数据设置距离为该数值
-- TRAJECTORY_BUILDER_2D.use_imu_data = false --是否使用imu数据
-- TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
-- --线距离搜索框,在这个框的大小内,搜索最佳scan匹配 减小该参数可以增强实时的建图效果,降低闭环优化的效果,形成闭环时,产生的重影较多
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher. angular_search_window = math.rad(10.) --角度搜索框的大小
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 20.
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 2e-1 --影响的是过程中的效果,间接会影响最后的优化时间长
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 30.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 30.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 7 --该参数误差波动较大,平均误差大时最大误差小,平均误差小时最大误差大,在7左右处于相对平衡点,推荐7-10为修改参数范围
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80 --num_range_data设置的值与CPU有这样一种关系,值小(10),CPU使用率比较稳定,整体偏高,值大时,CPU短暂爆发使用(插入子图的时候),平时使用率低,呈现极大的波动状态。 可以接受的调整参数范围在90 - 60之间,在80左右效果较为优秀。
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05 //尽量小点 // 如果移动距离过小, 或者时间过短, 不进行地图的更新
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.3)
-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5
-- -- 增加如下
-- TRAJECTORY_BUILDER_2D.voxel_filter_size=0.1 --平均误差在0.1 和0.2时发生起伏,虽然0.1的误差比0.2的更大,但由于0.2时绘图偏差严重,建议选择小于0.1的值。
-- TRAJECTORY_BUILDER_2D.submaps.resolution=0.05 --平均误差在0.1时达到顶峰,建议小于0.1较为良好。
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range=30 --可以接受的调整参数范围在50 - 20之间,在30左右效果较为优秀。
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length=4 --可以接受的调整参数范围在0.5 - 4之间,在2 - 4左右效果较为优秀。
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points=200 --可以接受的调整参数范围在100 - 200之间,100以上的效果较为优秀。
-- POSE_GRAPH.optimization_problem.huber_scale = 1e2 --鲁棒核函数,去噪
-- POSE_GRAPH.optimize_every_n_nodes = 35 --后端优化节点 (推荐参数修改范围为30-70,官方文档中建议减少该值来提高速度,推荐30左右配置较为良好)
-- POSE_GRAPH.global_constraint_search_after_n_seconds = 30 --该参数随着间隔时间的增加误差的方差和最大误差逐渐下降,但最大误差逐渐上升,在30-40时处于两者的平衡点。推荐30-40为修改参数范围
-- POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 15 --优化迭代步数
-- POSE_GRAPH.optimization_problem.ceres_solver_options.num_threads = 1
-- POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.25 --该参数在0.2-0.25左右平均误差处于相对低位。最小方差的位置在在0.25左右,推荐参数修改范围为0.2-0.3
-- POSE_GRAPH.constraint_builder.min_score = 0.75 --该参数在0.75-0.95误差的方差和最大误差处于相对低位,推荐0.75-0.95为修改参数范围
-- POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 3.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 5. --搜索方法,界定分支法,求解问题构成一个搜索树,depth是构造树的深度
-- POSE_GRAPH.global_sampling_ratio = 0.001 --在0.001,0.002时平均误差处于较小位置,优化时可以考虑的值为0.001和0.002
-- -- 增加如下
return options
参考资料
常用的公共数据集(一)_yhgao96的博客-CSDN博客_公共数据集
以上是关于学习笔记之——数据集的生成的主要内容,如果未能解决你的问题,请参考以下文章
实验笔记之——CARMEN (.log .clf)文件转换为rosbag