系統概述

LOAM的整體思想就是將復雜的SLAM問題分為：1. 高頻的運動估計； 2. 低頻的環境建圖。

Lidar接收數據，首先進行Point Cloud Registration，Lidar Odometry以10Hz的頻率進行運動估計和坐標轉換，Lidar Mapping以1Hz的頻率構建三維地圖，Transform Integration完成位姿的優化。這樣并行的結構保證了系統的實時性。

接下來是代碼的框架圖：

整個算法分為四個模塊，相對于其它直接匹配兩個點云的算法，LOAM是通過提取特征點進行匹配之后計算坐標變換。具體流程為：ScanRegistration 提取特征點并排除瑕點；LaserOdometry從特征點中估計運動，然后整合數據發送給LaserMapping；LaserMapping輸出的laser_cloud_surround為地圖；TransformMaintenance訂閱LaserOdometry與LaserMapping發布的Odometry消息，對位姿進行融合優化。后面將詳細進行說明。

ScanRegistration

這一模塊（節點）主要功能是：特征點的提取

一次掃描的點通過曲率值來分類，特征點曲率大于閾值的為邊緣點；特征點曲率小于閾值的為平面點。為了使特征點均勻的分布在環境中，將一次掃描劃分為4個獨立的子區域。每個子區域最多提供2個邊緣點和4個平面點。此外，將不穩定的特征點（瑕點）排除。下面將通過代碼進行說明。

從主函數開始：

int main(int argc, char** argv)
{ros::init(argc, argv, "scanRegistration");/** NodeHandle 是節點同ROS系統交流的主要接口* NodeHandle 在構造的時候會完整地初始化本節點 * NodeHandle 析構的時候會關閉此節點*/ros::NodeHandle nh;/** 參數1：話題名稱    * 參數2：信息隊列長度    * 參數3：回調函數，每當一個信息到來的時候，這個函數會被調用    * 返回一個ros::Subscriber類的對象，當此對象的所有引用都被銷毀是，本節點將不再是該話題的訂閱者 */   // 訂閱了velodyne_points和imu/dataros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_points", 2, laserCloudHandler);  ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);/** 我們通過advertise() 函數指定我們如何在給定的topic上發布信息* 它會觸發對ROS master的調用，master會記錄話題發布者和訂閱者* 在advertise()函數執行之后，master會通知每一個訂閱此話題的節點* 兩節點間由此可以建立直接的聯系* advertise()會返回一個Publisher對象，使用這個對象的publish方法我們就可以在此話題上發布信息* 當返回的Publisher對象的所有引用都被銷毀的時候，本節點將不再是該話題的發布者* 此函數是一個帶模板的函數，需要傳入具體的類型進行實例化* 傳入的類型就是要發布的信息的類型，在這里是String* 第一個參數是話題名稱* 第二個參數是信息隊列的長度，相當于信息的一個緩沖區* 在我們發布信息的速度大于處理信息的速度時* 信息會被緩存在先進先出的信息隊列里*/// 發布了6個話題：velodyne_cloud_2、laser_cloud_sharp、laser_cloud_flat、laser_cloud_less_flat、laser_cloud_less_sharp、imu_transpubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);/*** 它可以保證你指定的回調函數會被調用* 程序執行到spin()后就不調用其他語句了*/ros::spin();return 0;
}
回調函數，每當一個信息到來的時候，這個函數會被調用    * 返回一個ros::Subscriber類的對象，當此對象的所有引用都被銷毀是，本節點將不再是該話題的訂閱者 */   // 訂閱了velodyne_points和imu/dataros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_points", 2, laserCloudHandler);  ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);/** 我們通過advertise() 函數指定我們如何在給定的topic上發布信息* 它會觸發對ROS master的調用，master會記錄話題發布者和訂閱者* 在advertise()函數執行之后，master會通知每一個訂閱此話題的節點* 兩節點間由此可以建立直接的聯系* advertise()會返回一個Publisher對象，使用這個對象的publish方法我們就可以在此話題上發布信息* 當返回的Publisher對象的所有引用都被銷毀的時候，本節點將不再是該話題的發布者* 此函數是一個帶模板的函數，需要傳入具體的類型進行實例化* 傳入的類型就是要發布的信息的類型，在這里是String* 第一個參數是話題名稱* 第二個參數是信息隊列的長度，相當于信息的一個緩沖區* 在我們發布信息的速度大于處理信息的速度時* 信息會被緩存在先進先出的信息隊列里*/// 發布了6個話題：velodyne_cloud_2、laser_cloud_sharp、laser_cloud_flat、laser_cloud_less_flat、laser_cloud_less_sharp、imu_transpubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);/*** 它可以保證你指定的回調函數會被調用* 程序執行到spin()后就不調用其他語句了*/ros::spin();return 0;
}

主函數比較簡單，訂閱了2個節點和發布了6個節點。通過回調函數的處理，將處理后的點云重新發出去。主要涉及的函數為

laserCloudHandler與imuHandler。

·laserHandler

laserCloudHandler是這一模塊的重點部分，主要功能是對接收到的點云進行預處理，完成分類。具體分類內容為：一是將點云劃入不同線中存儲；二是對其進行特征分類。

首先對收到的點云進行處理

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{if (!systemInited) {systemInitCount++;if (systemInitCount >= systemDelay) {// systemDelay 有延時作用，保證有imu數據后在調用laserCloudHandlersystemInited = true;}return;}std::vector<int> scanStartInd(N_SCANS, 0);std::vector<int> scanEndInd(N_SCANS, 0);// Lidar的時間戳double timeScanCur = laserCloudMsg->header.stamp.toSec();pcl::PointCloud<pcl::PointXYZ> laserCloudIn;// fromROSmsg(input,cloud1) 轉為為模板點云laserCloudInpcl::fromROSMsg(*laserCloudMsg, laserCloudIn);std::vector<int> indices;//去除無效值pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);int cloudSize = laserCloudIn.points.size();//計算點云的起始角度/終止角度float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

接下來的處理是根據角度將點劃入不同數組中

  for (int i = 0; i < cloudSize; i++) {point.x = laserCloudIn.points[i].y;point.y = laserCloudIn.points[i].z;point.z = laserCloudIn.points[i].x;float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;int scanID;int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); if (roundedAngle > 0){scanID = roundedAngle;}else {// 角度大于0，由小到大劃入偶數線0-16；角度小于0，由大到小劃入奇數線15-1scanID = roundedAngle + (N_SCANS - 1);}if (scanID > (N_SCANS - 1) || scanID < 0 ){// 不在16線附近的點作為雜點進行剔除count--;continue;}

接下來計算每個點的相對方位角計算出相對時間，根據線性插值的方法計算速度及角度，并轉換到sweep k的初始imu坐標系下，再劃入16線數組中

  float ori = -atan2(point.x, point.z);if (!halfPassed) {if (ori < startOri - M_PI / 2) {ori += 2 * M_PI;} else if (ori > startOri + M_PI * 3 / 2) {ori -= 2 * M_PI;}if (ori - startOri > M_PI) {halfPassed = true;}} else {ori += 2 * M_PI;if (ori < endOri - M_PI * 3 / 2) {ori += 2 * M_PI;} else if (ori > endOri + M_PI / 2) {ori -= 2 * M_PI;} }//scanPeriod=0.1，是因為lidar工作周期是10HZ，意味著轉一圈是0.1秒；intensity是一個整數+小數，小數不會超過0.1，完成了按照時間排序的需求float relTime = (ori - startOri) / (endOri - startOri);point.intensity = scanID + scanPeriod * relTime;// imuPointerLast 是當前點，變量只在imu中改變，設為t時刻// 對每一個cloud point處理if (imuPointerLast >= 0) {float pointTime = relTime * scanPeriod;while (imuPointerFront != imuPointerLast) {// (timeScanCur + pointTime)設為ti時刻；imuPointerFront 是 ti后一個時刻if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {// 這個的意思是 imuPointerFront=imuPointerLast時候imuRollCur = imuRoll[imuPointerFront];imuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront];} else {// imuPointerBack = imuPointerFront - 1 線性插值求解出當前點對應的imu角度，位移和速度int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;}imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;}if (i == 0) {imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;} else {// 將Lidar位移轉到IMU起始坐標系下ShiftToStartIMU(pointTime);// 將Lidar運動速度轉到IMU起始坐標系下VeloToStartIMU();// 將點坐標轉到起始IMU坐標系下TransformToStartIMU(&point);}}// 將點按照每一層線，分類壓入16個數組中laserCloudScans[scanID].push_back(point);}

之后將對所有點進行曲率值的計算并記錄每一層曲率數組的起始和終止

cloudSize = count;pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());//將所有點存入laserCloud中，點按線序進行排列for (int i = 0; i < N_SCANS; i++) {*laserCloud += laserCloudScans[i];}int scanCount = -1;for (int i = 5; i < cloudSize - 5; i++) {// 對所有的激光點一個一個求出在該點前后5個點(10點)的偏差，作為cloudCurvature點云數據的曲率float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x+ laserCloud->points[i + 5].x;float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->fpoints[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y+ laserCloud->points[i + 5].y;float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z+ laserCloud->points[i + 5].z;cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;cloudSortInd[i] = i;cloudNeighborPicked[i] = 0;cloudLabel[i] = 0;if (int(laserCloud->points[i].intensity) != scanCount) {scanCount = int(laserCloud->points[i].intensity);//scanCount=scanID// 記錄每一層起始點和終止點的位置，需要根據這個起始/終止來操作點云曲率，在求曲率的過程中已經去除了前5個點和后5個點if (scanCount > 0 && scanCount < N_SCANS) {scanStartInd[scanCount] = i + 5;scanEndInd[scanCount - 1] = i - 5;}}}

接下來，對提到的兩種瑕點進行排除

 for (int i = 5; i < cloudSize - 6; i++) {float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;if (diff > 0.1) {float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + laserCloud->points[i].y * laserCloud->points[i].y +laserCloud->points[i].z * laserCloud->points[i].z);float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);/*— 針對論文的(b)情況，兩向量夾角小于某閾值b時（夾角小就可能存在遮擋），將其一側的臨近6個點設為不可標記為特征點的點 —*//*— 構建了一個等腰三角形的底向量，根據等腰三角形性質，判斷X[i]向量與X[i+1]的夾角小于5.732度(threshold=0.1) —*//*— depth1>depth2 X[i+1]距離更近，遠側點標記不特征；depth1<depth2 X[i]距離更近，遠側點標記不特征 —*/if (depth1 > depth2) {diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}} else {diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}}/*— 針對論文的(a)情況，當某點及其后點間的距離平方大于某閾值a（說明這兩點有一定距離） ———*//*— 若某點到其前后兩點的距離均大于c倍的該點深度，則該點判定為不可標記特征點的點 ———————*//*—（入射角越小，點間距越大，即激光發射方向與投射到的平面越近似水平） ———————————————*/float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;float dis = laserCloud->points[i].x * laserCloud->points[i].x+ laserCloud->points[i].y * laserCloud->points[i].y+ laserCloud->points[i].z * laserCloud->points[i].z;if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {cloudNeighborPicked[i] = 1;}}

之后對平面點以及角點進行篩選

  pcl::PointCloud<PointType> cornerPointsSharp;pcl::PointCloud<PointType> cornerPointsLessSharp;pcl::PointCloud<PointType> surfPointsFlat;pcl::PointCloud<PointType> surfPointsLessFlat;for (int i = 0; i < N_SCANS; i++) {/*—— 對于每一層激光點(總16層)，將每層區域分成6份，起始位置為sp，終止位置為ep。——————*//*—— 有兩個循環，作用是對cloudCurvature從小到大進行排序，cloudSortedInd是它的索引數組 ————*/pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);for (int j = 0; j < 6; j++) {int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;int ep = (scanStartInd[i] * (5 - j)  + scanEndInd[i] * (j + 1)) / 6 - 1;for (int k = sp + 1; k <= ep; k++) {for (int l = k; l >= sp + 1; l--) {if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {int temp = cloudSortInd[l - 1];cloudSortInd[l - 1] = cloudSortInd[l];cloudSortInd[l] = temp;}}}/*—— 篩選特征角點 Corner: label=2; LessCorner: label=1 ————*/   int largestPickedNum = 0;for (int k = ep; k >= sp; k--) {int ind = cloudSortInd[k];if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1) {largestPickedNum++;if (largestPickedNum <= 2) {cloudLabel[ind] = 2;cornerPointsSharp.push_back(laserCloud->points[ind]);cornerPointsLessSharp.push_back(laserCloud->points[ind]);} else if (largestPickedNum <= 20) {cloudLabel[ind] = 1;cornerPointsLessSharp.push_back(laserCloud->points[ind]);} else {break;}// 遍歷該曲率點后，將該點標記，并將該曲率點附近的前后5個點標記不被選取為特征點cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}}}/*—— 篩選特征平面點 Flat: label=-1 普通點和Flat點降采樣形成LessFlat: label=0 ————*/int smallestPickedNum = 0;for (int k = sp; k <= ep; k++) {int ind = cloudSortInd[k];if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1) {cloudLabel[ind] = -1;surfPointsFlat.push_back(laserCloud->points[ind]);smallestPickedNum++;if (smallestPickedNum >= 4) {break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}}}// surfPointsLessFlat為降采樣后的flat點，采樣前包含太多label=0的點for (int k = sp; k <= ep; k++) {if (cloudLabel[k] <= 0) {surfPointsLessFlatScan->push_back(laserCloud->points[k]);}}}// 降采樣pcl::PointCloud<PointType> surfPointsLessFlatScanDS;pcl::VoxelGrid<PointType> downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.filter(surfPointsLessFlatScanDS);surfPointsLessFlat += surfPointsLessFlatScanDS;}

之后再將信息發布出去

·imuHandler

imuHandler功能為imu信息的解析： ?

減去重力對imu的影響?
解析出當前時刻的imu時間戳，角度以及各個軸的加速度?
將加速度轉換到世界坐標系軸下?
進行航距推算，假定為勻速運動推算出當前時刻的位置)?
?推算當前時刻的速度信息

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// 減去重力加速度的影響float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;// 表明數組是一個長度為imuQueLength的循環數組imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuAccX[imuPointerLast] = accX;imuAccY[imuPointerLast] = accY;imuAccZ[imuPointerLast] = accZ;AccumulateIMUShift();
}
void AccumulateIMUShift()
{float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];// 轉換到世界坐標系下float x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;// imuPointerBack 為 imuPointerLast-1， 這樣處理是為了防止內存溢出int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];if (timeDiff < scanPeriod) {// 推算當前時刻的位置信息imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;// 推算當前時刻的速度信息imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;}
}

后面涉及的坐標變換即歐拉角的旋轉計算，這部分尚不熟悉，需要進一步學習。