手撕LOAM源代碼(一)--scanRegistration.cpp
作者：K.Fire ?| 來源：3D視覺工坊
添加微信：dddvisiona，備注：SLAM，拉你入群。文末附行業(yè)細(xì)分群。
寫在前面
本系列文章將對LOAM源代碼進(jìn)行講解，在講解過程中，涉及到論文中提到的部分，會(huì)結(jié)合論文以及我自己的理解進(jìn)行解讀，尤其是對于其中坐標(biāo)變換的部分，將會(huì)進(jìn)行詳細(xì)的講解。
本來是懶得寫的，一個(gè)是怕自己以后忘了，另外是我在學(xué)習(xí)過程中，其實(shí)沒有感覺哪一個(gè)博主能講解的通篇都能讓我很明白，特別是坐標(biāo)變換部分的代碼，所以想著自己學(xué)完之后，按照自己的理解，也寫一個(gè)LOAM解讀，希望能對后續(xù)學(xué)習(xí)LOAM的同學(xué)們有所幫助。
整體框架
LOAM多牛逼就不用多說了，直接開始。
推薦學(xué)習(xí)3D視覺工坊推出的課程（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化
先貼一下我詳細(xì)注釋的LOAM代碼，在這個(gè)版本的代碼上加入了我自己的理解。
我覺得最重要也是最惡心的一部分是其中的坐標(biāo)變換，在代碼里面真的看著頭大，所以先明確一下坐標(biāo)系(都是右手坐標(biāo)系)：
IMU(IMU坐標(biāo)系imu)：x軸向前，y軸向左，z軸向上
LIDAR(激光雷達(dá)坐標(biāo)系l)：x軸向前，y軸向左，z軸向上
CAMERA(相機(jī)坐標(biāo)系，也可以理解為里程計(jì)坐標(biāo)系c)：z軸向前，x軸向左，y軸向上
WORLD(世界坐標(biāo)系w，也叫全局坐標(biāo)系，與里程計(jì)第一幀init重合)：z軸向前，x軸向左，y軸向上
MAP(地圖坐標(biāo)系map，一定程度上可以理解為里程計(jì)第一幀init)：z軸向前，x軸向左，y軸向上
坐標(biāo)變換約定： 為了清晰，變換矩陣的形式與《SLAM十四講中一樣》，即：表示B坐標(biāo)系相對于A坐標(biāo)系的變換，B中一個(gè)向量通過可以變換到A中的向量。
首先對照ros的節(jié)點(diǎn)圖和論文中提到的算法框架來看一下：
可以看到節(jié)點(diǎn)圖和論文中的框架是一一對應(yīng)的，這幾個(gè)模塊的功能如下：
scanRegistration：對原始點(diǎn)云進(jìn)行預(yù)處理，計(jì)算曲率，提取特征點(diǎn)
laserOdometry：對當(dāng)前sweep與上一次sweep進(jìn)行特征匹配，計(jì)算一個(gè)快速(10Hz)但粗略的位姿估計(jì)
laserMapping：對當(dāng)前sweep與一個(gè)局部子圖進(jìn)行特征匹配，計(jì)算一個(gè)慢速(1Hz)比較精確的位姿估計(jì)
transformMaintenance：對兩個(gè)模塊計(jì)算出的位姿進(jìn)行融合，得到最終的精確地位姿估計(jì)
本文首先介紹scanRegistration模塊，它的具體功能如下：
將輸入的無序點(diǎn)云轉(zhuǎn)化為按照線號存儲(chǔ)的有序點(diǎn)云
接收IMU數(shù)據(jù)(如果有的話)，并基于勻速運(yùn)動(dòng)假設(shè)，使用IMU數(shù)據(jù)進(jìn)行插值
計(jì)算每個(gè)點(diǎn)云的曲率
剔除論文中提到的不合要求的點(diǎn)
提取edge point和planar point
一、main函數(shù)
main函數(shù)很簡單，主要是創(chuàng)建了一系列的發(fā)布者和訂閱者，，然后ros::spin()進(jìn)行無限循環(huán)，其中的主要程序都在回調(diào)函數(shù)中進(jìn)行：
imuHandler：接收IMU數(shù)據(jù)進(jìn)行處理
laserCloudHandler：接收激光雷達(dá)數(shù)據(jù)進(jìn)行處理，這個(gè)是該程序的主要函數(shù)，包含了算法的核心部分。
int?main(int?argc,?char**?argv){??ros::init(argc,?argv,?"scanRegistration");??ros::NodeHandle?nh;??ros::Subscriber?subLaserCloud?=?nh.subscribe<sensor_msgs::PointCloud2>???????????????????????????????????("/velodyne_points",?2,?laserCloudHandler);??ros::Subscriber?subImu?=?nh.subscribe<sensor_msgs::Imu>?("/imu/data",?50,?imuHandler);??pubLaserCloud?=?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);??ros::spin();??return?0;}二、接收IMU的消息-imuHandler()
這一部分比較難理解的部分是去除重力影響，主要對這部分進(jìn)行解釋。
IMU坐標(biāo)系為x軸向前，y軸向左，z軸向上的右手坐標(biāo)系。推薦學(xué)習(xí)3D視覺工坊推出的課程（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化
首先，接收ROS的IMU話題，分解出PRY角，IMU系相對于全局坐標(biāo)系的變換是XYZ的變換順序，即，旋轉(zhuǎn)矩陣，重力為全局坐標(biāo)系中一個(gè)向量，需要先變換到IMU坐標(biāo)系，有如下關(guān)系：
計(jì)算出來的結(jié)果為：
而我們測量出來的加速度值是收到重力影響的，可以表述為：
所以加速度真值為：
最后進(jìn)行坐標(biāo)系交換，變換==z軸向前，x軸向左，y軸向上==，就有了代碼中的樣子。
注意這里沒有進(jìn)行坐標(biāo)變換，只是換了一下坐標(biāo)軸的位置，是為了后面計(jì)算全局坐標(biāo)系下的速度和位移積分。
//接收imu消息，imu坐標(biāo)系為x軸向前，y軸向左，z軸向上的右手坐標(biāo)系void?imuHandler(const?sensor_msgs::Imu::ConstPtr&?imuIn){??double?roll,?pitch,?yaw;??tf::Quaternion?orientation;??//convert?Quaternion?msg?to?Quaternion??tf::quaternionMsgToTF(imuIn->orientation,?orientation);??//This?will?get?the?roll?pitch?and?yaw?from?the?matrix?about?fixed?axes?X,?Y,?Z?respectively.?That's?R?=?Rz(yaw)*Ry(pitch)*Rx(roll).??//Here?roll?pitch?yaw?is?in?the?global(init)?frame??tf::Matrix3x3(orientation).getRPY(roll,?pitch,?yaw);??//減去重力的影響,求出xyz方向的加速度實(shí)際值，并進(jìn)行坐標(biāo)軸交換，統(tǒng)一到z軸向前,x軸向左的右手坐標(biāo)系,?交換過后RPY對應(yīng)fixed?axes?ZXY(RPY---ZXY)。Now?R?=?Ry(yaw)*Rx(pitch)*Rz(roll).??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;??//循環(huán)移位效果，形成環(huán)形數(shù)組??imuPointerLast?=?(imuPointerLast?+?1)?%?imuQueLength;?????//?const?int?imuQueLength?=?200??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();}三、積分IMU的速度與位移-AccumulateIMUShift()
首先明確一點(diǎn)：在進(jìn)行了坐標(biāo)系轉(zhuǎn)換(轉(zhuǎn)換成了==z軸向前，x軸向左，y軸向上==)后，原來的當(dāng)前幀(局部坐標(biāo)系)到世界坐標(biāo)系(全局坐標(biāo)系)的變換變成了。
現(xiàn)在的加速度還是在局部坐標(biāo)系(imu)中，現(xiàn)在需要將其變換到世界坐標(biāo)系，然后與之前的速度、位移進(jìn)行積分。
下面就是根據(jù)初中學(xué)的公式進(jìn)行積分：
//積分速度與位移void?AccumulateIMUShift(){??float?roll?=?imuRoll[imuPointerLast];??float?pitch?=?imuPitch[imuPointerLast];??float?yaw?=?imuYaw[imuPointerLast];??//?accX、accY、acc都是世界坐標(biāo)系下??float?accX?=?imuAccX[imuPointerLast];??float?accY?=?imuAccY[imuPointerLast];??float?accZ?=?imuAccZ[imuPointerLast];??//將當(dāng)前時(shí)刻的加速度值繞交換過的ZXY固定軸（原XYZ）分別旋轉(zhuǎn)(roll,?pitch,?yaw)角，轉(zhuǎn)換得到世界坐標(biāo)系下的加速度值(right?hand?rule)??//繞z軸旋轉(zhuǎn)(roll)??float?x1?=?cos(roll)?*?accX?-?sin(roll)?*?accY;??float?y1?=?sin(roll)?*?accX?+?cos(roll)?*?accY;??float?z1?=?accZ;??//繞x軸旋轉(zhuǎn)(pitch)??float?x2?=?x1;??float?y2?=?cos(pitch)?*?y1?-?sin(pitch)?*?z1;??float?z2?=?sin(pitch)?*?y1?+?cos(pitch)?*?z1;??//繞y軸旋轉(zhuǎn)(yaw)??accX?=?cos(yaw)?*?x2?+?sin(yaw)?*?z2;??accY?=?y2;??accZ?=?-sin(yaw)?*?x2?+?cos(yaw)?*?z2;??//上一個(gè)imu點(diǎn)??int?imuPointerBack?=?(imuPointerLast?+?imuQueLength?-?1)?%?imuQueLength;??//上一個(gè)點(diǎn)到當(dāng)前點(diǎn)所經(jīng)歷的時(shí)間，即計(jì)算imu測量周期??double?timeDiff?=?imuTime[imuPointerLast]?-?imuTime[imuPointerBack];??//要求imu的頻率至少比lidar高，這樣的imu信息才使用，后面校正也才有意義??if?(timeDiff?<?scanPeriod)?{//（隱含從靜止開始運(yùn)動(dòng)）????//求每個(gè)imu時(shí)間點(diǎn)的位移與速度,兩點(diǎn)之間視為勻加速直線運(yùn)動(dòng)????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;??}}四、接收點(diǎn)云數(shù)據(jù)-laserCloudHandler()
首先接收到當(dāng)前sweep的點(diǎn)云數(shù)據(jù)后，先計(jì)算一下點(diǎn)云的起始角度startOri和終止角度endOri，對應(yīng)于下圖的α角，然后將結(jié)束角與起始角差值控制在(PI,3*PI)范圍，允許lidar不是一個(gè)圓周掃描。
推薦學(xué)習(xí)3D視覺工坊推出的課程（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化
下面這個(gè)for循環(huán)做了這些事情：
將點(diǎn)云從雷達(dá)坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系(是坐標(biāo)系轉(zhuǎn)換，不是坐標(biāo)變換)
計(jì)算每個(gè)點(diǎn)的俯仰角，對應(yīng)于上圖的ω角，用于計(jì)算scanID
如果收到IMU數(shù)據(jù)，使用IMU進(jìn)行插值
計(jì)算了每個(gè)點(diǎn)由于加減速產(chǎn)生的位移和速度畸變
將每個(gè)點(diǎn)變換到當(dāng)前sweep的初始時(shí)刻里程計(jì)的start時(shí)刻坐標(biāo)系
==要進(jìn)行說明的部分如下：==
點(diǎn)云強(qiáng)度保存：
point.intensity?=?scanID?+?scanPeriod?*?relTime;
這里點(diǎn)云強(qiáng)度值保存的格式為“線號 + 掃描周期(10Hz=0.1s) * 點(diǎn)云相對角度”，這樣保存的好處就是：對強(qiáng)度值取整，可以得到線號；強(qiáng)度值-取整，可以計(jì)算出相對角度。
推薦學(xué)習(xí)3D視覺工坊推出的課程（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化
IMU去畸變：
?if?(imuPointerLast?>=?0)?{//如果收到IMU數(shù)據(jù),使用IMU進(jìn)行插值??????float?pointTime?=?relTime?*?scanPeriod;//計(jì)算點(diǎn)的周期時(shí)間??????//尋找是否有點(diǎn)云的時(shí)間戳小于IMU的時(shí)間戳的IMU位置:imuPointerFront??????while?(imuPointerFront?!=?imuPointerLast)?{????????if?(timeScanCur?+?pointTime?<?imuTime[imuPointerFront])?{??????????break;????????}????????imuPointerFront?=?(imuPointerFront?+?1)?%?imuQueLength;??????}......這里的imuPointerLast表示最新收到IMU數(shù)據(jù)的位置，imuPointerFront表示時(shí)間戳剛好大于當(dāng)前點(diǎn)云時(shí)間戳的位置，在對點(diǎn)云進(jìn)行插值時(shí)，需要使用imuPointerFront和它之前一個(gè)位置imuPointerBack。
當(dāng)找到了一個(gè)滿足要求的imuPointerFront，就是用下式進(jìn)行插值：
文章中的式子如下：本質(zhì)上是一樣的：
第一個(gè)點(diǎn)云的數(shù)值都保存在以Start結(jié)尾的變量中。
ShiftToStartIMU(pointTime)函數(shù)：
這個(gè)函數(shù)用來計(jì)算相對于當(dāng)前sweep初始時(shí)刻 由于加減速產(chǎn)生的位移畸變，注意這里的變量都是在全局坐標(biāo)系下計(jì)算的 當(dāng)前時(shí)刻相對于該sweep初始時(shí)刻的畸變量。
推薦學(xué)習(xí)3D視覺工坊推出的課程（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化
然后將畸變量由全局坐標(biāo)系w變換到當(dāng)前sweep的初始時(shí)刻坐標(biāo)系(start)，而現(xiàn)在我們已知的量RPY角所構(gòu)成的變換為：
所以這里需要的變換為：
這樣得到了如下代碼。
//計(jì)算局部(start)坐標(biāo)系下點(diǎn)云中的點(diǎn)相對第一個(gè)開始點(diǎn)的由于加減速運(yùn)動(dòng)產(chǎn)生的位移畸變void?ShiftToStartIMU(float?pointTime){??//計(jì)算相對于第一個(gè)點(diǎn)由于加減速產(chǎn)生的畸變位移(全局(w)坐標(biāo)系下畸變位移量delta_Tg)??//imuShiftFromStartCur?=?imuShiftCur?-?(imuShiftStart?+?imuVeloStart?*?pointTime)??imuShiftFromStartXCur?=?imuShiftXCur?-?imuShiftXStart?-?imuVeloXStart?*?pointTime;??imuShiftFromStartYCur?=?imuShiftYCur?-?imuShiftYStart?-?imuVeloYStart?*?pointTime;??imuShiftFromStartZCur?=?imuShiftZCur?-?imuShiftZStart?-?imuVeloZStart?*?pointTime;??/********************************************************************************??delta_Tstart?=?Rz(roll).inverse?*?Rx(pitch).inverse?*?Ry(yaw).inverse?*?delta_Tg??transfrom?from?the?global(w)?frame?to?the?local(start)?frame??*********************************************************************************/??//繞y軸旋轉(zhuǎn)(-imuYawStart)，即Ry(yaw).inverse??float?x1?=?cos(imuYawStart)?*?imuShiftFromStartXCur?-?sin(imuYawStart)?*?imuShiftFromStartZCur;??float?y1?=?imuShiftFromStartYCur;??float?z1?=?sin(imuYawStart)?*?imuShiftFromStartXCur?+?cos(imuYawStart)?*?imuShiftFromStartZCur;??//繞x軸旋轉(zhuǎn)(-imuPitchStart)，即Rx(pitch).inverse??float?x2?=?x1;??float?y2?=?cos(imuPitchStart)?*?y1?+?sin(imuPitchStart)?*?z1;??float?z2?=?-sin(imuPitchStart)?*?y1?+?cos(imuPitchStart)?*?z1;??//繞z軸旋轉(zhuǎn)(-imuRollStart)，即Rz(pitch).inverse??imuShiftFromStartXCur?=?cos(imuRollStart)?*?x2?+?sin(imuRollStart)?*?y2;??imuShiftFromStartYCur?=?-sin(imuRollStart)?*?x2?+?cos(imuRollStart)?*?y2;??imuShiftFromStartZCur?=?z2;}VeloToStartIMU()函數(shù)：這個(gè)函數(shù)與上面函數(shù)的功能一致，是將計(jì)算由于加減速產(chǎn)生的速度畸變，并變換到start坐標(biāo)系中，不再贅述。
//計(jì)算局部(start)坐標(biāo)系下點(diǎn)云中的點(diǎn)相對第一個(gè)開始點(diǎn)由于加減速產(chǎn)生的的速度畸變（增量）void?VeloToStartIMU(){??//計(jì)算相對于第一個(gè)點(diǎn)由于加減速產(chǎn)生的畸變速度(全局(w)坐標(biāo)系下畸變速度增量delta_Vg)??imuVeloFromStartXCur?=?imuVeloXCur?-?imuVeloXStart;??imuVeloFromStartYCur?=?imuVeloYCur?-?imuVeloYStart;??imuVeloFromStartZCur?=?imuVeloZCur?-?imuVeloZStart;??/********************************************************************************????delta_Vstart?=?Rz(pitch).inverse?*?Rx(pitch).inverse?*?Ry(yaw).inverse?*?delta_Vg????transfrom?from?the?global(w)?frame?to?the?local(start)?frame??*********************************************************************************/????//繞y軸旋轉(zhuǎn)(-imuYawStart)，即Ry(yaw).inverse??float?x1?=?cos(imuYawStart)?*?imuVeloFromStartXCur?-?sin(imuYawStart)?*?imuVeloFromStartZCur;??float?y1?=?imuVeloFromStartYCur;??float?z1?=?sin(imuYawStart)?*?imuVeloFromStartXCur?+?cos(imuYawStart)?*?imuVeloFromStartZCur;??//繞x軸旋轉(zhuǎn)(-imuPitchStart)，即Rx(pitch).inverse??float?x2?=?x1;??float?y2?=?cos(imuPitchStart)?*?y1?+?sin(imuPitchStart)?*?z1;??float?z2?=?-sin(imuPitchStart)?*?y1?+?cos(imuPitchStart)?*?z1;??//繞z軸旋轉(zhuǎn)(-imuRollStart)，即Rz(pitch).inverse??imuVeloFromStartXCur?=?cos(imuRollStart)?*?x2?+?sin(imuRollStart)?*?y2;??imuVeloFromStartYCur?=?-sin(imuRollStart)?*?x2?+?cos(imuRollStart)?*?y2;??imuVeloFromStartZCur?=?z2;}TransformToStartIMU(&point)函數(shù)：這個(gè)函數(shù)將當(dāng)前sweep中的所有點(diǎn)云去除由于加減速產(chǎn)生的運(yùn)動(dòng)畸變，然后都變換到里程計(jì)坐標(biāo)系w下的初始時(shí)刻。
我們現(xiàn)在已知當(dāng)前時(shí)刻的PRY角，那么可以構(gòu)成當(dāng)前時(shí)刻坐標(biāo)系(curr坐標(biāo)系)相對于世界坐標(biāo)系(w)的變換：
同樣，已知了該sweep初始時(shí)刻的PRY角，可以構(gòu)成世界坐標(biāo)系w到該sweep的坐標(biāo)變換，和上面畸變量變換類似：
表示curr坐標(biāo)系下對應(yīng)于imustart姿態(tài)靜止掃描得到的點(diǎn)，那么最終的變換為：
最后再加上上面計(jì)算出的由于加減速產(chǎn)生的位移畸變量，就得到了如下代碼。
這個(gè)過程完成的工作應(yīng)該如下圖所示：
得到了一個(gè)去畸變后的點(diǎn)云結(jié)果，效果相當(dāng)于：得到了，在點(diǎn)云掃描開始位置靜止掃描，得到的點(diǎn)云位置，也就是說沒有對點(diǎn)云坐標(biāo)系進(jìn)行變換，第i個(gè)點(diǎn)云依然處在里程計(jì)坐標(biāo)系下的curr時(shí)刻坐標(biāo)系中，這一步的操作只是對點(diǎn)云的位置進(jìn)行調(diào)整，然后這一幀點(diǎn)云掃描過程中，按照這個(gè)調(diào)整后位置送入傳感器中。
還有一種解釋是說：這個(gè)函數(shù)操作完了之后，current時(shí)刻的坐標(biāo)系變成了：仍然以current時(shí)刻為原點(diǎn)，坐標(biāo)軸的姿態(tài)與初始時(shí)刻start的姿態(tài)相同，我覺得這個(gè)理解也可以。
//將當(dāng)前時(shí)刻curr坐標(biāo)系下的的點(diǎn)云變換到世界坐標(biāo)系w，然后在變換到當(dāng)前幀的初始時(shí)刻start坐標(biāo)系下void?TransformToStartIMU(PointType?*p){??/********************************************************************************????Pinit?=?Ry?*?Rx?*?Rz?*?Pcurr????transform?current?camara(curr)?frame?point?to?the?global(w)?frame??*********************************************************************************/??//繞z軸旋轉(zhuǎn)(imuRollCur)??float?x1?=?cos(imuRollCur)?*?p->x?-?sin(imuRollCur)?*?p->y;??float?y1?=?sin(imuRollCur)?*?p->x?+?cos(imuRollCur)?*?p->y;??float?z1?=?p->z;??//繞x軸旋轉(zhuǎn)(imuPitchCur)??float?x2?=?x1;??float?y2?=?cos(imuPitchCur)?*?y1?-?sin(imuPitchCur)?*?z1;??float?z2?=?sin(imuPitchCur)?*?y1?+?cos(imuPitchCur)?*?z1;??//繞y軸旋轉(zhuǎn)(imuYawCur)??float?x3?=?cos(imuYawCur)?*?x2?+?sin(imuYawCur)?*?z2;??float?y3?=?y2;??float?z3?=?-sin(imuYawCur)?*?x2?+?cos(imuYawCur)?*?z2;??/********************************************************************************????Pstart?=?Rz(pitch).inverse?*?Rx(pitch).inverse?*?Ry(yaw).inverse?*?Pinit????transfrom?global(w)?points?to?the?local(start)?frame??*********************************************************************************/????//繞y軸旋轉(zhuǎn)(-imuYawStart)??float?x4?=?cos(imuYawStart)?*?x3?-?sin(imuYawStart)?*?z3;??float?y4?=?y3;??float?z4?=?sin(imuYawStart)?*?x3?+?cos(imuYawStart)?*?z3;??//繞x軸旋轉(zhuǎn)(-imuPitchStart)??float?x5?=?x4;??float?y5?=?cos(imuPitchStart)?*?y4?+?sin(imuPitchStart)?*?z4;??float?z5?=?-sin(imuPitchStart)?*?y4?+?cos(imuPitchStart)?*?z4;??//繞z軸旋轉(zhuǎn)(-imuRollStart)，然后疊加平移量??p->x?=?cos(imuRollStart)?*?x5?+?sin(imuRollStart)?*?y5?+?imuShiftFromStartXCur;??p->y?=?-sin(imuRollStart)?*?x5?+?cos(imuRollStart)?*?y5?+?imuShiftFromStartYCur;??p->z?=?z5?+?imuShiftFromStartZCur;}當(dāng)上面這些操縱都做完之后，將得到的start坐標(biāo)系下去畸變的點(diǎn)，放入按scanID存儲(chǔ)的點(diǎn)云容器laserCloudScans，所有代碼如下：
//接收點(diǎn)云數(shù)據(jù)，velodyne雷達(dá)坐標(biāo)系安裝為x軸向前，y軸向左，z軸向上的右手坐標(biāo)系void?laserCloudHandler(const?sensor_msgs::PointCloud2ConstPtr&?laserCloudMsg){??if?(!systemInited)?{//丟棄前20個(gè)點(diǎn)云數(shù)據(jù)????systemInitCount++;????if?(systemInitCount?>=?systemDelay)?{??????systemInited?=?true;????}????return;??}??//記錄每個(gè)scan有曲率的點(diǎn)的開始和結(jié)束索引??std::vector<int>?scanStartInd(N_SCANS,?0);??std::vector<int>?scanEndInd(N_SCANS,?0);????//當(dāng)前sweep的時(shí)間??double?timeScanCur?=?laserCloudMsg->header.stamp.toSec();??pcl::PointCloud<pcl::PointXYZ>?laserCloudIn;??//消息轉(zhuǎn)換成pcl數(shù)據(jù)存放??pcl::fromROSMsg(*laserCloudMsg,?laserCloudIn);??std::vector<int>?indices;??//移除空點(diǎn)??pcl::removeNaNFromPointCloud(laserCloudIn,?laserCloudIn,?indices);??//點(diǎn)云點(diǎn)的數(shù)量??int?cloudSize?=?laserCloudIn.points.size();??//lidar?scan開始點(diǎn)的旋轉(zhuǎn)角,atan2范圍[-pi,+pi],計(jì)算旋轉(zhuǎn)角時(shí)取負(fù)號是因?yàn)関elodyne是順時(shí)針旋轉(zhuǎn)??float?startOri?=?-atan2(laserCloudIn.points[0].y,?laserCloudIn.points[0].x);??//lidar?scan結(jié)束點(diǎn)的旋轉(zhuǎn)角，加2*pi使點(diǎn)云旋轉(zhuǎn)周期為2*pi??float?endOri?=?-atan2(laserCloudIn.points[cloudSize?-?1].y,????????????????????????laserCloudIn.points[cloudSize?-?1].x)?+?2?*?M_PI;??//結(jié)束方位角與開始方位角差值控制在(PI,3*PI)范圍，允許lidar不是一個(gè)圓周掃描??//正常情況下在這個(gè)范圍內(nèi)：pi?<?endOri?-?startOri?<?3*pi，異常則修正??if?(endOri?-?startOri?>?3?*?M_PI)?{????endOri?-=?2?*?M_PI;??}?else?if?(endOri?-?startOri?<?M_PI)?{????endOri?+=?2?*?M_PI;??}??//lidar掃描線是否旋轉(zhuǎn)過半??bool?halfPassed?=?false;??int?count?=?cloudSize;??PointType?point;??std::vector<pcl::PointCloud<PointType>?>?laserCloudScans(N_SCANS);????/*?這個(gè)for循環(huán)做了這些事情：???*????1.將點(diǎn)云從雷達(dá)坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系???*????2.計(jì)算每個(gè)點(diǎn)的俯仰角，用于計(jì)算scanID???*????3.如果收到IMU數(shù)據(jù),使用IMU進(jìn)行插值???*????4.計(jì)算了每個(gè)點(diǎn)由于加減速產(chǎn)生的位移和速度畸變???*????5.將每個(gè)點(diǎn)變換到當(dāng)前sweep的初始時(shí)刻start坐標(biāo)系??*/??for?(int?i?=?0;?i?<?cloudSize;?i++)?{????//把雷達(dá)坐標(biāo)系(前-左-上)中的點(diǎn)云轉(zhuǎn)換到里程計(jì)標(biāo)系(左上前)?X->Z?Y->X?Z->Y????point.x?=?laserCloudIn.points[i].y;????point.y?=?laserCloudIn.points[i].z;????point.z?=?laserCloudIn.points[i].x;????//計(jì)算點(diǎn)的仰角(根據(jù)lidar文檔垂直角計(jì)算公式),根據(jù)仰角排列激光線號，velodyne每兩個(gè)scan之間間隔2度????float?angle?=?atan(point.y?/?sqrt(point.x?*?point.x?+?point.z?*?point.z))?*?180?/?M_PI;????int?scanID;????//仰角四舍五入(加減0.5截?cái)嘈Ч扔谒纳嵛迦?????int?roundedAngle?=?int(angle?+?(angle<0.0?-0.5:+0.5));?????if?(roundedAngle?>?0){??????scanID?=?roundedAngle;????}????else?{??????scanID?=?roundedAngle?+?(N_SCANS?-?1);????}????//過濾點(diǎn)，只挑選[-15度，+15度]范圍內(nèi)的點(diǎn),scanID屬于[0,15]????if?(scanID?>?(N_SCANS?-?1)?||?scanID?<?0?){??????count--;??????continue;????}????//該點(diǎn)的旋轉(zhuǎn)角????float?ori?=?-atan2(point.x,?point.z);????if?(!halfPassed)?{//根據(jù)掃描線是否旋轉(zhuǎn)過半選擇與起始位置還是終止位置進(jìn)行差值計(jì)算，從而進(jìn)行補(bǔ)償????????//確保-pi/2?<?ori?-?startOri?<?3*pi/2??????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;??????//確保-3*pi/2?<?ori?-?endOri?<?pi/2??????if?(ori?<?endOri?-?M_PI?*?3?/?2)?{????????ori?+=?2?*?M_PI;??????}?else?if?(ori?>?endOri?+?M_PI?/?2)?{????????ori?-=?2?*?M_PI;??????}?????}????//-0.5?<?relTime?<?1.5（點(diǎn)旋轉(zhuǎn)的角度與整個(gè)周期旋轉(zhuǎn)角度的比率,?即點(diǎn)云中點(diǎn)的相對時(shí)間）????float?relTime?=?(ori?-?startOri)?/?(endOri?-?startOri);????//點(diǎn)強(qiáng)度=線號+點(diǎn)相對時(shí)間（即一個(gè)整數(shù)+一個(gè)小數(shù)，整數(shù)部分是線號，小數(shù)部分是該點(diǎn)的相對時(shí)間）,勻速掃描：根據(jù)當(dāng)前掃描的角度和掃描周期計(jì)算相對掃描起始位置的時(shí)間????point.intensity?=?scanID?+?scanPeriod?*?relTime;????if?(imuPointerLast?>=?0)?{//如果收到IMU數(shù)據(jù),使用IMU進(jìn)行插值??????float?pointTime?=?relTime?*?scanPeriod;//計(jì)算點(diǎn)的周期時(shí)間??????//尋找是否有點(diǎn)云的時(shí)間戳小于IMU的時(shí)間戳的IMU位置:imuPointerFront??????while?(imuPointerFront?!=?imuPointerLast)?{????????if?(timeScanCur?+?pointTime?<?imuTime[imuPointerFront])?{??????????break;????????}????????imuPointerFront?=?(imuPointerFront?+?1)?%?imuQueLength;??????}??????if?(timeScanCur?+?pointTime?>?imuTime[imuPointerFront])?{//沒找到,此時(shí)imuPointerFront==imtPointerLast,只能以當(dāng)前收到的最新的IMU的速度，位移，歐拉角作為當(dāng)前點(diǎn)的速度，位移，歐拉角使用????????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?{//找到了點(diǎn)云時(shí)間戳小于IMU時(shí)間戳的IMU位置,則該點(diǎn)必處于imuPointerBack和imuPointerFront之間，據(jù)此線性插值，計(jì)算點(diǎn)云點(diǎn)的速度，位移和歐拉角????????int?imuPointerBack?=?(imuPointerFront?+?imuQueLength?-?1)?%?imuQueLength;???//?imuPointerBack是imuPointerFront的上一個(gè)位置????????//按時(shí)間距離計(jì)算權(quán)重分配比率,也即線性插值????????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;????????}????????//本質(zhì):imuVeloXCur?=?imuVeloX[imuPointerback]?+?(imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFront????????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)?{//如果是第一個(gè)點(diǎn),記住點(diǎn)云起始位置的速度，位移，歐拉角????????imuRollStart?=?imuRollCur;????????imuPitchStart?=?imuPitchCur;????????imuYawStart?=?imuYawCur;????????imuVeloXStart?=?imuVeloXCur;????????imuVeloYStart?=?imuVeloYCur;????????imuVeloZStart?=?imuVeloZCur;????????imuShiftXStart?=?imuShiftXCur;????????imuShiftYStart?=?imuShiftYCur;????????imuShiftZStart?=?imuShiftZCur;??????}?else?{????????ShiftToStartIMU(pointTime);????????VeloToStartIMU();????????TransformToStartIMU(&point);//將當(dāng)前時(shí)刻curr坐標(biāo)系下的的點(diǎn)云變換到世界坐標(biāo)系w，然后在變換到當(dāng)前幀的初始時(shí)刻start坐標(biāo)系下??????}????}????laserCloudScans[scanID].push_back(point);//將每個(gè)補(bǔ)償矯正的點(diǎn)放入對應(yīng)線號的容器??}
這一部分對應(yīng)論文中提到的曲率計(jì)算公式：
是第i個(gè)點(diǎn)云的位置，是左右兩邊各5個(gè)點(diǎn)，注意它們兩個(gè)都是==矢量==，那么就是一個(gè)指向的向量。
如上圖所示，如果一個(gè)點(diǎn)是edge point，那么向量求和后得到結(jié)果的模很大；如果一個(gè)點(diǎn)是planar point那么與兩側(cè)五個(gè)向量求和后結(jié)果是0，通過這種方式，區(qū)分特征點(diǎn)。
求解完所有點(diǎn)的曲率后，scanStartInd[]和scanEndInd[]數(shù)組用于記錄下每個(gè)scanID有曲率的起始點(diǎn)和終止點(diǎn)的索引。
//獲得有效范圍內(nèi)的點(diǎn)的數(shù)量??cloudSize?=?count;??//這里就按照scanID變成了有序點(diǎn)云??pcl::PointCloud<PointType>::Ptr?laserCloud(new?pcl::PointCloud<PointType>());??//將所有的點(diǎn)按照線號從小到大放入一個(gè)容器??for?(int?i?=?0;?i?<?N_SCANS;?i++)?{????*laserCloud?+=?laserCloudScans[i];??}??int?scanCount?=?-1;??//使用每個(gè)點(diǎn)的前后五個(gè)點(diǎn)計(jì)算曲率，因此前五個(gè)與最后五個(gè)點(diǎn)跳過??for?(int?i?=?5;?i?<?cloudSize?-?5;?i++)?{????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->points[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;????//曲率計(jì)算????cloudCurvature[i]?=?diffX?*?diffX?+?diffY?*?diffY?+?diffZ?*?diffZ;????//記錄曲率點(diǎn)的索引????cloudSortInd[i]?=?i;????//初始時(shí)，點(diǎn)全未篩選過????cloudNeighborPicked[i]?=?0;????//初始化為less?flat點(diǎn)????cloudLabel[i]?=?0;????//每個(gè)scan，只有第一個(gè)符合的點(diǎn)會(huì)進(jìn)來，因?yàn)槊總€(gè)scan的點(diǎn)都在一起存放????if?(int(laserCloud->points[i].intensity)?!=?scanCount)?{??????scanCount?=?int(laserCloud->points[i].intensity);//控制每個(gè)scan只進(jìn)入第一個(gè)點(diǎn)??????//曲率只取同一個(gè)scan計(jì)算出來的，跨scan計(jì)算的曲率非法，排除，也即排除每個(gè)scan的前后五個(gè)點(diǎn)??????if?(scanCount?>?0?&&?scanCount?<?N_SCANS)?{????????scanStartInd[scanCount]?=?i?+?5;????????scanEndInd[scanCount?-?1]?=?i?-?5;??????}????}??}??//第一個(gè)scan曲率點(diǎn)有效點(diǎn)序從第5個(gè)開始，最后一個(gè)激光線結(jié)束點(diǎn)序size-5??scanStartInd[0]?=?5;??scanEndInd.back()?=?cloudSize?-?5;
這部分對應(yīng)于論文中提到的，計(jì)算完曲率后，最終的特征點(diǎn)需要滿足以下要求：
所選邊緣點(diǎn)或平面點(diǎn)的個(gè)數(shù)不能超過子區(qū)域的最大值-------保證取點(diǎn)均勻，這一點(diǎn)下面會(huì)講到
它周圍的點(diǎn)都沒有被選中-------保證點(diǎn)不過于密集
它不能位于與激光束大致平行的平面上，也不能位于遮擋區(qū)域的邊界上-------剔除一些不好的點(diǎn)(下面的操作)
下面代碼中的這個(gè)if用于剔除類似于圖b中A點(diǎn)這樣的易遮擋點(diǎn)
if (diff > 0.1)
當(dāng)傳感器在這個(gè)角度時(shí)，A點(diǎn)看起來是edge point，但稍微移動(dòng)時(shí)，A點(diǎn)變?yōu)閜lanar或者不可見，這種是不靠譜的，A點(diǎn)和B點(diǎn)都需要剔除。
代碼中的意思是：如果A和B距離相差0.1米以上，就求解它們兩個(gè)的深度，將深度大的點(diǎn)放縮到同一距離水平，然后用"深度距離(距離很小，近似為弧長)/深度"，這個(gè)得到的就是兩者夾角的弧度值，如果這個(gè)夾角很小，說明就是圖b中的情況，A很容易被遮擋。
推薦學(xué)習(xí)3D視覺工坊推出的課程（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化
下面代碼中的這個(gè)if用于剔除類似于圖a中B點(diǎn)這樣的所在平面與激光束近似平行的點(diǎn)
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis)
diff和diff2分別是與后一個(gè)點(diǎn)、前一個(gè)點(diǎn)距離的平方，如果出現(xiàn)圖a中B點(diǎn)這樣的情況，diff和diff2值會(huì)很大，如果大于當(dāng)前點(diǎn)深度的0.0002，則認(rèn)為出現(xiàn)圖a中的情況，需要剔除。
??//這個(gè)for循環(huán)：排除容易被斜面擋住的點(diǎn)、所在平面近似與激光束平行的點(diǎn)以及離群點(diǎn)(噪點(diǎn))??for?(int?i?=?5;?i?<?cloudSize?-?6;?i++)?{//與后一個(gè)點(diǎn)差值，所以減6????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;????//計(jì)算有效曲率點(diǎn)與后一個(gè)點(diǎn)之間的距離平方和????float?diff?=?diffX?*?diffX?+?diffY?*?diffY?+?diffZ?*?diffZ;????//排除一些易遮擋的點(diǎn)，對應(yīng)論文中圖4(b)的A點(diǎn)????if?(diff?>?0.1)?{//前提:兩個(gè)點(diǎn)之間距離要大于0.1????????//點(diǎn)的深度??????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);??????//后一個(gè)點(diǎn)的深度??????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);??????//按照兩點(diǎn)的深度的比例，將深度較大的點(diǎn)拉回后計(jì)算距離??????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;????????//邊長比也即是弧度值，若小于0.1，說明夾角比較小，斜面比較陡峭,點(diǎn)深度變化比較劇烈,點(diǎn)處在近似與激光束平行的斜面上????????if?(sqrt(diffX?*?diffX?+?diffY?*?diffY?+?diffZ?*?diffZ)?/?depth2?<?0.1)?{//排除容易被斜面擋住的點(diǎn)??????????//該點(diǎn)及前面五個(gè)點(diǎn)（大致都在斜面上）全部置為篩選過??????????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;????????}??????}????}????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;????//與前一個(gè)點(diǎn)的距離平方和????float?diff2?=?diffX2?*?diffX2?+?diffY2?*?diffY2?+?diffZ2?*?diffZ2;????//點(diǎn)深度的平方和????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;????//與前后點(diǎn)的平方和都大于深度平方和的萬分之二，這些點(diǎn)視為離群點(diǎn)，包括陡斜面上的點(diǎn)，強(qiáng)烈凸凹點(diǎn)和空曠區(qū)域中的某些點(diǎn)，置為篩選過，棄用????//對應(yīng)于論文中的圖4(a)中的B????if?(diff?>?0.0002?*?dis?&&?diff2?>?0.0002?*?dis)?{??????cloudNeighborPicked[i]?=?1;????}??}
這部分與論文中說的有點(diǎn)不一樣，論文中說將當(dāng)前sweep分為4個(gè)相同區(qū)域，而代碼中是分為了6個(gè)區(qū)域，每個(gè)區(qū)域的起始點(diǎn)和終止點(diǎn)索引分別為sp和ed，其計(jì)算本質(zhì)如下：
六等份起點(diǎn)：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
六等份終點(diǎn)：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
1.按照曲率從小到大進(jìn)行冒泡排序，A-LOAM中使用的是sort函數(shù)。
2.然后，如果曲率值大于0.1則選擇為edge point(邊緣特征點(diǎn))或less edge point(沒那么尖銳的邊緣特征點(diǎn))，edge point對應(yīng)論文中提到的每個(gè)區(qū)域選擇2個(gè)，less edge point每個(gè)區(qū)域選擇20個(gè)。
3.每選擇一個(gè)點(diǎn)后就將曲率比較大的點(diǎn)的前后各5個(gè)連續(xù)距離比較近的點(diǎn)篩選出去，防止特征點(diǎn)聚集，使得特征點(diǎn)在每個(gè)方向上盡量分布均勻。
4.然后，如果曲率值小于0.1則選擇為planar point(平面特征點(diǎn))或less planar point(沒那么平坦的平面特征點(diǎn))，planar point對應(yīng)論文中提到的每個(gè)區(qū)域選擇4個(gè)，而該區(qū)域剩下的全都?xì)w入less edge point。
5.同樣的，每選擇一個(gè)點(diǎn)后就將曲率比較大的點(diǎn)的前后各5個(gè)連續(xù)距離比較近的點(diǎn)篩選出去，防止特征點(diǎn)聚集，使得特征點(diǎn)在每個(gè)方向上盡量分布均勻。
6.最后，由于less planar point點(diǎn)最多，對每個(gè)區(qū)域less planar point的點(diǎn)進(jìn)行體素柵格濾波
??pcl::PointCloud<PointType>?cornerPointsSharp;??pcl::PointCloud<PointType>?cornerPointsLessSharp;??pcl::PointCloud<PointType>?surfPointsFlat;??pcl::PointCloud<PointType>?surfPointsLessFlat;??//這個(gè)for循環(huán)：將每條線上的點(diǎn)分入相應(yīng)的類別：邊沿點(diǎn)和平面點(diǎn)??for?(int?i?=?0;?i?<?N_SCANS;?i++)?{????pcl::PointCloud<PointType>::Ptr?surfPointsLessFlatScan(new?pcl::PointCloud<PointType>);????//將每個(gè)scan的曲率點(diǎn)分成6等份處理,確保周圍都有點(diǎn)被選作特征點(diǎn)????for?(int?j?=?0;?j?<?6;?j++)?{??????//六等份起點(diǎn)：sp?=?scanStartInd?+?(scanEndInd?-?scanStartInd)*j/6??????int?sp?=?(scanStartInd[i]?*?(6?-?j)??+?scanEndInd[i]?*?j)?/?6;??????//六等份終點(diǎn)：ep?=?scanStartInd?-?1?+?(scanEndInd?-?scanStartInd)*(j+1)/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--)?{????????????//如果后面曲率點(diǎn)大于前面，則交換??????????if?(cloudCurvature[cloudSortInd[l]]?<?cloudCurvature[cloudSortInd[l?-?1]])?{????????????int?temp?=?cloudSortInd[l?-?1];????????????cloudSortInd[l?-?1]?=?cloudSortInd[l];????????????cloudSortInd[l]?=?temp;??????????}????????}??????}??????//挑選每個(gè)分段的曲率很大和比較大的點(diǎn)??????int?largestPickedNum?=?0;??????for?(int?k?=?ep;?k?>=?sp;?k--)?{????????int?ind?=?cloudSortInd[k];??//曲率最大點(diǎn)的點(diǎn)序????????//如果曲率大的點(diǎn)，曲率的確比較大，并且未被篩選過濾掉????????if?(cloudNeighborPicked[ind]?==?0?&&????????????cloudCurvature[ind]?>?0.1)?{????????????????????????largestPickedNum++;????????//?這里對應(yīng)選點(diǎn)規(guī)則第二點(diǎn)????????if?(largestPickedNum?<=?2)?{//挑選曲率最大的前2個(gè)點(diǎn)放入sharp點(diǎn)集合????????????cloudLabel[ind]?=?2;//2代表點(diǎn)曲率很大????????????cornerPointsSharp.push_back(laserCloud->points[ind]);????????????cornerPointsLessSharp.push_back(laserCloud->points[ind]);????????}?else?if?(largestPickedNum?<=?20)?{//挑選曲率最大的前20個(gè)點(diǎn)放入less?sharp點(diǎn)集合????????????cloudLabel[ind]?=?1;//1代表點(diǎn)曲率比較尖銳????????????cornerPointsLessSharp.push_back(laserCloud->points[ind]);????????}?else?{????????????break;????????}??????????cloudNeighborPicked[ind]?=?1;//篩選標(biāo)志置位??????????//這里對應(yīng)選點(diǎn)規(guī)則第三點(diǎn)??????????//將曲率比較大的點(diǎn)的前后各5個(gè)連續(xù)距離比較近的點(diǎn)篩選出去，防止特征點(diǎn)聚集，使得特征點(diǎn)在每個(gè)方向上盡量分布均勻??????????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;??????????}????????}??????}??????//挑選每個(gè)分段的曲率很小比較小的點(diǎn)??????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;//-1代表曲率很小的點(diǎn)??????????surfPointsFlat.push_back(laserCloud->points[ind]);??????????smallestPickedNum++;??????????if?(smallestPickedNum?>=?4)?{//只選最小的四個(gè)，剩下的Label==0,就都是曲率比較小的????????????break;??????????}??????????cloudNeighborPicked[ind]?=?1;??????????for?(int?l?=?1;?l?<=?5;?l++)?{//同樣防止特征點(diǎn)聚集????????????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)（包括之前被排除的點(diǎn)）全部歸入平面點(diǎn)中l(wèi)ess?flat類別中??????for?(int?k?=?sp;?k?<=?ep;?k++)?{????????if?(cloudLabel[k]?<=?0)?{??????????surfPointsLessFlatScan->push_back(laserCloud->points[k]);????????}??????}????}????//由于less?flat點(diǎn)最多，對每個(gè)分段less?flat的點(diǎn)進(jìn)行體素柵格濾波????pcl::PointCloud<PointType>?surfPointsLessFlatScanDS;????pcl::VoxelGrid<PointType>?downSizeFilter;????downSizeFilter.setInputCloud(surfPointsLessFlatScan);????downSizeFilter.setLeafSize(0.2,?0.2,?0.2);????downSizeFilter.filter(surfPointsLessFlatScanDS);????//less?flat點(diǎn)匯總????surfPointsLessFlat?+=?surfPointsLessFlatScanDS;??}
最后這部分就是ROS中的發(fā)布話題，沒什么可講的，總結(jié)一下發(fā)布的話題都是什么：
/velodyne)cloud_2：經(jīng)過處理后的所有點(diǎn)云(按照scanID排序的有序點(diǎn)云)
/laser_cloud_sharp：edge point特征點(diǎn)，一共162=192個(gè)
/laser_cloud_less_sharp：less edge point特征點(diǎn)，一共1620=1920個(gè)
/laser_cloud_flat：planar point特征點(diǎn)，一共164=384個(gè)
/laser_cloud_less_flat：less planar point特征點(diǎn)，其余的滿足要求的個(gè)
/imu_trans：包含了：當(dāng)前sweep的IMU測量的起始RPY角、終止RPY角、最后一個(gè)點(diǎn)相對于第一個(gè)點(diǎn)的畸變位移和速度
??//publich消除非勻速運(yùn)動(dòng)畸變后的所有的點(diǎn)??sensor_msgs::PointCloud2?laserCloudOutMsg;??pcl::toROSMsg(*laserCloud,?laserCloudOutMsg);??laserCloudOutMsg.header.stamp?=?laserCloudMsg->header.stamp;??laserCloudOutMsg.header.frame_id?=?"/camera";??pubLaserCloud.publish(laserCloudOutMsg);??//publish消除非勻速運(yùn)動(dòng)畸變后的平面點(diǎn)和邊沿點(diǎn)??sensor_msgs::PointCloud2?cornerPointsSharpMsg;??pcl::toROSMsg(cornerPointsSharp,?cornerPointsSharpMsg);??cornerPointsSharpMsg.header.stamp?=?laserCloudMsg->header.stamp;??cornerPointsSharpMsg.header.frame_id?=?"/camera";??pubCornerPointsSharp.publish(cornerPointsSharpMsg);??sensor_msgs::PointCloud2?cornerPointsLessSharpMsg;??pcl::toROSMsg(cornerPointsLessSharp,?cornerPointsLessSharpMsg);??cornerPointsLessSharpMsg.header.stamp?=?laserCloudMsg->header.stamp;??cornerPointsLessSharpMsg.header.frame_id?=?"/camera";??pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);??sensor_msgs::PointCloud2?surfPointsFlat2;??pcl::toROSMsg(surfPointsFlat,?surfPointsFlat2);??surfPointsFlat2.header.stamp?=?laserCloudMsg->header.stamp;??surfPointsFlat2.header.frame_id?=?"/camera";??pubSurfPointsFlat.publish(surfPointsFlat2);??sensor_msgs::PointCloud2?surfPointsLessFlat2;??pcl::toROSMsg(surfPointsLessFlat,?surfPointsLessFlat2);??surfPointsLessFlat2.header.stamp?=?laserCloudMsg->header.stamp;??surfPointsLessFlat2.header.frame_id?=?"/camera";??pubSurfPointsLessFlat.publish(surfPointsLessFlat2);??//publich?IMU消息,由于循環(huán)到了最后，因此是Cur都是代表最后一個(gè)點(diǎn)，即最后一個(gè)點(diǎn)的歐拉角，畸變位移及一個(gè)點(diǎn)云周期增加的速度??pcl::PointCloud<pcl::PointXYZ>?imuTrans(4,?1);????//?1行4列的有序點(diǎn)云??//起始點(diǎn)歐拉角??imuTrans.points[0].x?=?imuPitchStart;??imuTrans.points[0].y?=?imuYawStart;??imuTrans.points[0].z?=?imuRollStart;??//最后一個(gè)點(diǎn)的歐拉角??imuTrans.points[1].x?=?imuPitchCur;??imuTrans.points[1].y?=?imuYawCur;??imuTrans.points[1].z?=?imuRollCur;??//最后一個(gè)點(diǎn)相對于第一個(gè)點(diǎn)的畸變位移和速度??imuTrans.points[2].x?=?imuShiftFromStartXCur;??imuTrans.points[2].y?=?imuShiftFromStartYCur;??imuTrans.points[2].z?=?imuShiftFromStartZCur;??imuTrans.points[3].x?=?imuVeloFromStartXCur;??imuTrans.points[3].y?=?imuVeloFromStartYCur;??imuTrans.points[3].z?=?imuVeloFromStartZCur;??sensor_msgs::PointCloud2?imuTransMsg;??pcl::toROSMsg(imuTrans,?imuTransMsg);??imuTransMsg.header.stamp?=?laserCloudMsg->header.stamp;??imuTransMsg.header.frame_id?=?"/camera";??pubImuTrans.publish(imuTransMsg);}
總結(jié)
本篇文章介紹了LOAM代碼的第一個(gè)節(jié)點(diǎn)文件scanRegistration，我感覺這個(gè)代碼還是很好懂得，坐標(biāo)變換部分也不是很復(fù)雜，對照論文慢慢看就能看懂。
標(biāo)簽：
