scanRegistration
主要分为两部分：
- IMU处理
- lidar处理
IMU处理
- 接收IMU消息，
- 减去重力
- 坐标变换（从RPY — XYZ 变换到 RPY — ZXY）
imu旋转
根据欧拉角与旋转矩阵之间转换。
绕x轴旋转roll为:
R x = [ 1 0 0 0 c o s α − s i n α 0 s i n α c o s α ] R_{x}=\left [ \begin{matrix} 1& 0& 0\\ 0& cos\alpha &-sin\alpha \\ 0& sin\alpha &cos\alpha \end{matrix} \right ] Rx= 1000cosαsinα0−sinαcosα
绕Y轴旋转pitch为：
R y = [ c o s β 0 s i n β 0 1 0 − s i n β 0 c o s β ] R_{y}=\left [ \begin{matrix} cos\beta & 0& sin\beta \\ 0& 1 & 0 \\ -sin\beta & 0 &cos\beta \end{matrix} \right ] Ry= cosβ0−sinβ010sinβ0cosβ
饶Z轴旋转yaw为：
R z = [ c o s γ − s i n γ 0 s i n γ c o s γ 0 0 β 0 1 ] R_{z}=\left [ \begin{matrix} cos\gamma & -sin\gamma & 0 \\ sin\gamma & cos\gamma & 0 \\ 0\beta & 0 & 1 \end{matrix} \right ] Rz= cosγsinγ0β−sinγcosγ0001
旋转矩阵为:
R I W = R z ( y a w ) R y ( p i t c h ) R x ( r o l l ) R_{I}^{W} =R_{z}(yaw)R_{y}(pitch)R_{x}(roll) RIW=Rz(yaw)Ry(pitch)Rx(roll)
R I W = ( R W I ) − 1 R_{I}^{W} =(R_{W}^{I})^{-1} RIW=(RWI)−1
所以重力方向分量：
G
I
=
R
W
I
∗
G
W
G_{I}=R_{W}^{I}*G_{W}
GI=RWI∗GW
工程中将XYZ坐标系变换到ZXY，但是对应的旋转叫还是RPY
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;
imu积分
通过IMU的采样周期，进行位移积分。（从RPY — XYZ 变换到 RPY — ZXY）
- 将IMU从body转为world坐标。
a c c W = R I W ∗ a c c I acc_{W}=R_{I}^{W}*acc_{I} accW=RIW∗accI
//将当前时刻的加速度值绕交换过的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;
lidar处理
lidar处理主要分为3部分：
- 预处理
- 点云畸变矫正
- 特征提取
预处理
移除空点，计算scanID(扫描线速)，转换点云到ZXY，同时赋上该点的时间。
//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
point.intensity = scanID + scanPeriod * relTime;
点云畸变矫正（加速度误差）
将一帧点云中的点变换到起始点的坐标下，计算相对误差，矫正点云。
//去除点云加减速产生的位移畸变
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;
}
特征点提取
LOAM中特征点的提取分为两种：surf Point和corner Point。两种特征使用曲率来区分。曲率大的为corner Point，小的则为surf Point。
曲率计算
根据论文的曲率计算公式:
c = 1 ∣ S ∣ ∥ X ( k , i ) L ∥ ∥ ∑ j ∈ S , j ≠ i ( X ( k , i ) L − X ( k , j ) L ) ∥ c=\frac{1}{\left | S \right |\left \| X_{(k,i)}^{L} \right\| } \left \| \sum_{j\in S,j\ne i}^{} (X_{(k,i)}^{L}-X_{(k,j)}^{L}) \right \| c=∣S∣ X(k,i)L 1 j∈S,j=i∑(X(k,i)L−X(k,j)L)
计算曲率，就是将点云的前后五个点计算用于计算，计算代码所示。
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;
}
}
}
剔除无效点
- 当入射角与障碍物表面比较小的时候，判定为无效点
- 针对前后遮挡的时候，计算前后两点的间距的平方是否大于该点距离平方和的0.02%。
//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到
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;
}
}
角点，面点提取
定义曲率较大的为角点，曲率小的面点。将每个线束分为6段（特征点分布均匀），提取角点，面点。提取点前后的点筛选出去（防止特征点聚集）。具体代码就不粘贴出来，自己查看吧。
878
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
