loam算法_loam代码解析1
作为一名激光slam菜鸟，涉及到的首个开源算法就是loam。之前阅读过代码，时间久了容易记不清，现在记录下来以备随时查阅，也可为入门级菜鸟提供些许参考。
本人在阅读源码过程中参考了知乎众多loam解析文章，在此表示感谢。跟知乎上loam相关解析文章一样，本文按照loam代码文件分四部分介绍，各部分解析以求尽量详细，如有理解错误，欢迎指正。
本章首先介绍代码的第一部分scanRegistration。
- 从main()函数看起：
int main(int argc, char** argv)
{
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle nh;
ros::Subscriber subLaserCloud = nh.subscribe
("/rslidar_points", 2, laserCloudHandler); //订阅雷达数据
ros::Subscriber subImu = nh.subscribe
("/imu/data", 50, imuHandler); //订阅imu数据
pubLaserCloud = nh.advertise
("/velodyne_cloud_2", 2); //发布按线分类后的点云
pubCornerPointsSharp = nh.advertise
("/laser_cloud_sharp", 2); //边界线上特征点云
pubCornerPointsLessSharp = nh.advertise
("/laser_cloud_less_sharp", 2);
pubSurfPointsFlat = nh.advertise
("/laser_cloud_flat", 2); //面上特征点云
pubSurfPointsLessFlat = nh.advertise
("/laser_cloud_less_flat", 2);
pubImuTrans = nh.advertise
("/imu_trans", 5); //发布imu处理数据
ros::spin();
return 0;
} main函数首先注册scanRegistration节点，然后订阅雷达话题/rslidar_points和惯导话题/imu/data，雷达和imu的话题名根据实际发布情况而定。接着发布按线分类后的点云，以及边界特征点和面特征点点云和处理后的惯导数据。
2. 订阅雷达话题后进入回调函数laserCloudHandler：
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
/*延迟systemDelay帧数据后读取*/
if (!systemInited) {
systemInitCount++;
if (systemInitCount >= systemDelay) {
systemInited = true;
}
return;
}
//N_SCANS(16)保存每一线的起始点和终止点索引
std::vector scanStartInd(N_SCANS, 0);
std::vector scanEndInd(N_SCANS, 0);
double timeScanCur = laserCloudMsg->header.stamp.toSec(); //帧时间戳 回调函数首先延迟systemDelay帧数据，然后定义两个vector容器分别存放N_SCANS线数据的始末点索引，并定义当前帧时间戳变量。
/* 剔除异常点并计算起始和终止点的旋转角*/
pcl::PointCloud laserCloudIn; //将ros消息转换为pcl点云
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector 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; //当前帧终止角
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
} 上段代码将ros中点云*laserCloudMsg转换为pcl点云laserCloudIn，然后去除 laserCloudIn中的异常点。startOri和endOri分别为当前帧点云的第一个point和最后一个point的旋转角，两者差值约束在Pi-3Pi范围内。
bool halfPassed = false;
int count = cloudSize;
PointType point; // pcl::PointXYZI
std::vector > laserCloudScans(N_SCANS);//存储16线点云的vector
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; //该点对应的scanID
}
else {
scanID = roundedAngle + (N_SCANS - 1);
}
if (scanID > (N_SCANS - 1) || scanID < 0 ){ //scanID超出范围则丢弃该点
count--;
continue;
}
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;
}
}
float relTime = (ori - startOri) / (endOri - startOri); //该点在一帧数据中的相对时间 0～1
point.intensity = scanID + scanPeriod * relTime; //scanID + 0.1 * (0~1) 该部分首先定义halfPassed变量表示当前点是否过半，主要用于计算当前点角度确保处于startOri和endOri之间。容器laserCloudScans 用于存储N_SCANS线的数据。接着逐一计算当前帧每个点的垂直角并对应成0～N_SCANS-1线的scanID，剔除超出ID范围的激光点。然后计算该点的水平旋转角，和在当前帧中的相对时间，最后将scanID + scanPeriod * relTime值存在point变量的intensity成员中,，以备后续使用。
3. 因为后续程序涉及到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;
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回调函数首先读取方向角四元素到orientation，然后转换成欧拉角roll/pitch/yaw，接着计算消除重力加速度影响的各个方向的线加速度，此时加速度是在IMU自身坐标系中。imuPointerLast作为imu各个参数数组的索引，循环累加，然后将当前imu数据及对应的时刻保存在各自数组中。最后调用 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];
//按照z/x/y轴旋转 将加速度由惯导坐标系旋转到局部地球坐标系
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;
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; //计算位移，匀加速运动 s=v*t+0.5*a*t^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;
}
}AccumulateIMUShift() 函数首先读取IMU回调函数中存储的各变量信息，然后通过绕z/x/y轴分别旋转将加速度从IMU坐标系转换至局部地球坐标系。timeDiff表示相邻两个惯导数据的时间差，假设在这个时间差内载体做匀加速运动，根据前一时刻三个坐标轴方向位移imuShift和速度imuVelo求当前时刻位移和速度信息。此时IMU信息转换到了局部地球坐标系。
4. 至此IMU数据处理完，接着回到雷达回调函数对应位置：
//后续位姿插值时假设机器人匀速运动，因此先消除激光点IMU加速度影响
if (imuPointerLast >= 0) { //有imu数据
float pointTime = relTime * scanPeriod; //当前激光点偏移时间
while (imuPointerFront != imuPointerLast) { //时间同步
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
// 当前激光点的time大于找到的对应惯导数据的time,则惯导数据采用当前值
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
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参数
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 { // 当不为初始点时，去除IMU加速度影响
ShiftToStartIMU(pointTime);
VeloToStartIMU();
TransformToStartIMU(&point);
}
}当imuPointerLast>=0即存在惯导数据，则执行上段代码，用于去除激光点IMU加速度影响。pointTime表示当前点在该帧数据中的偏移时刻，其中第一个点的时刻为0，最后一个点为scanPeriod。首先进行当前激光点和imu数据的时间同步，即在imu数组中找到与当前激光点时间一致的索引imuPointFront。imuPointerLast 表示imu循环数组最新索引，如果当前激光点的绝对时间大于imuPointFront对应imu时间，则将imuPointFront向imuPointerLast靠拢。如果激光点时间比imu最新数据对应的时间还大，即imuPointFront=imuPointerLast，则与之对应的imu就选最新值。这就是if (timeScanCur + pointTime > imuTime[imuPointerFront]) 所对应的情况，否则插值imuPointerBack(=imuPointerFront-1)与imuPointerFront得到激光点对应的imu数据。如果当前激光点为该帧第一个点，则赋值相关变量，否则依次调用ShiftToStartIMU(pointTime)， VeloToStartIMU()， TransformToStartIMU(&point)三个函数，下面分析各函数功能：
void ShiftToStartIMU(float pointTime)
{
//IMU加速度造成的匀速运动位移偏差
imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;
float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
float y1 = imuShiftFromStartYCur;
float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;
float x2 = x1;
float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
//位移偏差由局部地球坐标系旋转到初始点位姿坐标系
imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
imuShiftFromStartZCur = z2;
}ShiftToStartIMU()函数首先求当前时刻imu位移与匀速运动假设情况下当前时刻imu位移之差imuShiftFromStartXCur，将世界坐标系下的该差值旋转到当前帧初始点坐标系。
//将当前时刻速度插值由局部世界坐标系旋转至初始点位姿坐标系
void VeloToStartIMU()
{
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart; //世界坐标系下的速度差
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
float y1 = imuVeloFromStartYCur;
float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;
float x2 = x1;
float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
imuVeloFromStartZCur = z2;
}匀速运动情况下当前时刻速度应该与初始点速度一致，将由imu加速度造成的速度差imuVeloFromStartXCur由局部地球坐标系旋转到初始点坐标系。
void TransformToStartIMU(PointType *p)
{
//当前激光点旋转到局部地球坐标系
float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;
float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
float z1 = p->z;
float x2 = x1;
float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;
float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
float y3 = y2;
float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;
//局部地球坐标系下激光点旋转至该帧初始点坐标系并消除imu加速度造成的位移偏差
float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
float y4 = y3;
float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;
float x5 = x4;
float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;
p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;//加上IMU位置运动补偿
p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
p->z = z5 + imuShiftFromStartZCur;
}TransformToStartIMU()函数首先将当前激光点根据当前imu姿态角旋转到局部地球坐标系，然后由局部坐标系旋转至初始点坐标系方向，并加上imu计算得到的位移偏差，最终激光点转换到匀速运动假设情况下当前帧初始点坐标系，接着将激光点存入laserCloudScans对应的scanID中，从而完成按线分类工作。按照线号从小到大，通过laserCloud指针将N_SCANS线的激光数据组成pcl点云。
5. 针对按线分类后的点云，通过激光点左右各5个点进行曲率计算：
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 = 。。。
float diffZ = 。。。
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
//计算每一线的起始点和终止点 即扣除每一线始末各5个点
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);
if (scanCount > 0 && scanCount < N_SCANS) {
scanStartInd[scanCount] = i + 5;
scanEndInd[scanCount - 1] = i - 5;
}
}
}
scanStartInd[0] = 5; //第一条线的起始位置
scanEndInd.back() = cloudSize - 5; //最后一条线的终止位置曲率计算方法为求相邻10个点与当前点i在x/y/z方向的坐标差，并求平方和，存放在cloudCurvature[i]。cloudSortInd[i]表示当前点在点云中索引，cloudNeighborPicked[i]用于标记是否可作为特征点，cloudLabel[i]用于标记特征点为边界线还是平面特征点。通过当前点intensity中存放的scanID信息，求每一个线的起始点和终止点索引，其中第一线的起始索引和最后线的终止索引直接赋值即可。
6. 判断当前点和周边点是否可以作为特征点，判断依据见论文。
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) { //前后两点距离的平方大于阈值 论文中b情况 可能存在遮挡
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) { //当前点可能被遮挡
//以短边i+1构成等腰三角形，diffX/diffY/diffZ平方根表示等腰三角形的底
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;
//实际表示i与i+1夹角小于5.732度 sin(5.732) ~= 0.1 认为被遮挡
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
cloudNeighborPicked[i - 5] = 1; //当前点i被遮挡，则i及其往前5个点都不能作为特征点
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; //当前点后一点i+1被遮挡，则i+1及其后5个点不能作为特征点
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
}
//针对论文fig4中(a)情况
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;
}
}剔除点云前后各5个点，逐个计算当前激光点与后一激光点的距离平方，如果距离方diff大于阈值，则表示可能存在遮挡。进一步计算两个激光点的深度信息depth1和depth2，如果depth1>depth2，则当前点可能被遮挡。进一步以短边depth2构成等腰三角形并计算三角形的底边长，根据
7. 前述工作得到了匀速运动假设后的按线分类的点云，且标记了不可作为特征点的激光点，现在进行特征点分类。
for (int i = 0; i < N_SCANS; i++) { //按线处理
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); //用于滤波降低数据量
//每个线分6段，sp/ep分别为各段起始和终止位置
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;
}
}
} 特征点按照每一线进行选取，首先将该线分成6段，根据每一线的始末激光点索引计算6段的始末点索引，然后每一段内部激光点按照曲率升序排列。
//从每一段最大曲率(每一段末尾)处往前判断，用于确定特征线点
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSortInd[k]; //排序后的点在点云中的索引
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1) {//没有被标记为不可取特征点且曲率大于0.1
largestPickedNum++;
if (largestPickedNum <= 2) { //2个特征线点
cloudLabel[ind] = 2; //每个点标记为2
cornerPointsSharp.push_back(laserCloud->points[ind]); //保存sharp点云中
cornerPointsLessSharp.push_back(laserCloud->points[ind]);//同时保存在lessSharp
} else if (largestPickedNum <= 20) { //20个lesssharp点(包括前面2个标记为2的点)
cloudLabel[ind] = 1; //后18个lesssharp特征线点，标记为1
cornerPointsLessSharp.push_back(laserCloud->points[ind]); //保存在lessSharp
} else {
break;
}
cloudNeighborPicked[ind] = 1; //该点被选为特征点后，标记为不可用
//对ind点周围的点是否能作为特征点进行判断 除非距离大于阈值，否则前后各五个点不能作为特征点
for (int l = 1; l <= 5; l++) { //后5个点
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--) { //前5个点
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;
}
}
}上段代码根据每段曲率由大到小筛选边界线特征点，如果该点没有被标记为不可取特征点且曲率大于阈值，则曲率最大的前2个点标记为SharpPoints，保存在cornerPointsSharp和cornerPointsLessSharp中，接着的18个激光点标记为LessSharpPoints，保存在cornerPointsLessSharp中。当前点被选为特征点后，对其前后各5个点进行标记，除非相邻点的距离大于阈值，否则标记为不可作特征点。
//从每一段曲率最小(前端)开始查找 用于确定平面点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1) {//没有被标记为不可取特征点 且曲率小于0.1
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]); //4个平面点 标记为-1
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[ind] = 1;
// 判断附近点是否需要标记为不可用特征点
。。。。。
。。。。。
}
}
for (int k = sp; k <= ep; k++) {//没被标记过的点和前述标记的平面点，作为lessFlat平面点
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}对于平面特征点，通过每一段线中曲率小端开始查找。如果当前点没有被标记且曲率小于阈值，则该点为FlatPoints，保存在surfPointsFlat中，平面特征点每段提取4个。同样的，需要判断前后各5个点是否需要标记为不可用特征点。最后，除了被标记过的点(本身不能作为特征点以及被标记为边界线的特征点)，其它点都作为lessFlatPoints存储到surfPointsLessFlatScan中，并进一步滤波降采样(lessFlat点太多),，最后存储到surfPointsLessFlat中。
最后，将各类消息发布出去。主要包括按线分类后的当前帧点云laserCloud，边界线上的sharp特征点/lessSharp特征点，平面上的flat特征点/lessFlat特征点，以及imu处理后的当前帧起始点和终止点的姿态角/转换到当前帧初始点坐标系的终止点位移偏差和速度偏差。
小结一下本章的主要内容，scanRegistration主要完成点云的按线分类，当有imu数据时完成对应激光点的由加速度造成的匀速运动假设参数补偿，接着完成激光点曲率计算并提取特征点，最后发布处理结果。