计算激光雷达各点的仰角并分析其线数——根据lidar文档垂直角公式计算自己的激光雷达参数/以aloam为例子

今日尝试大佬的激光slam代码，用自己的数据时，遇到了一点问题

如代码所示
将点云从ros的msg格式转换为pcl格式，并计算各点的仰角分析其线数，保存在laserCloudScans(N_SCANS)里，这部分使用的是16线的lidar
以我并没有看过多少slam代码的平平无奇的大脑，我立马想到，这不就是aloam里面相似的计算仰角的代码么？

大佬的代码：

//判定各点的线数，按线数保存
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

	//仰角
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;

        if (N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        laserCloudScans[scanID].push_back(point);
    }
    

aloam里面的计算仰角的代码：

 bool halfPassed = false;
    int count = cloudSize;
    PointType point;
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;//-90�ȵ�90��
        int scanID = 0;

        if (N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 32)
        {
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {   
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            ROS_BREAK();
        }
        //printf("angle %f scanID %d \n", angle, scanID);

在一些大佬对aloam的注释中，并没有详细解释这些参数是怎么的来的，但是比较详细的解释就是这句话
//计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度

于是我茅塞顿开

16线的使用的是velodyne的激光雷达，VLP-16激光雷达是Velodyne公司出品的最小型的3维激光雷达，保留了电机转速可调节的功能。实时上传周围距离和反射率的测量值。VLP-16具有100米的远量程测量距离。精巧的外观设计使得安装非常方便。重量轻，只有830g，非常适合安装在小型无人机和小型移动机器人上。 每秒高达30万个点数据输出。±15°的垂直视场，360°水平视场扫描。

重点来了，它是-15度开始扫描的，因此要用我们算出来的tangle-（-15）才是相对于第一条扫描线的角度，又因为他的每根扫描线间隔为[15-（-15）]/（16-1）=2，所以这就是我们算出来的点在第几根扫描线上
scanID = int((angle + 15) / 2 + 0.5);
仰角四舍五入(加减0.5截断效果等于四舍五入)

OK，现在我们把这个方法应用到我们自己的lidar上，先来查查lidar使用手册，使用手册上肯定会有每一根扫描线的角度的，像这样：

如果没有，那么要它何用？嗬嗬，这就是我们的手册上的：


我们的是32线的，可以清楚看见每根线的角度，使用刚才的方法来计算
初始角度是2.3125，直接可以得到，而间隔(89.5-2.3125)/(32-1)=2.8125
因此公式为：
scanID = int(((angle - 2.3125)/ 2.8125) + 0.5);

现在我们就学会了如何根据自己的lidar来修改仰角转换到线数的公式了吧，现在你可以去看看velodyne32/64的相关参数，然后检验一下自己是否可以算出来～