（四）可视化ＩＭＵ数据

一、ＩＭＵ数据格式

从KITTI的readme文件中https://github.com/yanii/kitti-pcl/blob/master/KITTI_README.TXT可以看到IMU里数据的格式,
　　KITTI中的IMUS数据文件存在oxts文件夹下，以txt格式保存。

　　其中有[‘lat’,‘lon’,‘alt’,‘roll’,‘pith’,‘yaw’,‘vn’,‘ve’,‘vf’,‘vl’,‘vu’,‘ax’,‘ay’,‘az’,‘af’,‘al’,‘au’,‘wx’,‘wy’,‘wz’,‘wf’,
‘wl’,‘wu’,‘posacc’,‘velacc’,‘navstat’,‘numsats’,‘posmode’,‘velomde’,‘orimode’]

而我们常用的数据有：

• lat: 维度(deg)
• lon：经度(deg)
• alt：海拔(m)
• roll :是围绕Z轴旋转，也叫翻滚角(rad)；
• pith :是围绕X轴旋转，也叫做俯仰角(rad)；
• yaw :是围绕Y轴旋转，也叫偏航角(rad)。
• vf：前进方向速度(m/s)
• vl：左方向速度(m/s)
• vu：垂直向上方向(m/s)
• ax：x方向加速度(m^2/s)
• ay：y方向加速度(m^2/s)
• az：z方向加速度(m^2/s)
• af：前进方向加速度(m^2/s)
• al：左方向加速度(m^2/s)
• au：垂直向上方向加速度(m^2/s)
  　　
  以上数据ax,ay,az以GPS/IMU坐标系下采集的数据，而vf,vl,vu,af,al,au是以车辆坐标系，在这里也就是velo坐标系的数据，因此使用这几项数据，而不用坐标转换。
  
  通过其中的roll pith 和yew 即可描述此时车辆的姿态，其代表含义如下：
  

二、ROS发布IMU Meaasge

在链接地址中是ROS中对IMU消息定义的sensor_msgs/Imu.msg消息格式：
http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html
在发布Message时需要建立一个Header，然后使用roll，pith，yaw设置车辆的旋转量，使用af，al，au设定线性加速度 以voludyne 坐标系为准，再使用wf，wl，wu设置角速度。

• Header header
• geometry_msgs/Quaternion orientation
  float64[9] orientation_covariance # Row major about x, y, z axes
• geometry_msgs/Vector3 angular_velocity
  float64[9] angular_velocity_covariance # Row major about x, y, z axes
• geometry_msgs/Vector3 linear_acceleration
  float64[9] linear_acceleration_covariance # Row major x, y z

使用ROSPY定义如下：
publish_utils.py

def publish_imu(imu_pub,imu_data):
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = FRAME_ID

# 使用roll,pith,yaw设定车辆旋转量
q=tf.transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pith), float(imu_data.yaw))
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]

# 使用af,al,au设定线形加速度 以voludyne 坐标系为准
imu.linear_acceleration.x = imu_data.af
imu.linear_acceleration.y = imu_data.al
imu.linear_acceleration.z = imu_data.au

# 使用wf,wl,wu设置角速度
imu.angular_velocity.x = imu_data.wf
imu.angular_velocity.y = imu_data.wl
imu.angular_velocity.z = imu_data.wu

imu_pub.publish(imu)

对imu文件进行解析读取：
data_utils.py

# 读取IMU数据
IMU_COLUMN_NAMES = ['lat','lon','alt','roll','pith','yaw','vn','ve','vf','vl','vu','ax','ay','az','af','al','au','wx','wy','wz','wf',
'wl','wu','posacc','velacc','navstat','numsats','posmode','velomde','orimode']

def read_imu(path):
df = pd.read_csv(path,header = None,sep = ' ')
df.columns = IMU_COLUMN_NAMES
return df

kitti.py

# 发布时使用ros中的imu格式发布
imu_pub = rospy.Publisher('kitti_imu', Imu ,queue_size=10)

publish_imu(imu_pub,imu_data)

将Imu的话题在RVIZ可视化后， 效果如下图：