目前最新的激光雷达驱动已包含这两个值的计算,但有些数据集上却没有
代码来自: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++;
        }
    }
Logo

DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐