KITTI数据集转ros_msg（odometry）

    最近做LOAM相关的东西，在这里把自己做的东西记下来，免得以后忘记。

    前些天看完了代码，虽然一些细节没弄清楚，但大致思路以及里面的一些推导都没问题了，现在要用KITTI的odometry部分来测试代码性能，遇到的第一个问题就是把odometry中的数据读取出来，再转化成rosmsg发布出去。

    首先从KITTI官网下载好odometry的数据包，数据包非常大，有几百个G。解压后顶层目录是odometry，下一层是dataset，在下一层是sequences和poses，sequence下是不同路段的相机、雷达数据以及校准和时序信息，相机数据包括双目彩色相机，黑白相机，poses下是不同路段的位姿数据，存的是变换矩阵T。

    LOAM用到的当然是激光数据，所以和我们相关的就只有sequence下的雷达数据与时序信息，当然后期进行位姿的误差计算还要用到位姿数据。

    官网上有数据集的接口文件，odometry数据集的接口文件还有Python脚本的例程，例程如下：

"""Example of pykitti.odometry usage."""import rospyfrom std_msgs.msg import Stringimport itertoolsimport matplotlib.pyplot as pltimport numpy as npfrom mpl_toolkits.mplot3d import Axes3Dimport pykitti__author__ = "Lee Clement"__email__ = "[e-mail]"start_time=time.time()# Change this to the directory where you store KITTI databasedir = '/home/luolun/Odometry/dataset'# Specify the dataset to loadsequence = '00'# Load the data. Optionally, specify the frame range to load.# Passing imformat='cv2' will convert images to uint8 and BGR for# easy use with OpenCV.# dataset = pykitti.odometry(basedir, sequence)dataset = pykitti.odometry(basedir, sequence, frames=range(0, 20, 5))         #function range generate a sequence staring from 0 and ending at 20 with a step of 5                                                                               #odometry.py line 139: dataset.velo is iterator for N*4 array of (x,y,z,reflection)# dataset.calib:      Calibration data are accessible as a named tuple# dataset.timestamps: Timestamps are parsed into a list of timedelta objects# dataset.poses:      Generator to load ground truth poses T_w_cam0# dataset.camN:       Generator to load individual images from camera N# dataset.gray:       Generator to load monochrome stereo pairs (cam0, cam1)# dataset.rgb:        Generator to load RGB stereo pairs (cam2, cam3)# dataset.velo:       Generator to load velodyne scans as [x,y,z,reflectance]# Grab some datasecond_pose = next(iter(itertools.islice(dataset.poses, 1, None)))           #islice here take the poses from pose 1 to pose index(end), iter make it be a iterator,next(iter) is the (iter+1)th pose #first_gray = next(iter(dataset.gray))#first_cam1 = next(iter(dataset.cam1))#first_rgb = next(iter(dataset.rgb))#first_cam2 = next(iter(dataset.cam2))third_velo = next(iter(itertools.islice(dataset.velo, 2, None)))# Display some of the data#np.set_printoptions(precision=4, suppress=True)                              #while print data, reserve 4 valid nums, supress disable science notation#print('\nSequence: ' + str(dataset.sequence))                                #it works for all the next print#print('\nFrame range: ' + str(dataset.frames))#print('\nGray stereo pair baseline [m]: ' + str(dataset.calib.b_gray))#print('\nRGB stereo pair baseline [m]: ' + str(dataset.calib.b_rgb))#print('\nFirst timestamp: ' + str(dataset.timestamps[0]))#print('\nSecond ground truth pose:\n' + str(second_pose))#f, ax = plt.subplots(2, 2, figsize=(15, 5))#ax[0, 0].imshow(first_gray[0], cmap='gray')#ax[0, 0].set_title('Left Gray Image (cam0)')#ax[0, 1].imshow(first_cam1, cmap='gray')#ax[0, 1].set_title('Right Gray Image (cam1)')#ax[1, 0].imshow(first_cam2)#ax[1, 0].set_title('Left RGB Image (cam2)')#ax[1, 1].imshow(first_rgb[1])#ax[1, 1].set_title('Right RGB Image (cam3)')#f2 = plt.figure()#ax2 = f2.add_subplot(111, projection='3d')#Plot every 100th point so things don't get too bogged down#velo_range = range(0, third_velo.shape[0], 10)#print(third_velo.shape[0])#print(third_velo[third_velo.shape[0]-1, 0])#ax2.scatter(third_velo[velo_range, 0],#            third_velo[velo_range, 1],#            third_velo[velo_range, 2],#            c=third_velo[velo_range, 3],#            cmap='Blues')#ax2.set_title('Third Velodyne scan (subsampled)')#plt.show()

    跟我们任务不相关的代码行我都注释掉了，得到的third_velo变量是第三个点云的数据，他是一个N*4的矩阵，N是点云点个数，每一行是[x ,y ,z ,reflection]，也就是坐标和反射率。想得到特定帧的点云在切片函数islice将起点改为对应帧-1即可。

    根据LOAM的代码，LOAM接收的msg格式是pointCloud2，所以要从sensor_msgs.msg中导入PointCloud2。

    然后导入point_cloud2，使用point_cloud2的create_cloud函数可以将点云转为PointCloud2类型的ros_msg。

"""Example of pykitti.odometry usage."""import rospyfrom std_msgs.msg import Headerimport itertoolsimport numpy as npfrom sensor_msgs.msg import PointCloud2, PointFieldfrom sensor_msgs import point_cloud2import pclimport pykitti __author__ = "Lun Luo"__email__ = "[e-mail]"# Change this to the directory where you store KITTI databasedir = '/home/luolun/Odometry/dataset'# Specify the dataset to loadsequence = '00'dataset = pykitti.odometry(basedir, sequence, frames=range(0, 20, 5))         #function range generate a sequence staring from 0 and ending at 20 with a step of 5                                                                               #odometry.py line 139: dataset.velo is iterator for N*4 array of (x,y,z,reflection)# Grab some datasecond_pose = next(iter(itertools.islice(dataset.poses, 1, None)))           #islice here take the poses from pose 1 to pose index(end), iter make it be a iterator,next(iter) is the (iter+1)th pose third_velo = next(iter(itertools.islice(dataset.velo, 2, None)))#pclPoints转ros_msgs::PointCloud2pointcloudHeader = Header(1,12,'1')pointFiledx = PointField('x',0,7,1)pointFiledy = PointField('y',4,7,1)pointFiledz = PointField('z',8,7,1)pointFiledi = PointField('intensity',12,7,1)pointFiled = [pointFiledx,pointFiledy,pointFieldz,pointFieldi]sensor_msg_pointcloud = point_cloud2.creat_cloud(header=pointcloudHeader,fields=pointFiled,data=third_velo)

    这里有几个参数说明一下。首先是header，header是标准msg中的header，第二个参数是时间戳，第三个参数是帧名，第一个帧序列数。PointField存了点云的格式信息，第一个参数字段名，第二个参数是偏移（offset，单位byte），第三个参数是数据类型，这里是float32，第四个参数是每个点中该字段数据个数。

    以上就是从odometry中的bin雷达数据到rosmsg的全过程，随后就可以直接编写ROS节点发布sensor_msg_pointcloud了。