LeGO-LOAM算法解析

SLAM-LaserSLAM

---

Author	Wenqiang Chen, Pengpeng Su, Silin Li
Data	07/01/2019

1 ImageProjection模块

1.1 激光雷达数据处理数学模型

• 如何找到一帧数据雷达扫描的起始点和终止点？

1.2 3D点云投影到2D深度图

• 点云投影到2D平面的行列确定？

1.2 去除地面点

上图是去除地面点的原理示意图，如果上下两线之间点的XYZ坐标差构成的的俯仰角$\theta$小于一定的阈值，程序中设置的是10°，那么可以认为是地面点。

核心代码如下

// 如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1
// 否则，则不是地面点，初始化为0，进行后续操作
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY))*180/M_PI;
if (abs(angle - sensorMountAngle) <= 10)
{
    groundMat.at<int8_t>(i, j) = 1;
    groundMat.at<int8_t>(i + 1, j) = 1;
}

1.3 点云分割

1.3.1 物体聚类算法原理

从顶视图看，这是1束激光（一共有16束）先后扫描到的两个点A和点B。OA和OB是扫描点和激光雷达传感器之间的距离。 我们可以在图中计算$\beta$角。

• 如果A, B两点属于同一对象，那么$\beta$角将更大
• 如果A, B两点不属于同一个对象，则$\beta$角度将更小

上面的$\beta$角可以按照激光雷达的水平角分辨率来设置阈值。
同样在竖直方向上，上下两个激光束之间扫描到的两个点也可以按照这种方法进行判断分类，只不过角度阈值设置为激光雷达垂直分辨率罢了。

1.3.2 分割算法流程

根据前面的处理，我们得到了如下信息：

• rangeMat初始化为FLT_MAX
  遍历当前帧点云，有点位置为range，没有点的位置是FLT_MAX
• fullInfoCloud->points[index].intensity变成了range
• groundMat初始化为0
  遍历当前帧地面点云部分（对16线来说就是水平线以下的7帧点云），是地面点的标注为1，否则为0
• labelMat初始化为0
  
  • 遍历深度图，地面点位置或者rangeMat处为FLT_MAX（没有深度值）位置，将labelMat设置为-1
  • 对labelMat为0的位置（去除地面点且有深度值的点）进行labelComponents算法进行标记

1.3.3 labelComponents算法

事实上是一个四邻域查找算法，算法伪代码如下：

//全局变量
label = 1
rangeMat(rangeMat为0的位置是没有标记过的)
void labelComponents(row, col, label)
{
    queue_size = 1;
    queue.push(row,col);
    all_queue.push(row,col);
    lineCountFlag[SCAN_ID] = false;//SCAN_ID=16雷达扫描线束总数
    while(queue.size>0)
    {
        row, col = queue.top();
        labelMat(row, col) = label;
        for(iter : neighbors of (row, col))//遍历该元素的四邻域
        {
            row,col = iter.row, iter.col;
            //标记过的点直接忽略
            if (labelMat(row,col) != 0)
                continue;
            //得到分离物体的角度阈值
            if(iter->position == horizon)
                beta_threshold = 0.2;//激光雷达水平角分辨率
            else
                beta_threshold = 2;//激光雷达垂直角分辨率
            //根据分割算法原理计算当前beta角
            beta_angle = atan2(d2*sin(alpha), (d1 - d2*cos(alpha)));
            if(beta_angle > beta_threshold)
            {
                queue.push(row, col);//将此neighbor加入queue
                all_queue.push(row, col)
                labelMat(row, col) = label;
                lineCountFlag[this_scan_id] = true;//垂直方向标记计数
            }
        }
    }
    //分辨是否是有效聚类，如果是则增加labelCount，标记下一块，如果不是，就标记为outliers点
    feasibleSegment = false;
    if(all_queue.size>=30)// 如果聚类超过30个点，直接标记为一个可用聚类
        feasibleSegment = true;
    else if(all_queue.size>=5)// 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
    {
        //遍历lineCountFlag[SCAN_ID]统计true的个数lineCount
        if(lineCount>=3)
            feasibleSegment = true;// 竖直方向上超过3个也将它标记为有效聚类
    }
    if(feasibleSegment==true)
    {
        label++;
    }
    else
    {
        for(iter : all_queue)
        {
            row, col = iter.row, iter.col;
            labelMat(row, col) = 999999;//如果不是有效聚类则标记outliers点
        }
    }
}

2 FeatureAssociation模块

2.1 IMU去除重力影响

IMU的重力加速度模型，在世界坐标系下表示为

$$\frac{f-mg}{m} = a_W^{true}$$
其中
$$g = [0, 0, 9.8]^T$$
加速计测量得到的值在世界坐标系下表示如下
$$a_W^{measure} = \frac{f}{m} = a_W^{true}+g$$
换算到Body坐标系下
$$R_{BW}a_W^{measure} = R_{BW}a_W{true}+R_{BW}g$$
化简为
$$a_B^{true} = a_B^{measure} - R_{BW}g$$
其中，$a_B^{measure}$是加速度计在Body系下的测量值， $a_B^{true}$是加速度计在Body系下的真实加速度值。通过这个公式可以将重力加速度从加速度计的测量值中去除。

回顾从I系到B系的坐标变换，此时我们使用ZYX欧拉角表示方法

$$R_{BI}=R_x(roll)*R_y(pitch)*R_z(yaw)$$
其中，
$R_z(yaw) \space \space = \left[\begin{array}{ccc}{\cos(y)} & {\sin (y)} & {0} \\ {-\sin (y)} & {\cos (y)} & {0} \\ {0} & {0} & {1}\end{array}\right]$

$R_y(pitch)=\left[\begin{array}{ccc}{\cos (p)} & {0} & {-\sin (p)} \\ {0} & {1} & {0} \\ {\sin(p)} & {0} & {\cos (p)}\end{array}\right]$

$R_x(roll) \space \space = \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos (r)} & {\sin (r)} \\ {0} & {-\sin (r)} & {\cos (r)}\end{array}\right]$

所以，

$$a_B^{true} = a_B^{measure} - R_{BW}g = a_B^{measure} - \begin{bmatrix} -sin(p)\\ sin(r)cos(p)\\ cos(r)cos(p) \end{bmatrix}*9.8$$

2.2 点云畸变矫正(Adjust Distortion)

• imuHandler

  • IMU数据存储
  • 利用roll, pitch, yaw将AccX从Body坐标系转到World系
  • 求每个imu时间点的位移与速度,两点之间视为匀加速直线运动

• runFeatureAssociation(200Hz)

3 MapOptimazation模块

4 TransformFusion模块

Note：

• 深度图输出