在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改-慈云数据

LIO-SAM跑KITTI数据集和自己数据集代码修改

一、编译并运行LIO-SAM
二、代码修改

1、cloud_info.msg
2、imageProjection.cpp
三、KITTI数据集准备
四、自己数据集准备
五、修改配置文件params.yaml
六、GPS信息的添加
七、效果图
八、轨迹保存
九、点云地图保存（PCD）

1、注意到save_map.srv文件
2、进入到mapOptmization.cpp
3、最后在配置文件params.yaml修改参数
4、PCD效果展示

全文参考文献

一、编译并运行LIO-SAM

参考我的另一篇文章：

Ubuntu20.04下的编译与运行LIO-SAM【问题解决】

二、代码修改

因为liosam 要求输入的点云每个点都有ring 信息和相对时间time信息，目前的雷达驱动基本具备这些信息，但是早期的KITTI数据集不具备，所以代码要自己计算一下 ring和time。方法可以参考lego-loam中这部分内容，具体修改如下。

1、cloud_info.msg

添加
# 用于改写ring和time相关
float32 startOrientation
float32 endOrientation
float32 orientationDiff

2、imageProjection.cpp

ring部分：

1、把ring通道强制关闭
2、添加计算ring代码
if (false){
	rowIdn = laserCloudIn->points[i].ring;
} else {
	verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
	// 拿16、32、64线激光雷达为例
	if(N_SCAN == 16) {
		rowIdn = int((verticalAngle + 15) / 2 + 0.5);
		if (rowIdn = N_SCAN)
		{
			continue;
		} else if(rowIdn % downsampleRate != 0) {
			continue;
		}
	} else if (N_SCAN == 32) {
    	rowIdn = int((verticalAngle + 92.0 / 3.0) * 3.0 / 4.0);
        if (rowIdn = N_SCAN)
        {
            continue;
        } else if(rowIdn % downsampleRate != 0) {
            continue;
        }
    } else if (N_SCAN == 64) {
        if (verticalAngle >= -8.83) {
            rowIdn = int((2 - verticalAngle) * 3.0 + 0.5);
        } else {
            rowIdn = N_SCAN / 2 + int((-8.83 - verticalAngle) * 2.0 + 0.5);
        }
        // use [0 50]  > 50 remove outlies 
        if (verticalAngle > 2 || verticalAngle  50 || rowIdn  
time部分： 
1、把time通道强制关闭
2、计算time并赋值
bool halfPassed = false;
int cloudNum = laserCloudIn->points.size();
// start and end orientation of this cloud
cloudInfo.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
cloudInfo.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
if (cloudInfo.endOrientation - cloudInfo.startOrientation > 3 * M_PI) {
    cloudInfo.endOrientation -= 2 * M_PI;
} else if (cloudInfo.endOrientation - cloudInfo.startOrientation points[i].x;
    point.y = laserCloudIn->points[i].y;
    point.z = laserCloudIn->points[i].z;
    float ori = -atan2(point.y, point.x);
    if (!halfPassed) {
        if (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 / 2) {
            ori -= 2 * M_PI;
        }
    }
    float relTime = (ori - cloudInfo.startOrientation) / cloudInfo.orientationDiff;
    // 激光雷达10Hz，周期0.1
    laserCloudIn->points[i].time = 0.1 * relTime;
}
 
需要修改好的，可以查看我的。 
三、KITTI数据集准备 
 
 关于KITTI数据集，已有公开的kitti2bag工具，使用方法：参见我的另一个博客在Ubuntu20.04系统上将KITTI原始数据集转化为.bag形式。但是输出的bag文件liosam是不能正常跑的，位姿Z型突变，仔细了解一下发现这个bag的imu频率跟雷达一样，也就是很低频，无法满足代码需求。liosam作者提供了一个2011_09_30_drive_0028.bag在Google Drive，但可能无法快速下载。 
 
如果想自己制作bag，作者自己改了kitti2bag就在源码的文件夹下，你需要准备如下文件（文件位置需对应）：
 
首先，在终端输入以下指令： 
pip3 install tqdm
 
效果： 
 
然后，在"2011_10_03"文件夹的上一级目录（即：此处的2011_10_03_calib文件下），打开终端，输入： 
python3 kitti2bag.py -t 2011_10_03 -r 0027 raw_synced
 
 
 注意自己的文件的文件名 
 
效果如下：
 
 
 我第一次文件位置不对，导致无法生成bag文件 
 
四、自己数据集准备 
具体采集步骤在后续更新… 
五、修改配置文件params.yaml 
1、话题名修改 
  # Topics
  pointCloudTopic: "points_raw"               # Point cloud data
  imuTopic: "imu_raw"                         # IMU data
 
2、根据KITTI采集数据的实际传感器修改对应参数 
  # KITTI
  sensor: velodyne                            # lidar sensor type, either 'velodyne' or 'ouster'
  N_SCAN: 64                                  # number of lidar channel (i.e., 16, 32, 64, 128)
  Horizon_SCAN: 2083                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used
 
 
 对照我的另一个博客：LeGO-LOAM跑KITTI数据集评估方法【代码改】 
 
3、外参的修改 
  # kitti外参
  extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01]
  extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03,
                  -7.854027e-04, 9.998898e-01, -1.482298e-02,
                  2.024406e-03, 1.482454e-02, 9.998881e-01]
  extrinsicRPY: [9.999976e-01,  7.553071e-04, -2.035826e-03,
                 -7.854027e-04, 9.998898e-01, -1.482298e-02,
                  2.024406e-03, 1.482454e-02, 9.998881e-01]
 
 
 注意点：
1、配置文件（params.yaml）内的参数通过参数服务器传送进源程序，会覆盖掉头文件内（utility.h）的对应参数。
2、其中extrinsicRot表示imu -> lidar的坐标变换，用于旋转imu坐标系下的角速度和线加速度到lidar坐标系下，extrinsicRPY则用于旋转imu坐标系下的欧拉角（姿态信息）到lidar坐标下，由于lio-sam作者使用的imu的欧拉角旋转指向与lidar坐标系不一致，因此使用了两个不同的旋转矩阵，但是大部分的设备两个旋转应该是设置为相同的。 
 
六、GPS信息的添加 
待更新… 
七、效果图 
KITTI：
 
 
 00的可能会飞，05以后的应该没问题 
 
八、轨迹保存 
找到输出位姿信息，通过以下代码，输出位姿信息（KITTI格式）： 
// 位姿输出到txt文档
std::ofstream pose1("/home/cupido/output-data/lio-sam/pose.txt", std::ios::app);
pose1.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 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 cali_paremeter;
/*cali_paremeter 
  string saveMapDirectory;
  cout 
      *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i],  &cloudKeyPoses6D-points[i]);
      *globalSurfCloud   += *transformPointCloud(surfCloudKeyFrames[i],    &cloudKeyPoses6D-points[i]);
      // 类似进度条的功能
      cout 
    cout 
    // save corner cloud
    pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
    // save surf cloud
    pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
  }
  // save global point cloud map（保存到一起，全局关键帧特征点集合）
  *globalMapCloud += *globalCornerCloud;
  *globalMapCloud += *globalSurfCloud;
  // 保存全局地图
  int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
  res.success = ret == 0;
  downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
  downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
  cout 
        // 更新频率设置为0.2hz
        ros::Rate rate(0.2);
        while (ros::ok()){
            rate.sleep();
            // 发布局部关键帧map的特征点云
            publishGlobalMap();
        }
        // 当ros被杀死之后，执行保存地图功能
        if (savePCD == false)
            return;
        lio_sam::save_mapRequest  req;
        lio_sam::save_mapResponse res;
        // 保存全局关键帧特征点集合
        if(!saveMapService(req, res)){
            cout