激光雷达每个点的ring值和相对时间计算

最新推荐文章于 2024-09-14 09:11:57 发布

灬今天学习了吗灬

最新推荐文章于 2024-09-14 09:11:57 发布

阅读量938

点赞数 2

文章标签： github

本文链接：https://blog.youkuaiyun.com/hongtiandi/article/details/131544037

版权

该代码段展示了如何处理激光雷达（LiDAR）数据，计算点云中每个点的对应时间。它使用了16线激光雷达的配置，计算每个点在竖直和水平方向上的角度，并根据扫描频率得出相对时间。程序会清除超出预设线范围的点，并更新点的时间戳信息。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

目前最新的激光雷达驱动已包含这两个值的计算，但有些数据集上却没有
代码来自：https://github.com/ZikangYuan/kaist2bag.git

  const int N_SCAN = 16;  //16线
  const int Horizon_SCAN = 1800; // 每线1800个点的数据
  const float ang_res_x = 0.2; // 水平上每条线间隔0.2°
  const float ang_res_y = 2.0; // 竖直方向上每条线间隔2°
  const float ang_bottom = 15.0+0.1; // 竖直方向上起始角度是负角度，与水平方向相差15.1°
  const int  SCAN_RATE = 10; //雷达的扫描频率
void VelodyneConverter::calculateTime(pcl::PointCloud<velodyne_ros::Point> &pcl_cloud, int64_t timestamp)
    {
        //设置一个长度为16的vector，用来存放（是否是每条ring的第一个点的标志位）
        std::vector<bool> is_first;
        is_first.resize(N_SCAN);
        fill(is_first.begin(), is_first.end(), true);
        //设置一个长度为16的vector，用来存放（每条ring的第一个点的偏航角）
        std::vector<double> yaw_first_point;
        yaw_first_point.resize(N_SCAN);
        fill(yaw_first_point.begin(), yaw_first_point.end(), 0.0);

        int i = 0;

        while (i < pcl_cloud.points.size())
        {
            // 计算竖直方向上的角度（雷达的第几线）
            verticalAngle = atan2(pcl_cloud.points[i].z, sqrt(pcl_cloud.points[i].x * pcl_cloud.points[i].x + pcl_cloud.points[i].y * pcl_cloud.points[i].y)) * 180 / M_PI;

            // rowIdn计算出该点激光雷达是竖直方向上第几线的
            // 从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)
            rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
            // std::cout << "ring : " << rowIdn << std::endl;
            // std::cout << i << " point rowIdn :  " << rowIdn << std::endl;

            if (rowIdn < 0 || rowIdn >= N_SCAN)
            {
                i=pcl_cloud.points.erase(pcl_cloud.points.begin() + i) - pcl_cloud.points.begin();
                continue;
            }

            pcl_cloud.points[i].ring = rowIdn;
            //先把除以1000，后面再除1000
            double omega = 0.361 * SCAN_RATE;
            double yaw_angle = atan2(pcl_cloud.points[i].y, pcl_cloud.points[i].x) * 57.2957;
            int layer = pcl_cloud.points[i].ring;

            if (is_first[layer])
            {
                yaw_first_point[layer] = yaw_angle;
                is_first[layer] = false;
                relative_time = 0.0;
                i++;
                continue;
            }
            // compute offset time
            if (yaw_angle <= yaw_first_point[layer])
            {
                relative_time = (yaw_first_point[layer] - yaw_angle) / omega;
            }
            else
            {
                relative_time = (yaw_first_point[layer] - yaw_angle + 360.0) / omega;
            }
            if (relative_time / double(1000) >= 0.1 && relative_time / double(1000) < 0.0)
            {
                i=pcl_cloud.points.erase(pcl_cloud.points.begin() + i) - pcl_cloud.points.begin();
                continue;
            }
            pcl_cloud.points[i].time = relative_time / float(1000);
            i++;
        }
    }