loam的scanRegistration.cpp文件学习

上一篇博客解析了imuhandler和AccumulateIMUShift()函数，知道了imu预积分。本篇文章就一块看看，点云生成以及点云特征是如何提取的。

一、首先看订阅点云函数 void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)。先看代码了

//接收点云数据，velodyne雷达坐标系安装为x轴向前，y轴向左，z轴向上的右手坐标系
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{if (!systemInited) {//丢弃前20个点云数据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*pifloat endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;//结束方位角与开始方位角差值控制在(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;}//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/2if (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/2if (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;//点时间=点云时间+周期时间if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变float pointTime = relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (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])*ratioFrontimuVeloXCur = 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);//将每个补偿矫正的点放入对应线号的容器}//获得有效范围内的点的数量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-5scanStartInd[0] = 5;scanEndInd.back() = cloudSize - 5;//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到for (int i = 5; i < cloudSize - 6; i++) {//与后一个点差值，所以减6float 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;}}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/6int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;//六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6int 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;}//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);
}

分为几个点：

1）认为一个周期扫描的角度近似为一个圆周，也就是360度，也就是说该帧点云中，结束角度减去起始角度得到的角度值，应该在PI和3*PI之间。startori和endori都是用atan2计算得到的，并且取了负值，所以从第二象限到第一象限是从180度到0度，而第四象限到第三象限，角度是从0-180度的。

     这就相当于是顺时针为正了。而激光雷达内部电机刚好顺时针转的，所以就是这样计算了。

endori由于加上了2PI，所以，endori的值与在PI-3PI之间。

     现在我们让startori不动，来调节endori的角度。那么想象一下，startori处于0度到180度，假设为90度，ebdori减去startori之后，小于PI，假设endori为160度，那么显然转了一个周期，才转了70度是不太合理的，所以endori要加上2PI，为520度。

        我们再想，如果startori是-PI，而endori是400度，则这时候相减，为580度了，相当于是一圈半还多了，所以要减去2PI。就像下面的式子。

当然这只是两种模拟情况，实际上还需要根据数据一点一点推导出来经验公式。

  //lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);//lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pifloat endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;//结束方位角与开始方位角差值控制在(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;}

2）再看scanid的分配

  //计算点的仰角(根据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;}

明显，scanid的分配是很粗糙的，垂直角大于0的是奇数，垂直角小于0的是0-15的偶数，宾全国ie间隔都是2。所以有点奇怪。不是按照0-15顺序排列的。

3）点角度矫正

    //该点的旋转角float ori = -atan2(point.x, point.z);if (!halfPassed) {//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿//确保-pi/2 < ori - startOri < 3*pi/2if (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/2if (ori < endOri - M_PI * 3 / 2) {ori += 2 * M_PI;} else if (ori > endOri + M_PI / 2) {ori -= 2 * M_PI;} }

分成了前半段和后半段，区分就是是否从startori转过了180度。转过了之后就是后半段了，没转过就是前半段了。具体如何来计算的，只能说也是经验公式。

4）计算出来点在imu中的时间

reltime是根据角度比例以及扫描周期（0.1ms）算出来的一个时间pointtime。timescancur是该帧点云的起始时间。两个相加，通过imuPointerFront不断的遍历，获取第一个超过timescancur+pointtime的索引

//-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）float relTime = (ori - startOri) / (endOri - startOri);//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间point.intensity = scanID + scanPeriod * relTime;//点时间=点云时间+周期时间if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变float pointTime = relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}

，如果遍历完imu数组也没有找到一个索引，

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];} 

如果遍历过程中找到了一个imu匹配的时间。

那么就采用插值的方法来计算该点的位置、姿态和速度。比较好看懂，都是在imu世界坐标系中计算得到的。

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])*ratioFrontimuVeloXCur = 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;}

5）计算点云

如果是第一个点，则不加处理。

if (i == 0) {//如果是第一个点,记住点云起始位置的速度，位移，欧拉角imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;}

如果是非第一个点，则计算他的位置。

ShiftToStartIMU(pointTime); 是计算由于加速度所产生的运动偏移

VeloToStartIMU();是计算速度增量在start点坐标系下的矢量

TransformToStartIMU(&point);是计算点在start坐标系下的位置。

详细来看  imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime; imuShiftXCur就是imu积分出来的并通过插值得到的imu世界坐标系下的当前点位置；imuShiftXStart就是imu积分出来的并通过插值得到的imu世界坐标系下的start点位置；imuVeloXStart * pointTime就是start点处的imu世界坐标系下的速度乘上时间增量，最后就得到了非匀速运动所产生的运动增量。然后通过R.inverse()变换，把这个imu世界坐标系下的增量变换到了start点所在的坐标系下的增量矢量。

//计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变
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_Tgtransfrom from the global frame to the local frame*********************************************************************************///绕y轴旋转(-imuYawStart)，即Ry(yaw).inversefloat x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;float y1 = imuShiftFromStartYCur;float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inversefloat x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverseimuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuShiftFromStartZCur = z2;
}

再来看这个函数

就是把imu世界坐标系下的当前点速度，减去imu世界坐标系下的start点速度，得到了一个imu世界坐标系下的速度矢量，然后通过R.inverse()把imu世界坐标系下的速度矢量，变换到了start点坐标系下。

//计算局部坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变（增量）
void VeloToStartIMU()
{//计算相对于第一个点由于加减速产生的畸变速度(全局坐标系下畸变速度增量delta_Vg)imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;/********************************************************************************Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vgtransfrom from the global frame to the local frame*********************************************************************************///绕y轴旋转(-imuYawStart)，即Ry(yaw).inversefloat x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;float y1 = imuVeloFromStartYCur;float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inversefloat x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverseimuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuVeloFromStartZCur = z2;
}

再看TransformToStartIMU(&point);先把激光雷达坐标系下的点变换到imu世界坐标系中，然后再变换到start坐标系下，并且要加上非匀速运动所产生的运动增量，最后得到了该点在start坐标系下的点坐标。

//去除点云加减速产生的位移畸变
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 * Pgtransfrom 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;
}

三、面点和角点的提取

这两类特征点的提取的程序相对来说好看懂，并且在其他博客中都有很详细的介绍。属于只要认真看就可以看懂的。所以在这里不详细介绍了。

参考：

【3D激光SLAM】LOAM源代码解析--scanRegistration.cpp_c#类型转换的几种方式-CSDN博客