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 scanStartInd(N_SCANS, 0);
std::vector scanEndInd(N_SCANS, 0);
//当前点云时间
double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud laserCloudIn;
//消息转换成pcl数据存放
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector 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;
//结束方位角与开始方位角差值控制在(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 > 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;
}
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;
//点时间=点云时间+周期时间
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);//将每个补偿矫正的点放入对应线号的容器
}
//获得有效范围内的点的数量
cloudSize = count;
pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());
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;
//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到
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;
}
}
pcl::PointCloud cornerPointsSharp;
pcl::PointCloud cornerPointsLessSharp;
pcl::PointCloud surfPointsFlat;
pcl::PointCloud surfPointsLessFlat;
//将每条线上的点分入相应的类别：边沿点和平面点
for (int i = 0; i < N_SCANS; i++) {
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);
//将每个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 surfPointsLessFlatScanDS;
pcl::VoxelGrid 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 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*pi
float 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/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;
}
}分成了前半段和后半段，区分就是是否从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位置:imuPointerFront
while (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])*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;
}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_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;
}再来看这个函数
就是把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_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;
}再看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 * 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;
}三、面点和角点的提取
这两类特征点的提取的程序相对来说好看懂，并且在其他博客中都有很详细的介绍。属于只要认真看就可以看懂的。所以在这里不详细介绍了。
参考：
【3D激光SLAM】LOAM源代码解析--scanRegistration.cpp_c#类型转换的几种方式-CSDN博客