legoloam总结（三）

最近在调试legoloam的时候发现一个问题，环境的不同，legoloam的定位以及建图效果差别很大，猜测是可能会和环境点云的特征提取有关。环境复杂时，无法提取合适的特征来估算位姿。

三.点云的预处理

1.获取雷达角度范围

在处理点云之前，legoloam首先获取了雷达的范围，也就是激光雷达的角度范围，并进行了编号（猜测这一步影响了混合式雷达的建图过程，因为混合式雷达并不是360度旋转，因此在适用legoloam的时候需要修改），求范围的方法也很简单，直接对点云的第一个点的xy求反函数得到初始角，对点云的最后一个点的xy求反函数（tan的值）得到最后一个末尾的角度值。

// 获取激光数据的角度范围   
void findStartEndAngle()
{
    // 雷达内部旋转扫描方向：Z轴俯视下来，顺时针方向（Z轴右手定则反向）

    // atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
    // segMsg.startOrientation范围为(-PI,PI]
    // segMsg.endOrientation范围为(PI,3PI]
    // 因为内部雷达旋转是顺时针的
    segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
    // 下面这句话怀疑作者可能写错了，laserCloudIn->points.size() - 2应该是laserCloudIn->points.size() - 1
    segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                 laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;

// segMsg.endOrientation - segMsg.startOrientation范围为(0,4PI)
    // 如果角度差大于3Pi或小于Pi，说明角度差有问题，进行调整。
    if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) 
    {    // 说明终点 角度为  pi - theta + 2*pi    起点的角度为 -pi + theta
        segMsg.endOrientation -= 2 * M_PI;    // 这样的话 endOrientation 就不需要 + 2*pi了    
    } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
    {   // 终点角度 < 0 (-pi左右), 起点>0 (pi左右)  
        segMsg.endOrientation += 2 * M_PI;
    }
// segMsg.orientationDiff的范围为(PI,3PI),一圈大小为2PI，应该在2PI左右
    segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
}

2.点云图像化处理

获得角度之后就要进行点云图像化处理。

在进行图像化处理之前需要知道距离图像坐标 (rowIdn, columnIdn)的。求解公式如下，ang_bottom是雷达视场角，ang_res_y垂直分辨率，Horizon_SCAN点云数。因此如果雷达参数设置错误，直接影响点云的图像化，导致建图出错。

verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;

horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;

3.点云分割

在点云分割环节，框架定义了一个函数labelComponents()，该函数对所有的点进行遍历，并通过广度优先搜索进行搜索判断是否是同一个聚类。聚类的分割以角度值为阈值。大于这个阈值说明两者之间没有突变，所以可以认为是同一个聚类。