在研讀了論文及開源代碼后，對LOAM的一些理解做一個(gè)整理。

文章：Low-drift and real-time lidar odometry and mapping

開源代碼：https://github.com/daobilige-su/loam_velodyne

系統(tǒng)概述

LOAM的整體思想就是將復(fù)雜的SLAM問題分為：1. 高頻的運(yùn)動(dòng)估計(jì)； 2. 低頻的環(huán)境建圖。

Lidar接收數(shù)據(jù)，首先進(jìn)行Point Cloud Registration，Lidar Odometry以10Hz的頻率進(jìn)行運(yùn)動(dòng)估計(jì)和坐標(biāo)轉(zhuǎn)換，Lidar Mapping以1Hz的頻率構(gòu)建三維地圖，Transform Integration完成位姿的優(yōu)化。這樣并行的結(jié)構(gòu)保證了系統(tǒng)的實(shí)時(shí)性。

接下來是代碼的框架圖：

?

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

ScanRegistration

這一模塊（節(jié)點(diǎn)）主要功能是：特征點(diǎn)的提取

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

從主函數(shù)開始：

int main(int argc, char** argv) {ros::init(argc, argv, "scanRegistration");/** NodeHandle 是節(jié)點(diǎn)同ROS系統(tǒng)交流的主要接口* NodeHandle 在構(gòu)造的時(shí)候會(huì)完整地初始化本節(jié)點(diǎn) * NodeHandle 析構(gòu)的時(shí)候會(huì)關(guān)閉此節(jié)點(diǎn)*/ros::NodeHandle nh;/** 參數(shù)1：話題名稱 * 參數(shù)2：信息隊(duì)列長度 * 參數(shù)3：回調(diào)函數(shù)，每當(dāng)一個(gè)信息到來的時(shí)候，這個(gè)函數(shù)會(huì)被調(diào)用 * 返回一個(gè)ros::Subscriber類的對象，當(dāng)此對象的所有引用都被銷毀是，本節(jié)點(diǎn)將不再是該話題的訂閱者 */ // 訂閱了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() 函數(shù)指定我們?nèi)绾卧诮o定的topic上發(fā)布信息* 它會(huì)觸發(fā)對ROS master的調(diào)用，master會(huì)記錄話題發(fā)布者和訂閱者* 在advertise()函數(shù)執(zhí)行之后，master會(huì)通知每一個(gè)訂閱此話題的節(jié)點(diǎn)* 兩節(jié)點(diǎn)間由此可以建立直接的聯(lián)系* advertise()會(huì)返回一個(gè)Publisher對象，使用這個(gè)對象的publish方法我們就可以在此話題上發(fā)布信息* 當(dāng)返回的Publisher對象的所有引用都被銷毀的時(shí)候，本節(jié)點(diǎn)將不再是該話題的發(fā)布者* 此函數(shù)是一個(gè)帶模板的函數(shù)，需要傳入具體的類型進(jìn)行實(shí)例化* 傳入的類型就是要發(fā)布的信息的類型，在這里是String* 第一個(gè)參數(shù)是話題名稱* 第二個(gè)參數(shù)是信息隊(duì)列的長度，相當(dāng)于信息的一個(gè)緩沖區(qū)* 在我們發(fā)布信息的速度大于處理信息的速度時(shí)* 信息會(huì)被緩存在先進(jìn)先出的信息隊(duì)列里*/// 發(fā)布了6個(gè)話題：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);/*** 它可以保證你指定的回調(diào)函數(shù)會(huì)被調(diào)用* 程序執(zhí)行到spin()后就不調(diào)用其他語句了*/ros::spin();return 0; } 回調(diào)函數(shù)，每當(dāng)一個(gè)信息到來的時(shí)候，這個(gè)函數(shù)會(huì)被調(diào)用 * 返回一個(gè)ros::Subscriber類的對象，當(dāng)此對象的所有引用都被銷毀是，本節(jié)點(diǎn)將不再是該話題的訂閱者 */ // 訂閱了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() 函數(shù)指定我們?nèi)绾卧诮o定的topic上發(fā)布信息* 它會(huì)觸發(fā)對ROS master的調(diào)用，master會(huì)記錄話題發(fā)布者和訂閱者* 在advertise()函數(shù)執(zhí)行之后，master會(huì)通知每一個(gè)訂閱此話題的節(jié)點(diǎn)* 兩節(jié)點(diǎn)間由此可以建立直接的聯(lián)系* advertise()會(huì)返回一個(gè)Publisher對象，使用這個(gè)對象的publish方法我們就可以在此話題上發(fā)布信息* 當(dāng)返回的Publisher對象的所有引用都被銷毀的時(shí)候，本節(jié)點(diǎn)將不再是該話題的發(fā)布者* 此函數(shù)是一個(gè)帶模板的函數(shù)，需要傳入具體的類型進(jìn)行實(shí)例化* 傳入的類型就是要發(fā)布的信息的類型，在這里是String* 第一個(gè)參數(shù)是話題名稱* 第二個(gè)參數(shù)是信息隊(duì)列的長度，相當(dāng)于信息的一個(gè)緩沖區(qū)* 在我們發(fā)布信息的速度大于處理信息的速度時(shí)* 信息會(huì)被緩存在先進(jìn)先出的信息隊(duì)列里*/// 發(fā)布了6個(gè)話題：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);/*** 它可以保證你指定的回調(diào)函數(shù)會(huì)被調(diào)用* 程序執(zhí)行到spin()后就不調(diào)用其他語句了*/ros::spin();return 0; }

主函數(shù)比較簡單，訂閱了2個(gè)節(jié)點(diǎn)和發(fā)布了6個(gè)節(jié)點(diǎn)。通過回調(diào)函數(shù)的處理，將處理后的點(diǎn)云重新發(fā)出去。主要涉及的函數(shù)為

laserCloudHandler與imuHandler。

·laserHandler

laserCloudHandler是這一模塊的重點(diǎn)部分，主要功能是對接收到的點(diǎn)云進(jìn)行預(yù)處理，完成分類。具體分類內(nèi)容為：一是將點(diǎn)云劃入不同線中存儲(chǔ)；二是對其進(jìn)行特征分類。

首先對收到的點(diǎn)云進(jìn)行處理

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) {if (!systemInited) {systemInitCount++;if (systemInitCount >= systemDelay) {// systemDelay 有延時(shí)作用，保證有imu數(shù)據(jù)后在調(diào)用laserCloudHandlersystemInited = true;}return;}std::vector<int> scanStartInd(N_SCANS, 0);std::vector<int> scanEndInd(N_SCANS, 0);// Lidar的時(shí)間戳double timeScanCur = laserCloudMsg->header.stamp.toSec();pcl::PointCloud<pcl::PointXYZ> laserCloudIn;// fromROSmsg(input,cloud1) 轉(zhuǎn)為為模板點(diǎn)云laserCloudInpcl::fromROSMsg(*laserCloudMsg, laserCloudIn);std::vector<int> indices;//去除無效值pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);int cloudSize = laserCloudIn.points.size();//計(jì)算點(diǎn)云的起始角度/終止角度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;

接下來的處理是根據(jù)角度將點(diǎn)劃入不同數(shù)組中

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，由小到大劃入偶數(shù)線0-16；角度小于0，由大到小劃入奇數(shù)線15-1scanID = roundedAngle + (N_SCANS - 1);}if (scanID > (N_SCANS - 1) || scanID < 0 ){// 不在16線附近的點(diǎn)作為雜點(diǎn)進(jìn)行剔除count--;continue;}

接下來計(jì)算每個(gè)點(diǎn)的相對方位角計(jì)算出相對時(shí)間，根據(jù)線性插值的方法計(jì)算速度及角度，并轉(zhuǎn)換到sweep k的初始imu坐標(biāo)系下，再劃入16線數(shù)組中

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，是因?yàn)閘idar工作周期是10HZ，意味著轉(zhuǎn)一圈是0.1秒；intensity是一個(gè)整數(shù)+小數(shù)，小數(shù)不會(huì)超過0.1，完成了按照時(shí)間排序的需求float relTime = (ori - startOri) / (endOri - startOri);point.intensity = scanID + scanPeriod * relTime;// imuPointerLast 是當(dāng)前點(diǎn)，變量只在imu中改變，設(shè)為t時(shí)刻// 對每一個(gè)cloud point處理if (imuPointerLast >= 0) {float pointTime = relTime * scanPeriod;while (imuPointerFront != imuPointerLast) {// (timeScanCur + pointTime)設(shè)為ti時(shí)刻；imuPointerFront 是 ti后一個(gè)時(shí)刻if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {// 這個(gè)的意思是 imuPointerFront=imuPointerLast時(shí)候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 線性插值求解出當(dāng)前點(diǎn)對應(yīng)的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位移轉(zhuǎn)到IMU起始坐標(biāo)系下ShiftToStartIMU(pointTime);// 將Lidar運(yùn)動(dòng)速度轉(zhuǎn)到IMU起始坐標(biāo)系下VeloToStartIMU();// 將點(diǎn)坐標(biāo)轉(zhuǎn)到起始IMU坐標(biāo)系下TransformToStartIMU(&point);}}// 將點(diǎn)按照每一層線，分類壓入16個(gè)數(shù)組中l(wèi)aserCloudScans[scanID].push_back(point);}

之后將對所有點(diǎn)進(jìn)行曲率值的計(jì)算并記錄每一層曲率數(shù)組的起始和終止

cloudSize = count;pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());//將所有點(diǎn)存入laserCloud中，點(diǎn)按線序進(jìn)行排列for (int i = 0; i < N_SCANS; i++) {*laserCloud += laserCloudScans[i];}int scanCount = -1;for (int i = 5; i < cloudSize - 5; i++) {// 對所有的激光點(diǎn)一個(gè)一個(gè)求出在該點(diǎn)前后5個(gè)點(diǎn)(10點(diǎn))的偏差，作為cloudCurvature點(diǎn)云數(shù)據(jù)的曲率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// 記錄每一層起始點(diǎn)和終止點(diǎn)的位置，需要根據(jù)這個(gè)起始/終止來操作點(diǎn)云曲率，在求曲率的過程中已經(jīng)去除了前5個(gè)點(diǎn)和后5個(gè)點(diǎn)if (scanCount > 0 && scanCount < N_SCANS) {scanStartInd[scanCount] = i + 5;scanEndInd[scanCount - 1] = i - 5;}}}

接下來，對提到的兩種瑕點(diǎn)進(jìn)行排除

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時(shí)（夾角小就可能存在遮擋），將其一側(cè)的臨近6個(gè)點(diǎn)設(shè)為不可標(biāo)記為特征點(diǎn)的點(diǎn) —*//*— 構(gòu)建了一個(gè)等腰三角形的底向量，根據(jù)等腰三角形性質(zhì)，判斷X[i]向量與X[i+1]的夾角小于5.732度(threshold=0.1) —*//*— depth1>depth2 X[i+1]距離更近，遠(yuǎn)側(cè)點(diǎn)標(biāo)記不特征；depth1<depth2 X[i]距離更近，遠(yuǎn)側(cè)點(diǎn)標(biāo)記不特征 —*/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)情況，當(dāng)某點(diǎn)及其后點(diǎn)間的距離平方大于某閾值a（說明這兩點(diǎn)有一定距離） ———*//*— 若某點(diǎn)到其前后兩點(diǎn)的距離均大于c倍的該點(diǎn)深度，則該點(diǎn)判定為不可標(biāo)記特征點(diǎn)的點(diǎn) ———————*//*—（入射角越小，點(diǎn)間距越大，即激光發(fā)射方向與投射到的平面越近似水平） ———————————————*/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;}}

之后對平面點(diǎn)以及角點(diǎn)進(jìn)行篩選

pcl::PointCloud<PointType> cornerPointsSharp;pcl::PointCloud<PointType> cornerPointsLessSharp;pcl::PointCloud<PointType> surfPointsFlat;pcl::PointCloud<PointType> surfPointsLessFlat;for (int i = 0; i < N_SCANS; i++) {/*—— 對于每一層激光點(diǎn)(總16層)，將每層區(qū)域分成6份，起始位置為sp，終止位置為ep。——————*//*—— 有兩個(gè)循環(huán)，作用是對cloudCurvature從小到大進(jìn)行排序，cloudSortedInd是它的索引數(shù)組 ————*/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;}}}/*—— 篩選特征角點(diǎn) 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;}// 遍歷該曲率點(diǎn)后，將該點(diǎn)標(biāo)記，并將該曲率點(diǎn)附近的前后5個(gè)點(diǎn)標(biāo)記不被選取為特征點(diǎn)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;}}}/*—— 篩選特征平面點(diǎn) Flat: label=-1 普通點(diǎn)和Flat點(diǎn)降采樣形成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點(diǎn)，采樣前包含太多l(xiāng)abel=0的點(diǎn)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;}

之后再將信息發(fā)布出去

·imuHandler

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

?

減去重力對imu的影響?

解析出當(dāng)前時(shí)刻的imu時(shí)間戳，角度以及各個(gè)軸的加速度?

將加速度轉(zhuǎn)換到世界坐標(biāo)系軸下?

進(jìn)行航距推算，假定為勻速運(yùn)動(dòng)推算出當(dāng)前時(shí)刻的位置)?

?推算當(dāng)前時(shí)刻的速度信息

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;// 表明數(shù)組是一個(gè)長度為imuQueLength的循環(huán)數(shù)組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];// 轉(zhuǎn)換到世界坐標(biāo)系下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， 這樣處理是為了防止內(nèi)存溢出int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];if (timeDiff < scanPeriod) {// 推算當(dāng)前時(shí)刻的位置信息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;// 推算當(dāng)前時(shí)刻的速度信息imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;} }

后面涉及的坐標(biāo)變換即歐拉角的旋轉(zhuǎn)計(jì)算，這部分尚不熟悉，需要進(jìn)一步學(xué)習(xí)。

·小結(jié)

?

?

以上數(shù)據(jù)傳輸流幫助理解這個(gè)模塊源代碼的功能。

?

——————————————————————————————————

感謝@清酒不是九提出的問題與解決方案

增加以下內(nèi)容：loam在運(yùn)行較大的圖時(shí)，修改decay time可以讓之前的點(diǎn)云不被刷新截掉。地圖稠密。

創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎(jiǎng)勵(lì)來咯，堅(jiān)持創(chuàng)作打卡瓜分現(xiàn)金大獎(jiǎng)

總結(jié)

以上是生活随笔為你收集整理的LOAM_velodyne学习（一）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。