一、kitti转ROS bag
1.1下载kitti数据集
其中kitti中的十个序列对应的raw data关系如下：
00: 2011_10_03_drive_0027
01: 2011_10_03_drive_0042
02: 2011_10_03_drive_0034
03: 2011_09_26_drive_0067
04: 2011_09_30_drive_0016
05: 2011_09_30_drive_0018
06: 2011_09_30_drive_0020
07: 2011_09_30_drive_0027
08: 2011_09_30_drive_0028
09: 2011_09_30_drive_0033
10: 2011_09_30_drive_0034
也就是意味着需要下载这几个数据集对应的，其中下载链接整理如下：
1）官网下载：KITTI，需要进行注册，比较麻烦。
2）百度云下载kitti数据集下载中的KITTI data_odometry_velodyne下载地址：百度云 密码:tc10，及大佬分享的文件
KITTI RAW 百度云盘，提取码：tsdp
找到数据集链接，接下来就是选择下载怎样的数据集：
1）任乾大佬的框架从零开始做自动驾驶定位(二): 数据集，下载*_sync.zip相关的数据集即可。
2）如果是LIO-SAM相关的算法，需要下载_extract.zip和_sync.zip两个部分，这是因为_extract.zip 包含的IMU数据是100Hz, 但是视觉的数据没有去畸变，此外激光数据是以txt格式存储的，在转换为bag格式的时候非常耗时．虽然_sync.zip数据中的IMU是10Hz, 但是视觉数据已经去畸变了，并且视觉和激光的时间戳已经同步好了，激光数据的存储格式是二进制格式bin存储的。
1.2kitti数据转ros bag
1）*_extract.zip转化方式
使用官方的kitti2bag进行转换，具体操作方式如下：
A. 升级numpy
这一步很重要，不然后面运行会报错。kitti2bag要求numpy版本>=1.12，ubuntu 16.04默认的是1.11，升级可以通过一条指令来完成。
sudo pip install -U numpy
B. 安装kitti2bag
sudo pip install kitti2bag
C.按照以下方式进行存储
D.运行
kitti2bag -t 2011_09_30 -r 0016 raw_synced
2）*_extract.zip和*_sync.zip
该种方式需要下载*_extract.zip对应的*_sync.zip，步骤与上述类似。
A.安装tpdm
pip3 install tqdm
B.下载kitti2bag.py
该文件是LIO-SAM作者提供的文件。
#!env python# -*- coding: utf-8 -*-import sys
try:import pykitti
except ImportError as e:print('Could not load module \'pykitti\'. Please run `pip install pykitti`')
sys.exit(1)import tf
import os
import cv2
import rospy
import rosbag
from tqdm import tqdm
from tf2_msgs.msg import TFMessage
from datetime import datetime
from std_msgs.msg import Header
from sensor_msgs.msg import CameraInfo, Imu, PointField, NavSatFix
import sensor_msgs.point_cloud2 as pcl2
from geometry_msgs.msg import TransformStamped, TwistStamped, Transform
from cv_bridge import CvBridge
import numpy as np
import argparse
defsave_imu_data(bag, kitti, imu_frame_id, topic):print("Exporting IMU")for timestamp, oxts inzip(kitti.timestamps, kitti.oxts):
q = tf.transformations.quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw)
imu = Imu()
imu.header.frame_id = imu_frame_id
imu.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
imu.linear_acceleration.x = oxts.packet.af
imu.linear_acceleration.y = oxts.packet.al
imu.linear_acceleration.z = oxts.packet.au
imu.angular_velocity.x = oxts.packet.wf
imu.angular_velocity.y = oxts.packet.wl
imu.angular_velocity.z = oxts.packet.wu
bag.write(topic, imu, t=imu.header.stamp)defsave_imu_data_raw(bag, kitti, imu_frame_id, topic):print("Exporting IMU Raw")
synced_path = kitti.data_path
unsynced_path = synced_path.replace('sync','extract')
imu_path = os.path.join(unsynced_path,'oxts')# read time stamp (convert to ros seconds format)withopen(os.path.join(imu_path,'timestamps.txt'))as f:
lines = f.readlines()
imu_datetimes =[]for line in lines:iflen(line)==1:continue
timestamp = datetime.strptime(line[:-4],'%Y-%m-%d %H:%M:%S.%f')
imu_datetimes.append(float(timestamp.strftime("%s.%f")))# fix imu time using a linear model (may not be ideal, ^_^)
imu_index = np.asarray(range(len(imu_datetimes)), dtype=np.float64)
z = np.polyfit(imu_index, imu_datetimes,1)
imu_datetimes_new = z[0]* imu_index + z[1]
imu_datetimes = imu_datetimes_new.tolist()# get all imu data
imu_data_dir = os.path.join(imu_path,'data')
imu_filenames =sorted(os.listdir(imu_data_dir))
imu_data =[None]*len(imu_filenames)for i, imu_file inenumerate(imu_filenames):
imu_data_file =open(os.path.join(imu_data_dir, imu_file),"r")for line in imu_data_file:iflen(line)==1:continue
stripped_line = line.strip()
line_list = stripped_line.split()
imu_data[i]= line_list
assertlen(imu_datetimes)==len(imu_data)for timestamp, data inzip(imu_datetimes, imu_data):
roll, pitch, yaw =float(data[3]),float(data[4]),float(data[5]),
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
imu = Imu()
imu.header.frame_id = imu_frame_id
imu.header.stamp = rospy.Time.from_sec(timestamp)
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
imu.linear_acceleration.x =float(data[11])
imu.linear_acceleration.y =float(data[12])
imu.linear_acceleration.z =float(data[13])
imu.angular_velocity.x =float(data[17])
imu.angular_velocity.y =float(data[18])
imu.angular_velocity.z =float(data[19])
bag.write(topic, imu, t=imu.header.stamp)
imu.header.frame_id ='imu_enu_link'
bag.write('/imu_correct', imu, t=imu.header.stamp)# for LIO-SAM GPSdefsave_dynamic_tf(bag, kitti, kitti_type, initial_time):print("Exporting time dependent transformations")if kitti_type.find("raw")!=-1:for timestamp, oxts inzip(kitti.timestamps, kitti.oxts):
tf_oxts_msg = TFMessage()
tf_oxts_transform = TransformStamped()
tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
tf_oxts_transform.header.frame_id ='world'
tf_oxts_transform.child_frame_id ='base_link'
transform =(oxts.T_w_imu)
t = transform[0:3,3]
q = tf.transformations.quaternion_from_matrix(transform)
oxts_tf = Transform()
oxts_tf.translation.x = t[0]
oxts_tf.translation.y = t[1]
oxts_tf.translation.z = t[2]
oxts_tf.rotation.x = q[0]
oxts_tf.rotation.y = q[1]
oxts_tf.rotation.z = q[2]
oxts_tf.rotation.w = q[3]
tf_oxts_transform.transform = oxts_tf
tf_oxts_msg.transforms.append(tf_oxts_transform)
bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp)elif kitti_type.find("odom")!=-1:
timestamps =map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)for timestamp, tf_matrix inzip(timestamps, kitti.T_w_cam0):
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
tf_stamped.header.frame_id ='world'
tf_stamped.child_frame_id ='camera_left'
t = tf_matrix[0:3,3]
q = tf.transformations.quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)defsave_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time):print("Exporting camera {}".format(camera))if kitti_type.find("raw")!=-1:
camera_pad ='{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path,'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir,'data')
image_filenames =sorted(os.listdir(image_path))withopen(os.path.join(image_dir,'timestamps.txt'))as f:
image_datetimes =map(lambda x: datetime.strptime(x[:-4],'%Y-%m-%d %H:%M:%S.%f'), f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height =tuple(util['S_rect_{}'.format(camera_pad)].tolist())
calib.distortion_model ='plumb_bob'
calib.K = util['K_{}'.format(camera_pad)]
calib.R = util['R_rect_{}'.format(camera_pad)]
calib.D = util['D_{}'.format(camera_pad)]
calib.P = util['P_rect_{}'.format(camera_pad)]elif kitti_type.find("odom")!=-1:
camera_pad ='{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path,'image_{}'.format(camera_pad))
image_filenames =sorted(os.listdir(image_path))
image_datetimes =map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.P = util['P{}'.format(camera_pad)]
iterable =zip(image_datetimes, image_filenames)for dt, filename in tqdm(iterable, total=len(image_filenames)):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]if camera in(0,1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding ="mono8"if camera in(0,1)else"bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw")!=-1:
image_message.header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt,"%s.%f")))
topic_ext ="/image_raw"elif kitti_type.find("odom")!=-1:
image_message.header.stamp = rospy.Time.from_sec(dt)
topic_ext ="/image_rect"
calib.header.stamp = image_message.header.stamp
bag.write(topic + topic_ext, image_message, t = image_message.header.stamp)
bag.write(topic +'/camera_info', calib, t = calib.header.stamp)defsave_velo_data(bag, kitti, velo_frame_id, topic):print("Exporting velodyne data")
velo_path = os.path.join(kitti.data_path,'velodyne_points')
velo_data_dir = os.path.join(velo_path,'data')
velo_filenames =sorted(os.listdir(velo_data_dir))withopen(os.path.join(velo_path,'timestamps.txt'))as f:
lines = f.readlines()
velo_datetimes =[]for line in lines:iflen(line)==1:continue
dt = datetime.strptime(line[:-4],'%Y-%m-%d %H:%M:%S.%f')
velo_datetimes.append(dt)
iterable =zip(velo_datetimes, velo_filenames)
count =0for dt, filename in tqdm(iterable, total=len(velo_filenames)):if dt isNone:continue
velo_filename = os.path.join(velo_data_dir, filename)# read binary data
scan =(np.fromfile(velo_filename, dtype=np.float32)).reshape(-1,4)# get ring channel
depth = np.linalg.norm(scan,2, axis=1)
pitch = np.arcsin(scan[:,2]/ depth)# arcsin(z, depth)
fov_down =-24.8/180.0* np.pi
fov =(abs(-24.8)+abs(2.0))/180.0* np.pi
proj_y =(pitch +abs(fov_down))/ fov # in [0.0, 1.0]
proj_y *=64# in [0.0, H]
proj_y = np.floor(proj_y)
proj_y = np.minimum(64-1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32)# in [0,H-1]
proj_y = proj_y.reshape(-1,1)
scan = np.concatenate((scan,proj_y), axis=1)
scan = scan.tolist()for i inrange(len(scan)):
scan[i][-1]=int(scan[i][-1])# create header
header = Header()
header.frame_id = velo_frame_id
header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt,"%s.%f")))# fill pcl msg
fields =[PointField('x',0, PointField.FLOAT32,1),
PointField('y',4, PointField.FLOAT32,1),
PointField('z',8, PointField.FLOAT32,1),
PointField('intensity',12, PointField.FLOAT32,1),
PointField('ring',16, PointField.UINT16,1)]
pcl_msg = pcl2.create_cloud(header, fields, scan)
pcl_msg.is_dense =True# print(pcl_msg)
bag.write(topic, pcl_msg, t=pcl_msg.header.stamp)# count += 1# if count > 200:# breakdefget_static_transform(from_frame_id, to_frame_id, transform):
t = transform[0:3,3]
q = tf.transformations.quaternion_from_matrix(transform)
tf_msg = TransformStamped()
tf_msg.header.frame_id = from_frame_id
tf_msg.child_frame_id = to_frame_id
tf_msg.transform.translation.x =float(t[0])
tf_msg.transform.translation.y =float(t[1])
tf_msg.transform.translation.z =float(t[2])
tf_msg.transform.rotation.x =float(q[0])
tf_msg.transform.rotation.y =float(q[1])
tf_msg.transform.rotation.z =float(q[2])
tf_msg.transform.rotation.w =float(q[3])return tf_msg
definv(transform):"Invert rigid body transformation matrix"
R = transform[0:3,0:3]
t = transform[0:3,3]
t_inv =-1* R.T.dot(t)
transform_inv = np.eye(4)
transform_inv[0:3,0:3]= R.T
transform_inv[0:3,3]= t_inv
return transform_inv
defsave_static_transforms(bag, transforms, timestamps):print("Exporting static transformations")
tfm = TFMessage()for transform in transforms:
t = get_static_transform(from_frame_id=transform[0], to_frame_id=transform[1], transform=transform[2])
tfm.transforms.append(t)for timestamp in timestamps:
time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))for i inrange(len(tfm.transforms)):
tfm.transforms[i].header.stamp = time
bag.write('/tf_static', tfm, t=time)defsave_gps_fix_data(bag, kitti, gps_frame_id, topic):for timestamp, oxts inzip(kitti.timestamps, kitti.oxts):
navsatfix_msg = NavSatFix()
navsatfix_msg.header.frame_id = gps_frame_id
navsatfix_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
navsatfix_msg.latitude = oxts.packet.lat
navsatfix_msg.longitude = oxts.packet.lon
navsatfix_msg.altitude = oxts.packet.alt
navsatfix_msg.status.service =1
bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp)defsave_gps_vel_data(bag, kitti, gps_frame_id, topic):for timestamp, oxts inzip(kitti.timestamps, kitti.oxts):
twist_msg = TwistStamped()
twist_msg.header.frame_id = gps_frame_id
twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
twist_msg.twist.linear.x = oxts.packet.vf
twist_msg.twist.linear.y = oxts.packet.vl
twist_msg.twist.linear.z = oxts.packet.vu
twist_msg.twist.angular.x = oxts.packet.wf
twist_msg.twist.angular.y = oxts.packet.wl
twist_msg.twist.angular.z = oxts.packet.wu
bag.write(topic, twist_msg, t=twist_msg.header.stamp)if __name__ =="__main__":
parser = argparse.ArgumentParser(description ="Convert KITTI dataset to ROS bag file the easy way!")# Accepted argument values
kitti_types =["raw_synced","odom_color","odom_gray"]
odometry_sequences =[]for s inrange(22):
odometry_sequences.append(str(s).zfill(2))
parser.add_argument("kitti_type", choices = kitti_types,help="KITTI dataset type")
parser.add_argument("dir", nargs ="?", default = os.getcwd(),help="base directory of the dataset, if no directory passed the deafult is current working directory")
parser.add_argument("-t","--date",help="date of the raw dataset (i.e. 2011_09_26), option is only for RAW datasets.")
parser.add_argument("-r","--drive",help="drive number of the raw dataset (i.e. 0001), option is only for RAW datasets.")
parser.add_argument("-s","--sequence", choices = odometry_sequences,help="sequence of the odometry dataset (between 00 - 21), option is only for ODOMETRY datasets.")
args = parser.parse_args()
bridge = CvBridge()
compression = rosbag.Compression.NONE
# compression = rosbag.Compression.BZ2# compression = rosbag.Compression.LZ4# CAMERAS
cameras =[(0,'camera_gray_left','/kitti/camera_gray_left'),(1,'camera_gray_right','/kitti/camera_gray_right'),(2,'camera_color_left','/kitti/camera_color_left'),(3,'camera_color_right','/kitti/camera_color_right')]if args.kitti_type.find("raw")!=-1:if args.date ==None:print("Date option is not given. It is mandatory for raw dataset.")print("Usage for raw dataset: kitti2bag raw_synced [dir] -t <date> -r <drive>")
sys.exit(1)elif args.drive ==None:print("Drive option is not given. It is mandatory for raw dataset.")print("Usage for raw dataset: kitti2bag raw_synced [dir] -t <date> -r <drive>")
sys.exit(1)
bag = rosbag.Bag("kitti_{}_drive_{}_{}.bag".format(args.date, args.drive, args.kitti_type[4:]),'w', compression=compression)
kitti = pykitti.raw(args.dir, args.date, args.drive)ifnot os.path.exists(kitti.data_path):print('Path {} does not exists. Exiting.'.format(kitti.data_path))
sys.exit(1)iflen(kitti.timestamps)==0:print('Dataset is empty? Exiting.')
sys.exit(1)try:# IMU
imu_frame_id ='imu_link'
imu_topic ='/kitti/oxts/imu'
imu_raw_topic ='/imu_raw'
gps_fix_topic ='/gps/fix'
gps_vel_topic ='/gps/vel'
velo_frame_id ='velodyne'
velo_topic ='/points_raw'
T_base_link_to_imu = np.eye(4,4)
T_base_link_to_imu[0:3,3]=[-2.71/2.0-0.05,0.32,0.93]# tf_static
transforms =[('base_link', imu_frame_id, T_base_link_to_imu),(imu_frame_id, velo_frame_id, inv(kitti.calib.T_velo_imu)),(imu_frame_id, cameras[0][1], inv(kitti.calib.T_cam0_imu)),(imu_frame_id, cameras[1][1], inv(kitti.calib.T_cam1_imu)),(imu_frame_id, cameras[2][1], inv(kitti.calib.T_cam2_imu)),(imu_frame_id, cameras[3][1], inv(kitti.calib.T_cam3_imu))]
util = pykitti.utils.read_calib_file(os.path.join(kitti.calib_path,'calib_cam_to_cam.txt'))# Export# save_static_transforms(bag, transforms, kitti.timestamps)# save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=None)# save_imu_data(bag, kitti, imu_frame_id, imu_topic)
save_imu_data_raw(bag, kitti, imu_frame_id, imu_raw_topic)
save_gps_fix_data(bag, kitti, imu_frame_id, gps_fix_topic)
save_gps_vel_data(bag, kitti, imu_frame_id, gps_vel_topic)for camera in cameras:
save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=None)break
save_velo_data(bag, kitti, velo_frame_id, velo_topic)finally:print("## OVERVIEW ##")print(bag)
bag.close()elif args.kitti_type.find("odom")!=-1:if args.sequence ==None:print("Sequence option is not given. It is mandatory for odometry dataset.")print("Usage for odometry dataset: kitti2bag {odom_color, odom_gray} [dir] -s <sequence>")
sys.exit(1)
bag = rosbag.Bag("kitti_data_odometry_{}_sequence_{}.bag".format(args.kitti_type[5:],args.sequence),'w', compression=compression)
kitti = pykitti.odometry(args.dir, args.sequence)ifnot os.path.exists(kitti.sequence_path):print('Path {} does not exists. Exiting.'.format(kitti.sequence_path))
sys.exit(1)
kitti.load_calib()
kitti.load_timestamps()iflen(kitti.timestamps)==0:print('Dataset is empty? Exiting.')
sys.exit(1)if args.sequence in odometry_sequences[:11]:print("Odometry dataset sequence {} has ground truth information (poses).".format(args.sequence))
kitti.load_poses()try:
util = pykitti.utils.read_calib_file(os.path.join(args.dir,'sequences',args.sequence,'calib.txt'))
current_epoch =(datetime.utcnow()- datetime(1970,1,1)).total_seconds()# Exportif args.kitti_type.find("gray")!=-1:
used_cameras = cameras[:2]elif args.kitti_type.find("color")!=-1:
used_cameras = cameras[-2:]
save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=current_epoch)for camera in used_cameras:
save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=current_epoch)finally:print("## OVERVIEW ##")print(bag)
bag.close()
C. 建立文件夹结构
D. 运行
在上一级文件夹运行
python3 kitti2bag.py -t 2011_09_30 -r 0016 raw_synced
结果可以看到
二、LIO SAM源码改编
这部分参考LOAM、Lego-liom、Lio-sam轨迹保存，与Kitti数据集真值进行评估和在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改。
改编原因：
1.因为LIO-SAM 要求输入的点云每个点都有ring 信息和相对时间time信息，目前的雷达驱动基本具备这些信息，但是早期的KITTI数据集不具备，所以代码要自己计算一下 ring和time;
2.因为EVO评价需要kitti或tum格式数据，因此需要改正激光里程计算输出未tum格式。
具体修改如下：
2.1加入ring和time
int rowIdn =-1;if(has_ring ==true){
rowIdn = laserCloudIn->points[i].ring;}else{float verticalAngle, horizonAngle;
verticalAngle =atan2(thisPoint.z,sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y))*180/ M_PI;
rowIdn =(verticalAngle + ang_bottom)/ ang_res_y;}if(rowIdn <0|| rowIdn >= N_SCAN)continue;if(rowIdn % downsampleRate !=0)continue;int columnIdn =-1;if(sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER){float horizonAngle =atan2(thisPoint.x, thisPoint.y)*180/ M_PI;staticfloat ang_res_x =360.0/float(Horizon_SCAN);
columnIdn =-round((horizonAngle-90.0)/ang_res_x)+ Horizon_SCAN/2;if(columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;}elseif(sensor == SensorType::LIVOX){
columnIdn = columnIdnCountVec[rowIdn];
columnIdnCountVec[rowIdn]+=1;}if(columnIdn <0|| columnIdn >= Horizon_SCAN)continue;if(rangeMat.at<float>(rowIdn, columnIdn)!= FLT_MAX)continue;if(has_ring ==true)
thisPoint =deskewPoint(&thisPoint, laserCloudIn->points[i].time);else{float ori =-atan2(thisPoint.y, thisPoint.x);if(!halfPassed){if(ori < cloudInfo.startOrientation - M_PI /2){
ori +=2* M_PI;}elseif(ori > cloudInfo.startOrientation + M_PI *3/2){
ori -=2* M_PI;}if(ori - cloudInfo.startOrientation > M_PI){
halfPassed =true;}}else{
ori +=2* M_PI;if(ori < cloudInfo.endOrientation - M_PI *3/2){
ori +=2* M_PI;}elseif(ori > cloudInfo.endOrientation + M_PI /2){
ori -=2* M_PI;}}float relTime =(ori - cloudInfo.startOrientation)/ cloudInfo.orientationDiff;// 激光雷达10Hz，周期0.1
laserCloudIn->points[i].time =0.1* relTime;
thisPoint =deskewPoint(&thisPoint, laserCloudIn->points[i].time);}
2.2输出tum格式的激光里程计数据
// 位姿输出到txt文档
std::ofstream pose2("zz/pose2.txt", std::ios::app);
pose2.setf(std::ios::scientific, std::ios::floatfield);// pose1.precision(15);//save final trajectory in the left camera coordinate system.
Eigen::Matrix3d rotation_matrix;
rotation_matrix =Eigen::AngleAxisd(pose_in.yaw, Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(pose_in.pitch, Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(pose_in.roll, Eigen::Vector3d::UnitX());
Eigen::Matrix<double,4,4> mylio_pose;
mylio_pose.topLeftCorner(3,3)= rotation_matrix;mylio_pose(0,3)= pose_in.x;mylio_pose(1,3)= pose_in.y;mylio_pose(2,3)= pose_in.z;
Eigen::Matrix<double,4,4> cali_paremeter;/*cali_paremeter << 4.276802385584e-04, -9.999672484946e-01, -8.084491683471e-03, -1.198459927713e-02, //00-02
-7.210626507497e-03, 8.081198471645e-03, -9.999413164504e-01, -5.403984729748e-02,
9.999738645903e-01, 4.859485810390e-04, -7.206933692422e-03, -2.921968648686e-01,
0, 0, 0, 1;*//*cali_paremeter << -1.857739385241e-03,-9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03,
-6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02,
9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01, //04-10
0 0, 0, 1;*/
cali_paremeter <<2.347736981471e-04,-9.999441545438e-01,-1.056347781105e-02,-2.796816941295e-03,1.044940741659e-02,1.056535364138e-02,-9.998895741176e-01,-7.510879138296e-02,9.999453885620e-01,1.243653783865e-04,1.045130299567e-02,-2.721327964059e-01,0,0,0,1;
Eigen::Matrix<double,4,4> myloam_pose_f;
myloam_pose_f = cali_paremeter * mylio_pose * cali_paremeter.inverse();
pose2 <<myloam_pose_f(0,0)<<" "<<myloam_pose_f(0,1)<<" "<<myloam_pose_f(0,2)<<" "<<myloam_pose_f(0,3)<<" "<<myloam_pose_f(1,0)<<" "<<myloam_pose_f(1,1)<<" "<<myloam_pose_f(1,2)<<" "<<myloam_pose_f(1,3)<<" "<<myloam_pose_f(2,0)<<" "<<myloam_pose_f(2,1)<<" "<<myloam_pose_f(2,2)<<" "<<myloam_pose_f(2,3)<< std::endl;
pose2.close();
修改后的完整代码链接：LIO-SAM for KITTI;
参照链接编译：关于LIO-SAM编译时出现错误 /usr/bin/ld: 找不到 -lBoost::timer
运行：
roslaunch lio_sam run.launch
看到以下结果：
使用以下命令进行地图保存：
source devel/setup.bash
rosservice call /lio_sam/save_map 0.2"/lio_sam_kitti_ws/src/MY-LIO-SAM-main/LIO-SAM-master/slam_data/"
下载kitti真值对应的TUM文件：KITTI数据集基准、转换成tum以及十个groundtruth对应图
三、evo评价
evo的安装可以参考evo安装与使用和EVO安装与问题解决，这里介绍快捷安装：
pip install evo --upgrade --no-binary evo
建立文件夹，将待评价的文件和真值文件放在同一个文件夹内：
3.1APE评价
在上述文件夹内：
evo_ape tum kitti_01_tum.txt 01_1.txt -r trans_part -va --plot --plot_mode xz --save_plot ~/kitti/dataset/01/result/01_1.pdf --save_results ~/kitti/dataset/01/result/01_1.zip
得到如下结果：
3.2RPE评价
evo_rpe tum kitti_01_tum.txt 01_1.txt -r trans_part -va --plot --plot_mode xz --save_plot ~/kitti/dataset/01/result/01_1r.pdf --save_results ~/kitti/dataset/01/result/01_1r.zip
3.3RES评价
evo_res 01_1.zip 01_2.zip 01_3.zip 01_4.zip 01_5.zip --use_filenames -p
3.4轨迹输出
evo_traj tum 01_1.txt 01_2.txt 01_3.txt 01_4.txt 01_5.txt kitti_01_tum.txt --ref=kitti_01_tum.txt -p -a --plot_mode xz --save_plot /home/ubuntu/kitti/dataset/01/result/traj01.pdf
该部分参考：
1.EVO工具的使用
2.SLAM评估工具-EVO使用(二)
3.evo安装、evo使用方法详细介绍使用教程，SLAM轨迹精度评估工具，如何用来评估ORB-SLAM2生成的轨迹精度，评估激光雷达SLAM与视觉SLAM的轨迹精度，量化SLAM的误差
本文转载自: https://blog.csdn.net/weixin_47552638/article/details/129759500
版权归原作者 小霍金 所有， 如有侵权，请联系我们删除。
版权归原作者 小霍金 所有， 如有侵权，请联系我们删除。