1 scanRegistration.cpp
代碼：https://github.com/cuitaixiang/LOAM_NOTED
根據VLP16的雷射掃描模型, 對單幀點雲（paper中稱為一個Sweep）進行分線束（分為16束）, 每束稱為一個Scan， 并記錄每個點所屬線束和每個點在此幀點雲内的相對掃描時間（相對于本幀第一個點）。
針對單個Scan提取特征點, 而相對時間會用于插值計算非勻速運動畸變補償。每幀點雲,輸出角點和面點的四種特征點雲, 給下一個節點laserOdometry。
在對代碼進行閱讀之前，需要先了解坐标系的設定：
- imu為x軸向前,y軸向左,z軸向上的右手坐标系
- velodyne lidar為x軸向前,y軸向左,z軸向上的右手坐标系
- scanRegistration會把兩者通過交換坐标軸，都統一到z軸向前,x軸向左,y軸向上的右手坐标系 ，這是J. Zhang的論文裡面使用的坐标系
scanRegistration.cpp
1）對應節點：/scanRegistration
2）功能：
1.點雲處理 （點雲回調函數）
1.1 計算點的線号與點到該幀初始點的時間；
1.2 去除點雲加速度産生的畸變
1.3 計算所有點的曲率
1.4 去除平行點與遮擋點
1.5 提取面點與角點
1.6 釋出面點、角點以及IMU消息
2.imu處理（imu回調函數）
2.1 提取姿态角 (imu坐标系x軸向前，y軸向左，z軸向上，roll,pitch,yaw)
加速度（imu坐标系轉換到z軸向前,x軸向左，y軸向上的坐标系）
2.2 根據上面的加速度，轉換得到世界坐标系下的加速度值，使用勻加速模型得出目前時刻車輛在世界坐标系下的速度與位移
3）代碼分析：
訂閱話題2個：
/velodyne_points
點雲資料
/imu/data
IMU資料
釋出話題6個：
/imu_trans
釋出IMU資訊
/velodyne_cloud_2
消除非勻速運動畸變後的點
/laser_cloud_sharp(flat)、/laser_cloud_less_sharp(flat)
消除非勻速運動畸變後的平面點和邊沿點
從main函數開始：
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/data
ros::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_trans
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);
/**
* 它可以保證你指定的回調函數會被調用
* 程式執行到spin()後就不調用其他語句了
*/
ros::spin();
return 0;
}
laserCloudHandler
main函數主要是訂閱前一個節點釋出的點雲topic，一旦接受到一幀點雲就執行一次回調函數laserCloudHandler：
接收點雲處理：
//接收點雲資料，velodyne雷達坐标系安裝為x軸向前，y軸向左，z軸向上的右手坐标系
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
if (!systemInited) {//丢棄前20幀點雲資料,systemInited初始為false
systemInitCount++;
if (systemInitCount >= systemDelay) {
systemInited = true;
}
return;
}
//記錄每個scan有曲率的點的開始和結束索引
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
//目前點雲時間
double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
//消息轉換成pcl資料存放
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
//移除空點
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
//點雲點的數量
int cloudSize = laserCloudIn.points.size();
//lidar scan開始點的旋轉角,atan2範圍（-pi,+pi],計算旋轉角時取負号是因為velodyne是順時針旋轉
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//lidar scan結束點的旋轉角，加2*pi使點雲旋轉周期為2*pi
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; //加2 * M_PI在于，若結束點在x軸附近，幾度/-幾度
//結束方位角與開始方位角內插補點控制在(PI,3*PI)範圍，允許lidar不是一個圓周掃描
//正常情況下在這個範圍内：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;
}
計算每個點線号與每個點相對該幀初始時刻的時間
point.x=laserCloudIn.points[i].y; point.y=laserCloudIn.points[i].z; point.z=laserCloudIn.points[I].x
;
将坐标系轉化為z向前，x向左，y向上的右手坐标系
(原作标系x向前，y向左，z向上)
//lidar掃描線是否旋轉過半
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++) {
//坐标軸交換，velodyne lidar的坐标系也轉換到z軸向前，x軸向左的右手坐标系
point.x = laserCloudIn.points[i].y;
point.y = laserCloudIn.points[i].z;
point.z = laserCloudIn.points[i].x;
//計算點的仰角(根據lidar文檔垂直角計算公式),根據仰角排列雷射線号，velodyne每兩個scan之間間隔2度
float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
int scanID;
//仰角四舍五入(加減0.5截斷效果等于四舍五入)
int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));
if (roundedAngle > 0){
scanID = roundedAngle;
}
else {
scanID = roundedAngle + (N_SCANS - 1);
}
//過濾點，隻挑選[-15度，+15度]範圍内的點,scanID屬于[0,15]
if (scanID > (N_SCANS - 1) || scanID < 0 ){
count--;
continue;
}
//該點的旋轉角
float ori = -atan2(point.x, point.z);
if (!halfPassed) {//根據掃描線是否旋轉過半選擇與起始位置還是終止位置進行內插補點計算，進而進行補償
//確定-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;
}
//ori: 0-2pi?
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（點旋轉的角度與整個周期旋轉角度的比率, 即點雲中點的相對時間）
float relTime = (ori - startOri) / (endOri - startOri);
//點強度=線号+點相對時間（即一個整數+一個小數，整數部分是線号，小數部分是該點的相對時間）,勻速掃描：根據目前掃描的角度和掃描周期計算相對掃描起始位置的時間
point.intensity = scanID + scanPeriod * relTime;
使用imu資料進行插值去除車輛非勻速運動造成的畸變，最後将每個補償畸變的點放入對應線号的容器：
下面的代碼實作了：
1.imu時間戳恰好大于點雲時間戳的位置，該點必處于imuPointerBack和imuPointerFront之間，據此線性插值，計算點雲點的速度，位移和歐拉角
2.如果沒有，以目前收到的最新的IMU的速度，位移，歐拉角作為目前點的速度，位移，歐拉角使用；
3.記錄此sweep中的第一個點的位置、位移和歐拉角；
4.計算之後每個點相對于第一個點的由于加減速非勻速運動産生的位移速度畸變，并對點雲中的每個點位置資訊重新補償矯正（即采用三個全局函數進行處理
ShiftToStartIMU(pointTime);,VeloToStartIMU();,TransformToStartIMU(&point);
//點時間=點雲時間+周期時間
if (imuPointerLast >= 0) {//如果收到IMU資料,使用IMU矯正點雲畸變
float pointTime = relTime * scanPeriod;//計算點的周期時間
//尋找是否有點雲的時間戳小于IMU的時間戳的IMU位置:imuPointerFront
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//沒找到,此時imuPointerFront==imtPointerLast,隻能以目前收到的最新的IMU的速度，位移，歐拉角作為目前點的速度，位移，歐拉角使用
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 {//找到了點雲時間戳小于IMU時間戳的IMU位置,則該點必處于imuPointerBack和imuPointerFront之間，據此線性插值，計算點雲點的速度，位移和歐拉角
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[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) {//如果是第一個點,記住點雲起始位置的速度，位移，歐拉角 //一幀執行一次
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
} else {//計算之後每個點相對于第一個點的由于加減速非勻速運動産生的位移速度畸變，并對點雲中的每個點位置資訊重新補償矯正
ShiftToStartIMU(pointTime);
VeloToStartIMU();
TransformToStartIMU(&point);//去除加速度産生的畸變
}
}
laserCloudScans[scanID].push_back(point);//将每個補償矯正的點放入對應線号的容器
}
1).
ShiftToStartIMU(float pointTime)
我們已經知道了每個點對應的車輛在世界坐标系下的位置，而我們想要求的是這一幀資料中該點相對于起始點由于加減速造成的運動畸變。是以我們首先要求出世界坐标系下的加減速造成的運動畸變，然後将運動畸變值經過繞y、x、z軸旋轉後得到該sweep起始點坐标系下的運動畸變。
需要了解：為什麼隻有旋轉變換而沒有位移變換？因為imu的旋轉資訊是與imu全局坐标系的變換關系，是以可以隻旋轉。
//計算sweep起始坐标系下點雲中的點相對第一個開始點的由于加減速運動産生的位移畸變
void ShiftToStartIMU(float pointTime)
{
//計算相對于第一個點由于加減速産生的畸變位移(全局坐标系下畸變位移量delta_Tg)
//imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;
/********************************************************************************
Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tg
transfrom from the global frame to the local frame
*********************************************************************************/
//繞y軸旋轉(-imuYawStart)，即Ry(yaw).inverse
float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
float y1 = imuShiftFromStartYCur;
float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;
//繞x軸旋轉(-imuPitchStart)，即Rx(pitch).inverse
float x2 = x1;
float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
//繞z軸旋轉(-imuRollStart)，即Rz(pitch).inverse
imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
imuShiftFromStartZCur = z2;
}
2).
void VeloToStartIMU()
作用是求目前點對應的車輛速度相對于sweep起始點的速度畸變，先計算全局坐标系下的然後再轉換到起始點的坐标系中。
//計算sweep起始坐标系下點雲中的點相對第一個開始點由于加減速産生的的速度畸變（增量）
void VeloToStartIMU()
{
//計算相對于第一個點由于加減速産生的畸變速度(全局坐标系下畸變速度增量delta_Vg)
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
/********************************************************************************
Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vg
transfrom from the global frame to the local frame
*********************************************************************************/
//繞y軸旋轉(-imuYawStart)，即Ry(yaw).inverse
float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
float y1 = imuVeloFromStartYCur;
float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;
//繞x軸旋轉(-imuPitchStart)，即Rx(pitch).inverse
float x2 = x1;
float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
//繞z軸旋轉(-imuRollStart)，即Rz(pitch).inverse
imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
imuVeloFromStartZCur = z2;
}
3).
void TransformToStartIMU(PointType *p)
作用是将目前點先轉換到世界坐标系下然後再由世界坐标轉換到sweep起始點坐标系下。
然後減去(代碼中是加上)加減速造成的非勻速畸變的值。
//去除點雲加減速産生的位移畸變
void TransformToStartIMU(PointType *p)
{
/********************************************************************************
Ry*Rx*Rz*Pl, transform point to the global frame
*********************************************************************************/
//繞z軸旋轉(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軸旋轉(imuPitchCur)
float x2 = x1;
float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;
//繞y軸旋轉(imuYawCur)
float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
float y3 = y2;
float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;
/********************************************************************************
Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pg
transfrom global points to the local frame
*********************************************************************************/
//繞y軸旋轉(-imuYawStart)
float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
float y4 = y3;
float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;
//繞x軸旋轉(-imuPitchStart)
float x5 = x4;
float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;
//繞z軸旋轉(-imuRollStart)，然後疊加平移量
p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;
p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
p->z = z5 + imuShiftFromStartZCur;
}
下面計算所有點的曲率，并記錄每一條線開始點與結束點在所有點中的索引：
//獲得有效範圍内的點的數量
cloudSize = count;
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {//将所有的點按照線号從小到大放入一個容器
*laserCloud += laserCloudScans[i];
}
int scanCount = -1;
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;
//曲率計算
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
//記錄曲率點的索引
cloudSortInd[i] = i;
//初始時，點全未篩選過
cloudNeighborPicked[i] = 0;
//初始化為less flat點
cloudLabel[i] = 0;
//每個scan，隻有第一個符合的點會進來，因為每個scan的點都在一起存放
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);//控制每個scan隻進入第一個點
//曲率隻取同一個scan計算出來的，跨scan計算的曲率非法，排除，也即排除每個scan的前後五個點
if (scanCount > 0 && scanCount < N_SCANS) {
scanStartInd[scanCount] = i + 5;
scanEndInd[scanCount - 1] = i - 5;
}
}
}
//第一個scan曲率點有效點序從第5個開始，最後一個雷射線結束點序size-5
scanStartInd[0] = 5;
scanEndInd.back() = cloudSize - 5;
不選取以下兩種類型的點作為特征點：
a.與雷射束平行的平面上的面點;
b.遮擋處不選取作為邊界點（盡管曲率較大）.
//挑選點，排除容易被斜面擋住的點以及離群點，有些點容易被斜面擋住，而離群點可能出現帶有偶然性，這些情況都可能導緻前後兩次掃描不能被同時看到
for (int i = 5; i < cloudSize - 6; i++) {//與後一個點內插補點，是以減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;
//計算有效曲率點與後一個點之間的距離平方和
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
if (diff > 0.1) {//前提:兩個點之間距離要大于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);
//按照兩點的深度的比例，将深度較大的點拉回後計算距離
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，說明夾角比較小，斜面比較陡峭,點深度變化比較劇烈,點處在近似與雷射束平行的斜面上
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;
}
}
}
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;
}
}
然後提取平面點和角點，16條線，每條線分六段，每段中根據曲率提取2個sharp, 20個lesssharp; 4個flat,剩餘的點都歸入less flat，由于less flat 點數量較多，每條線執行一次體素濾波：
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
//将每條線上的點分入相應的類别：邊沿點和平面點
for (int i = 0; i < N_SCANS; i++) {
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
//将每個scan的曲率點分成6等份處理,確定周圍都有點被選作特征點
for (int j = 0; j < 6; j++) {
//六等份起點：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6;
//六等份終點：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--) {
//如果後面曲率點小于前面，則交換 （每段後面曲率大，前面曲率小）
if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
int temp = cloudSortInd[l - 1];
cloudSortInd[l - 1] = cloudSortInd[l];
cloudSortInd[l] = temp;
}
}
}
//挑選每個分段的曲率很大和比較大的點
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) {//挑選曲率最大的前2個點放入sharp點集合
cloudLabel[ind] = 2;//2代表點曲率很大
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else if (largestPickedNum <= 20) {//挑選曲率最大的前20個點放入less sharp點集合
cloudLabel[ind] = 1;//1代表點曲率比較尖銳
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;//篩選标志置位
//将曲率比較大的點的前後各5個連續距離比較近的點篩選出去，防止特征點聚集，使得特征點在每個方向上盡量分布均勻
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;
}
}
}
//挑選每個分段的曲率很小比較小的點
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代表曲率很小的點
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {//隻選最小的四個，剩下的Label==0,就都是曲率比較小的
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;
}
}
}
//将剩餘的點（包括之前被排除的點）全部歸入平面點中less flat類别中
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
//由于less flat點最多，對每個分段less flat的點進行體素栅格濾波
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
//less flat點彙總
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
釋出去除非勻速運動畸變後的整幀點雲，四種特征點的消息，以及imu得到的起始點歐拉角、sweep最後一個點的歐拉角、sweep最後一個點相對于第一個點的畸變位移和速度
//publich消除非勻速運動畸變後的所有的點
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera";
pubLaserCloud.publish(laserCloudOutMsg);
//publich消除非勻速運動畸變後的平面點和邊沿點
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消息,由于循環到了最後，是以是Cur都是代表最後一個點，即最後一個點的歐拉角，畸變位移及一個點雲周期增加的速度
pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1);
//起始點歐拉角
imuTrans.points[0].x = imuPitchStart;
imuTrans.points[0].y = imuYawStart;
imuTrans.points[0].z = imuRollStart;
//最後一個點的歐拉角
imuTrans.points[1].x = imuPitchCur;
imuTrans.points[1].y = imuYawCur;
imuTrans.points[1].z = imuRollCur;
//最後一個點相對于第一個點的畸變位移和速度
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);
imuHandler
我們先總體說一下imuHandler（）函數做了些什麼。首先為什麼要用IMU？可能很多人認為IMU的作用類似于cartographer中提供位姿的先驗資訊。但LOAM中并沒有使用IMU提供下一時刻位姿的先驗資訊，而是利用IMU去除雷射傳感器在運動過程中非勻速（加減速）部分造成的誤差（運動畸變）。為什麼這樣做呢？因為LOAM是基于勻速運動的假設，但實際中雷射傳感器的運動肯定不是勻速的，是以使用IMU來去除非勻速運動部分造成的誤差，以滿足勻速運動的假設。
那麼imuHandler（）函數具體做了些什麼呢？我們知道IMU的資料可以提供給我們IMU坐标系三個軸相對于世界坐标系的歐拉角和三個軸上的加速度。但由于加速度受到重力的影響是以首先得去除重力影響。在去除重力影響後我們想要獲得IMU在世界坐标系下的運動，是以根據歐拉角就可以将IMU三軸上的加速度轉換到世界坐标系下的加速度。 然後根據加速度利用 公式s1 = s2+ vt + 1/2at*t來計算位移。是以我們可以求出每一幀IMU資料在世界坐标系下對應的位移和速度。 至此imuHandler（）函數就完成了它的使命。
接收imu資訊，執行imuHandler回調函數：
//接收imu消息，imu坐标系為x軸向前，y軸向左，z軸向上的右手坐标系
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 frame
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
//減去重力的影響,求出xyz方向的加速度實際值，并進行坐标軸交換，統一到z軸向前,x軸向左的右手坐标系, 交換過後RPY對應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;
//循環移位效果，形成環形數組
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();
}
imu傳感器得到的角速度、加速度為載體系相對于慣性系（世界坐标系）的角速度、加速度在載體系下的投影（坐标）
1） 将sensor_msgs::Imu資訊中的四元數資訊轉存為tf中的四元數資訊，然後再使用getRPY得到imu坐标系下roll、pitch&yaw資訊
2)消除重力影響，并将IMU坐标系（x前y左z上）下資料轉化到 z前x左y上坐标系
imu坐标系下旋轉矩陣：
R = Rz(yaw)*Ry(pitch)*Rx(roll)
.
重力的在imu中讀數
[0,0,9.8]
(自由落體讀數為0)
去除重力即加上
[0,0,-9.8]
，
其在原坐标系下分量為
[0,0,−9.8] = R∗[x,y,z](即 RT ∗ [ 0, 0, − 9.8] = [x,y,z])
可以得到在x前，y左，z上的右手系下 x , y , z 的加速度
[x,y,z]=[9.8sinβ,−9.8sinαcosβ,−9.8cosβcosα]
其中 α = roll, β=pitch
最後交換坐标系z前x左y上：x變為z，y變為x，z變為y，減去重力得各個軸加速度 ：
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;
3）儲存每一幀imu資料的資訊（時間，IMU坐标系下的旋轉角度roll,pitch,yaw，IMU下變換坐标系後的加速度，最後調用
AccumulateIMUShift()
計算一幀imu資料在世界坐标系下的位移和速度）
4)
AccumulateIMUShift()
：這個函數作用就是将加速度轉換到世界坐标系下，假設每幀IMU之間勻加速直線運動，計算目前時刻車在世界坐标系下的位置、速度。
(因為坐标系已經轉化與世界坐标系一緻，是以角度就代表目前位置坐标系到世界坐标系下的旋轉)
//積分速度與位移
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];
//将目前時刻的加速度值繞交換過的ZXY固定軸（原XYZ）分别旋轉(roll, pitch, yaw)角，轉換得到世界坐标系下的加速度值(right hand rule)
//繞z軸旋轉(roll)
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
//繞x軸旋轉(pitch)
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//繞y軸旋轉(yaw)
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
//上一個imu點
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
//上一個點到目前點所經曆的時間，即計算imu測量周期
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
//要求imu的頻率至少比lidar高，這樣的imu資訊才使用，後面校正也才有意義
if (timeDiff < scanPeriod) {//（隐含從靜止開始運動）
//求每個imu時間點的位移與速度,兩點之間視為勻加速直線運動
// 推算目前時刻的位置資訊
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;
}
}
該節點結束.