1. 概述
本节主要讲节LeogLoam中点云特征提取部分
2. 特征提取
2.1 点云预处理
- 点云数据的坐标轴进行交换，变换后的坐标轴如下图:
点云坐标系.jpg- 点云数据计算偏航角yaw, yaw = -arctan(point.x, point.z) (-atan2返回 x / z的反正切), 由于有负号，所以yaw角是顺时针角度，且yaw的范围为 yaw = [-Pi, Pi) ，
{
bool halfPassed = false;
int cloudSize = segmentedCloud->points.size();
PointType point;
for (int i = 0; i < cloudSize; i ) {
// 这里xyz与laboshin_loam代码中的一样经过坐标轴变换
// imuhandler() 中相同的坐标变换
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw，因为前面有负号，ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI)，在2PI附近
// 下面过程的主要作用是调整ori大小，满足start<ori<end
float ori = -atan2(point.x, point.z);
if (!halfPassed) {
if (ori < segInfo.startOrientation - M_PI / 2)
// start-ori>M_PI/2，说明ori小于start，不合理，
// 正常情况在前半圈的话，ori-stat范围[0,M_PI]
ori = 2 * M_PI;
else if (ori > segInfo.startOrientation M_PI * 3 / 2)
// ori-start>3/2*M_PI,说明ori太大，不合理
ori -= 2 * M_PI;
if (ori - segInfo.startOrientation > M_PI)
halfPassed = true;
} else {
ori = 2 * M_PI;
if (ori < segInfo.endOrientation - M_PI * 3 / 2)
// end-ori>3/2*PI,ori太小
ori = 2 * M_PI;
else if (ori > segInfo.endOrientation M_PI / 2)
// ori-end>M_PI/2,太大
ori -= 2 * M_PI;
}
// 用 point.intensity 来保存时间
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) scanPeriod * relTime;
}2.2 点云去畸变
- 遍历每个点云，根据点云的周期时间，计算每个点云的时间戳
- 寻找对应的imu数据，由于imu数据和点云数据时间戳不对齐，因此对imu数据的三个角度进行插值计算出点云时间戳下的imu的三个姿态角，特别的对于起始时刻的点云数据对应的imu三个姿态角保存
for (int i = 0; i < cloudSize; i ) {
// 2.1中点云数据预处理
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
imuPointerFront = imuPointerLastIteration;
// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
if (timeScanCur pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront 1) % imuQueLength;
}
if (timeScanCur pointTime > imuTime[imuPointerFront]) {
// 该条件内imu数据比激光数据早，但是没有更后面的数据
// (打个比方,激光在9点时出现，imu现在只有8点的)
// 这种情况上面while循环是以imuPointerFront == imuPointerLast结束的
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数据充足的情况下可以进行插补
// 当前timeScanCur pointTime < imuTime[imuPointerFront]，
// 而且imuPointerFront是最早一个时间大于timeScanCur pointTime的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]);
// 通过上面计算的ratioFront以及ratioBack进行插补
// 因为imuRollCur和imuPitchCur通常都在0度左右，变化不会很大，因此不需要考虑超过2M_PI的情况
// imuYaw转的角度比较大，需要考虑超过2*M_PI的情况
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;
}
// imu速度插补
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront imuVeloZ[imuPointerBack] * ratioBack;
// imu位置插补
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront imuShiftZ[imuPointerBack] * ratioBack;
}
if (i == 0) {
// 此处更新过的角度值主要用在updateImuRollPitchYawStartSinCos()中,
// 更新每个角的正余弦值
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
if (timeScanCur pointTime > imuTime[imuPointerFront]) {
// 该条件内imu数据比激光数据早，但是没有更后面的数据
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
} else {
// 在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]);
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront imuAngularRotationX[imuPointerBack] * ratioBack;
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront imuAngularRotationY[imuPointerBack] * ratioBack;
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront imuAngularRotationZ[imuPointerBack] * ratioBack;
}
// 距离上一次插补，旋转过的角度变化值
imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;
imuAngularRotationXLast = imuAngularRotationXCur;
imuAngularRotationYLast = imuAngularRotationYCur;
imuAngularRotationZLast = imuAngularRotationZCur;
// 这里更新的是i=0时刻的rpy角，后面将速度坐标投影过来会用到i=0时刻的值
updateImuRollPitchYawStartSinCos();
} else {
// 速度投影到初始i=0时刻
VeloToStartIMU();
// 将点的坐标变换到初始i=0时刻
TransformToStartIMU(&point);
}
}
segmentedCloud->points[i] = point;
}
imuPointerLastIteration = imuPointerLast;- 将imu的三轴速度转到每帧点云的起始点云对应的imu坐标系
void VeloToStartIMU()
{
// imuVeloXStart,imuVeloYStart,imuVeloZStart是点云索引i=0时刻的速度
// 此处计算的是相对于初始时刻i=0时的相对速度，由于这里的速度加速度是world坐标系
// 因此，相对速度在world坐标系下, world坐标系是z前，y上，x左的坐标系
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
// 现在将world坐标系转换到start坐标系，roll,pitch,yaw 取负值
// 首先绕y轴进行(-yaw)的角度
// |cosry 0 sinry|
// Ry=|0 1 0|.
// |-sinry 0 cosry|
float x1 = cosImuYawStart * imuVeloFromStartXCur - sinImuYawStart * imuVeloFromStartZCur;
float y1 = imuVeloFromStartYCur;
float z1 = sinImuYawStart * imuVeloFromStartXCur cosImuYawStart * imuVeloFromStartZCur;
// 绕当前x轴旋转(-pitch)的角度
// |1 0 0|
// Rx=|0 cosrx -sinrx|
// |0 sinrx cosrx|
float x2 = x1;
float y2 = cosImuPitchStart * y1 sinImuPitchStart * z1;
float z2 = -sinImuPitchStart * y1 cosImuPitchStart * z1;
// 绕当前z轴旋转(-roll)的角度
// |cosrz -sinrz 0|
// Rz=|sinrz cosrz 0|
// |0 0 1|
imuVeloFromStartXCur = cosImuRollStart * x2 sinImuRollStart * y2;
imuVeloFromStartYCur = -sinImuRollStart * x2 cosImuRollStart * y2;
imuVeloFromStartZCur = z2;
}- 点云进行去畸变，将点云根据点云生成时刻的roll、pitch和roll角转到world坐标系，然后根据上面记录的每帧点云的起始时刻的roll、pitch和roll角转到每帧点云的起始坐标系；(注意两次变换的顺序，roll->pitch->yaw, -yaw->-ptich->-roll)
// 该函数的功能是把点云坐标变换到初始imu时刻
void TransformToStartIMU(PointType *p)
{
// 因为在adjustDistortion函数中有对xyz的坐标进行交换的过程
// 交换的过程是x=原来的y，y=原来的z，z=原来的x
// 所以下面其实是绕Z轴(原先的x轴)旋转，对应的是roll角
//
// |cosrz -sinrz 0|
// Rz=|sinrz cosrz 0|
// |0 0 1|
// [x1,y1,z1]^T=Rz*[x,y,z]
//
// 因为在imuHandler中进行过坐标变换，
// 所以下面的roll其实已经对应于新坐标系中(X-Y-Z)的yaw
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轴(原先的y轴)旋转
//
// [x2,y2,z2]^T=Rx*[x1,y1,z1]
// |1 0 0|
// Rx=|0 cosrx -sinrx|
// |0 sinrx cosrx|
float x2 = x1;
float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
float z2 = sin(imuPitchCur) * y1 cos(imuPitchCur) * z1;
// 最后再绕Y轴(原先的Z轴)旋转
// |cosry 0 sinry|
// Ry=|0 1 0|
// |-sinry 0 cosry|
float x3 = cos(imuYawCur) * x2 sin(imuYawCur) * z2;
float y3 = y2;
float z3 = -sin(imuYawCur) * x2 cos(imuYawCur) * z2;
// 下面部分的代码功能是从imu坐标的原点变换到i=0时imu的初始时刻(从世界坐标系变换到start坐标系)
// 变换方式和函数VeloToStartIMU()中的类似
// 变换顺序：Cur-->世界坐标系-->Start，这两次变换中，
float x4 = cosImuYawStart * x3 - sinImuYawStart * z3;
float y4 = y3;
float z4 = sinImuYawStart * x3 cosImuYawStart * z3;
float x5 = x4;
float y5 = cosImuPitchStart * y4 sinImuPitchStart * z4;
float z5 = -sinImuPitchStart * y4 cosImuPitchStart * z4;
// 绕z轴(原先的x轴)变换角度到初始imu时刻，另外需要加上imu的位移漂移
// 后面加上的 imuShiftFromStart.. 表示从start时刻到cur时刻的漂移，
// (imuShiftFromStart.. 在start坐标系下)
p->x = cosImuRollStart * x5 sinImuRollStart * y5 imuShiftFromStartXCur;
p->y = -sinImuRollStart * x5 cosImuRollStart * y5 imuShiftFromStartYCur;
p->z = z5 imuShiftFromStartZCur;
}2.3 点云特征提取
- 点云计算曲率， d = (sum_{jin S, ineq j}^{} {r_i - r_j})^2
- 去除遮挡点，过比较两点的距离差值，标记距离远的一侧附近点（即A点），车辆运动过程中，这类点很容易被前面的B点遮挡住，所以A点需要标记剔除
void markOccludedPoints() {
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 6; i) {
float depth1 = segInfo.segmentedCloudRange[i];
float depth2 = segInfo.segmentedCloudRange[i 1];
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i 1] - segInfo.segmentedCloudColInd[i]));
if (columnDiff < 10) {
// 选择距离较远的那些点，并将他们标记为1
if (depth1 - depth2 > 0.3) {
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
} else if (depth2 - depth1 > 0.3) {
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 diff1 = std::abs(segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i]);
float diff2 = std::abs(segInfo.segmentedCloudRange[i 1] - segInfo.segmentedCloudRange[i]);
// 选择距离变化较大的点，并将他们标记为1
if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
cloudNeighborPicked[i] = 1;
}
}- 特征提取，每条扫描线的特征点，分成6段，每段找出2个曲率最大的非地面点特征作为cornerPointsSharp，和20个曲率大的非地面点作为cornerPointsLessSharp；选择4个曲率最小的地面特征点作为surfPointsFlat，并且在选取的点周围标记为Picked，防止选取的点过于密集，影响odom的计算精度。
void extractFeatures() {
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
for (int i = 0; i < N_SCAN; i ) {
surfPointsLessFlatScan->clear();
for (int j = 0; j < 6; j ) {
// 这里就是将每条点云数据分成了6段
int sp = (segInfo.startRingIndex[i] * (6 - j) segInfo.endRingIndex[i] * j) / 6;
int ep = (segInfo.startRingIndex[i] * (5 - j) segInfo.endRingIndex[i] * (j 1)) / 6 - 1;
if (sp >= ep)
continue;
// 按照cloudSmoothness.value从小到大排序
std::sort(cloudSmoothness.begin() sp, cloudSmoothness.begin() ep, by_value());
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
// 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum ;
if (largestPickedNum <= 2) {
// 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列，
// 所以这边只要选择最后两个放进队列即可
// cornerPointsSharp标记为2
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) {
// 塞20个点到cornerPointsLessSharp中去
// cornerPointsLessSharp标记为1
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l ) {
// 从ind l开始后面5个点，每个点index之间的差值，
// 确保columnDiff<=10,然后标记为我们需要的点
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind l] - segInfo.segmentedCloudColInd[ind l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 从ind l开始前面五个点，计算差值然后标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind l] - segInfo.segmentedCloudColInd[ind l 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind l] = 1;
}
}
}
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k ) {
int ind = cloudSmoothness[k].ind;
// 平面点只从地面点中进行选择? 为什么要这样做?
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < surfThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == true) {
cloudLabel[ind] = -1;
surfPointsFlat->push_back(segmentedCloud->points[ind]);
// 论文中nFp=4，将4个最平的平面点放入队列中
smallestPickedNum ;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l ) {
// 从前面往后判断是否是需要的邻接点，是的话就进行标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind l] - segInfo.segmentedCloudColInd[ind l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 从后往前开始标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind l] - segInfo.segmentedCloudColInd[ind l 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind l] = 1;
}
}
}
for (int k = sp; k <= ep; k ) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
}
// surfPointsLessFlatScan中有过多的点云，如果点云太多，计算量太大
// 进行下采样，可以大大减少计算量
surfPointsLessFlatScanDS->clear();
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);
*surfPointsLessFlat = *surfPointsLessFlatScanDS;
}
}- 根据imu计算初始的位姿变换
void updateInitialGuess() {
imuPitchLast = imuPitchCur;
imuYawLast = imuYawCur;
imuRollLast = imuRollCur;
imuShiftFromStartX = imuShiftFromStartXCur;
imuShiftFromStartY = imuShiftFromStartYCur;
imuShiftFromStartZ = imuShiftFromStartZCur;
imuVeloFromStartX = imuVeloFromStartXCur;
imuVeloFromStartY = imuVeloFromStartYCur;
imuVeloFromStartZ = imuVeloFromStartZCur;
// transformCur是在Cur坐标系下的 p_start = R*p_cur t
// R和t是在Cur坐标系下的
// 而imuAngularFromStart在 imu坐标系，加负号即 last扫描的角度 - cur 扫描的角度
if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0) {
// imgAngularFromStartX 表示一次扫描imu旋转过的角度
transformCur[0] = -imuAngularFromStartY;
transformCur[1] = -imuAngularFromStartZ;
transformCur[2] = -imuAngularFromStartX;
}
// 速度乘以时间，当前变换中的位移
if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0) {
// imuVeloFromStartX 表示一次扫描imu 运动的距离, 位移向量在每次扫描的Start坐标系
transformCur[3] -= imuVeloFromStartX * scanPeriod;
transformCur[4] -= imuVeloFromStartY * scanPeriod;
transformCur[5] -= imuVeloFromStartZ * scanPeriod;
}
}- 根据点云匹配计算相对前后帧的odom 点云补偿，根据imu计算到的当前帧的车辆的位姿变换，将点云补偿到当前帧起始时刻的imu坐标系下
void TransformToStart(PointType const *const pi, PointType *const po) {
// intensity代表的是：整数部分ring序号，小数部分是当前点在这一圈中所花的时间
// 由于点云是10HZ,即单帧扫描是0.1ms, 那么s表示扫描到当前point的时间占整个scan的时间的比例
float s = 10 * (pi->intensity - int(pi->intensity));
float rx = s * transformCur[0];
float ry = s * transformCur[1];
float rz = s * transformCur[2];
float tx = s * transformCur[3];
float ty = s * transformCur[4];
float tz = s * transformCur[5];
// 这里rz 旋转是imu原坐标系的x轴，新坐标的z轴
// 就是这里是补偿到开始帧的imu坐标系，因为这里的transformCur 的不管是旋转还是平移，都是Start - Curr, 所以该位姿变换以后是补偿到上一帧扫描起始时刻
float x1 = cos(rz) * (pi->x - tx) sin(rz) * (pi->y - ty);
float y1 = -sin(rz) * (pi->x - tx) cos(rz) * (pi->y - ty);
float z1 = (pi->z - tz);
float x2 = x1;
float y2 = cos(rx) * y1 sin(rx) * z1;
float z2 = -sin(rx) * y1 cos(rx) * z1
po->x = cos(ry) * x2 - sin(ry) * z2;
po->y = y2;
po->z = sin(ry) * x2 cos(ry) * z2;
po->intensity = pi->intensity;
}
