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 < 0 || 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 < 0 || 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 < -24.33 || rowIdn > 50 || rowIdn < 0){continue;} else if(rowIdn % downsampleRate != 0) {continue;}} else {printf("wrong scan number\n");ROS_BREAK();} }

---

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 < M_PI) {cloudInfo.endOrientation += 2 * M_PI; } cloudInfo.orientationDiff = cloudInfo.endOrientation - cloudInfo.startOrientation; PointType point; for (int i = 0; i < cloudNum; ++i) {point.x = laserCloudIn->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 / 2) {ori += 2 * M_PI;} else 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 * 3 / 2) {ori += 2 * M_PI;} else if (ori > cloudInfo.endOrientation + M_PI / 2) {ori -= 2 * M_PI;}}float relTime = (ori - cloudInfo.startOrientation) / cloudInfo.orientationDiff;// 激光雷達10Hz，周期0.1laserCloudIn->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、話題名修改

# TopicspointCloudTopic: "points_raw" # Point cloud dataimuTopic: "imu_raw" # IMU data

2、根據KITTI采集數據的實際傳感器修改對應參數

# KITTIsensor: 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 / 1lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be usedlidarMaxRange: 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<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-100 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();pose1 << 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;pose1.close();

歐拉角到四元素的轉換除了用eigen，還可以參考大佬：四元數與歐拉角（Yaw、Pitch、Roll）的轉換

補充tum格式的軌跡輸出（拿ALOAM舉例，LIO-SAM修改相關參數即可）

// 位姿輸出到txt文檔 std::ofstream pose1("/home/cupido/output-data/kitti/sequences/07/aloam.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 = q_w_curr.toRotationMatrix(); Eigen::Matrix<double, 4, 4> myaloam_pose; myaloam_pose.topLeftCorner(3,3) = rotation_matrix;myaloam_pose(0,3) = t_w_curr.x(); myaloam_pose(1,3) = t_w_curr.y(); myaloam_pose(2,3) = t_w_curr.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, //04-10-6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02,9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01,0, 0, 0, 1; /*cali_paremeter << 2.347736981471e-04, -9.999441545438e-01, -1.056347781105e-02, -2.796816941295e-03, // 031.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 * myaloam_pose * cali_paremeter.inverse();Eigen::Matrix3d temp; temp = myloam_pose_f.topLeftCorner(3,3); Eigen::Quaterniond quaternion(temp);// 獲取當前更新的時間 這樣與ground turth對比才更準確 static double timeStart = odometryBuf.front()-> header.stamp.toSec(); // 相當于是第一幀的時間 auto T1 = ros::Time().fromSec(timeStart); pose1 << odomAftMapped.header.stamp - T1 << " "<< myloam_pose_f(0,3) << " "<< myloam_pose_f(1,3) << " "<< myloam_pose_f(2,3) << " "<< quaternion.x() << " "<< quaternion.y() << " "<< quaternion.z() << " "<< quaternion.w() << std::endl;pose1.close();

注意點：
1、輸出路徑注意修改
2、評估軌跡精度的時候，輸出軌跡若發現未和真值完全對齊（這里指的是，不考慮自己算法精度，單純兩軌跡對齊），可以在終端輸入以下指令：

//（注意是-a，不是--align_origin） evo_ape tum groundtruth.txt xxx.txt -a -p

九、點云地圖保存（PCD）

追根溯源：

1、注意到save_map.srv文件

float32 resolution string destination --- bool success

2、進入到mapOptmization.cpp

相關代碼：

// 訂閱一個保存地圖功能的服務 srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);/*** 保存全局關鍵幀特征點集合*/ bool saveMapService(lio_sam::save_mapRequest& req, lio_sam::save_mapResponse& res) {string saveMapDirectory;cout << "****************************************************" << endl;cout << "Saving map to pcd files ..." << endl;// 如果是空，說明是程序結束后的自動保存，否則是中途調用ros的service發布的保存指令if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;else saveMapDirectory = std::getenv("HOME") + req.destination;cout << "Save destination: " << saveMapDirectory << endl;// create directory and remove old files;// 刪掉之前有可能保存過的地圖int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());// save key frame transformations// 首先保存歷史關鍵幀軌跡pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);// extract global point cloud map（提取歷史關鍵幀角點、平面點集合）pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());// 遍歷所有關鍵幀，將點云全部轉移到世界坐標系下去for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);// 類似進度條的功能cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";}// 如果沒有指定分辨率，就是直接保存if(req.resolution != 0){cout << "\n\nSave resolution: " << req.resolution << endl;// down-sample and save corner cloud// 使用指定分辨率降采樣，分別保存角點地圖和面點地圖downSizeFilterCorner.setInputCloud(globalCornerCloud);downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);downSizeFilterCorner.filter(*globalCornerCloudDS);pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);// down-sample and save surf clouddownSizeFilterSurf.setInputCloud(globalSurfCloud);downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);downSizeFilterSurf.filter(*globalSurfCloudDS);pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);}else{// save corner cloudpcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);// save surf cloudpcl::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 << "****************************************************" << endl;cout << "Saving map to pcd files completed\n" << endl;return true; } std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO); // 可視化的線程（負責rviz相關可視化接口的發布）visualizeMapThread.join();/*** 展示線程* 1、發布局部關鍵幀map的特征點云* 2、保存全局關鍵幀特征點集合*/// 全局可視化線程void visualizeGlobalMapThread(){// 更新頻率設置為0.2hzros::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 << "Fail to save map" << endl;}}

這里注意到 if (savePCD == false)判斷條件，轉至配置文件params.yaml

3、最后在配置文件params.yaml修改參數

打開開關：

savePCD: true # https://github.com/TixiaoShan/LIO-SAM/issues/3

更改路徑：

savePCDDirectory: "/output-data/lio-sam/PCD/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

對應路徑和自身電腦的全局路徑的關系如下：

4、PCD效果展示

1、指令：

pcl_viewer xxx.pcd

2、效果圖：

此節參考大佬：
1、lio-sam中點云地圖保存
2、復現lio_sam激光slam算法創建點云地圖
3、PCL保存LIO-SAM地圖時報錯[pcl::PCDWriter::writeBinary]

全文參考文獻

1、ubuntu18運行編譯LIO-SAM并用官網和自己的數據建圖(修改匯總）
2、LIO-SAM運行自己數據包遇到的問題解決–SLAM不學無數術小問題
3、使用開源激光SLAM方案LIO-SAM運行KITTI數據集，如有用，請評論雷鋒
4、LIO-SAM：配置環境、安裝測試、適配??采集數據集
5、SLAM學習筆記（十九）開源3D激光SLAM總結大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及區別
6、多傳感器融合定位 第六章 慣性導航結算及誤差模型

總結

以上是生活随笔為你收集整理的在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。