适配禾赛OT-128

[ParkerChan]

感谢开源！
想咨询下，我的激光雷达是禾赛的OT-128
我查看了工程源码，支持XT-32、AT-128
采集场景：结构化道路、乘用车、路口数据。车速约4~50km/h
我也查看了我自己的点云数据结构，为XYZIRT，对应的timestamp是每个点的绝对时间戳，我就直接适配到的xt32handle，但也加了一个根据相对时间对点云排序的代码：

void Preprocess::xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg)
{
  pl_surf.clear();
  pl_corn.clear();
  pl_full.clear();

  pcl::PointCloud<xt32_ros::Point> pl_orig;
  pcl::fromROSMsg(*msg, pl_orig);
  int plsize = pl_orig.points.size();
  pl_surf.reserve(plsize);

  bool is_first[MAX_LINE_NUM];
  double yaw_fp[MAX_LINE_NUM] = {0};     // yaw of first scan point
  double omega_l = 3.61;                 // scan angular velocity
  float yaw_last[MAX_LINE_NUM] = {0.0};  // yaw of last scan point
  float time_last[MAX_LINE_NUM] = {0.0}; // last offset time

  if (pl_orig.points[plsize - 1].timestamp > 0) { given_offset_time = true; }
  else
  {
    given_offset_time = false;
    memset(is_first, true, sizeof(is_first));
    double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
    double yaw_end = yaw_first;
    int layer_first = pl_orig.points[0].ring;
    for (uint i = plsize - 1; i > 0; i--)
    {
      if (pl_orig.points[i].ring == layer_first)
      {
        yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
        break;
      }
    }
  }

  double time_head = pl_orig.points[0].timestamp;

  if (feature_enabled)
  {
    for (int i = 0; i < N_SCANS; i++)
    {
      pl_buff[i].clear();
      pl_buff[i].reserve(plsize);
    }

    for (int i = 0; i < plsize; i++)
    {
      PointType added_pt;
      added_pt.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      int layer = pl_orig.points[i].ring;
      if (layer >= N_SCANS) continue;
      added_pt.x = pl_orig.points[i].x;
      added_pt.y = pl_orig.points[i].y;
      added_pt.z = pl_orig.points[i].z;
      added_pt.intensity = pl_orig.points[i].intensity;
      added_pt.curvature = pl_orig.points[i].timestamp / 1000.0; // units: ms

      if (!given_offset_time)
      {
        double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
        if (is_first[layer])
        {
          // printf("layer: %d; is first: %d", layer, is_first[layer]);
          yaw_fp[layer] = yaw_angle;
          is_first[layer] = false;
          added_pt.curvature = 0.0;
          yaw_last[layer] = yaw_angle;
          time_last[layer] = added_pt.curvature;
          continue;
        }

        if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; }
        else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; }

        if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;

        yaw_last[layer] = yaw_angle;
        time_last[layer] = added_pt.curvature;
      }

      pl_buff[layer].points.push_back(added_pt);
    }

    for (int j = 0; j < N_SCANS; j++)
    {
      PointCloudXYZI &pl = pl_buff[j];
      int linesize = pl.size();
      if (linesize < 2) continue;
      vector<orgtype> &types = typess[j];
      types.clear();
      types.resize(linesize);
      linesize--;
      for (uint i = 0; i < linesize; i++)
      {
        types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
        vx = pl[i].x - pl[i + 1].x;
        vy = pl[i].y - pl[i + 1].y;
        vz = pl[i].z - pl[i + 1].z;
        types[i].dista = vx * vx + vy * vy + vz * vz;
      }
      types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
      give_feature(pl, types);
    }
  }
  else
  {
    for (int i = 0; i < plsize; i++)
    {
      PointType added_pt;
      // cout<<"!!!!!!"<<i<<" "<<plsize<<endl;

      added_pt.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      added_pt.x = pl_orig.points[i].x;
      added_pt.y = pl_orig.points[i].y;
      added_pt.z = pl_orig.points[i].z;
      added_pt.intensity = pl_orig.points[i].intensity;
      added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * 1000.f;

      // printf("added_pt.curvature: %lf %lf \n", added_pt.curvature,
      // pl_orig.points[i].timestamp);

      // if(i==(plsize-1))  printf("index: %d layer: %d, yaw: %lf, offset-time:
      // %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature,
      // prints);
      if (i % point_filter_num == 0)
      {
        double r2 = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
        if (r2 > blind_sqr && r2 < 3600)
        {
          pl_surf.points.push_back(added_pt);
          // printf("time mode: %d time: %d \n", given_offset_time,
          // pl_orig.points[i].t);
        }
      }
    }

    auto comparePoints = [](const PointType& a, const PointType& b) -> bool
    {
      return a.curvature < b.curvature;
    };

    std::sort(pl_surf.points.begin(), pl_surf.points.end(), comparePoints);
  }
  // pub_func(pl_surf, pub_full, msg->header.stamp);
  // pub_func(pl_surf, pub_surf, msg->header.stamp);
  // pub_func(pl_surf, pub_corn, msg->header.stamp);
}

我运行lo模式，发现水平方向上的重影比较严重；想询问下，是否是我激光type没有适配正确导致的？
对应的config：请忽略config中线束的数字，因为我将原始的128按ring根据固定步长抽线为32线，取的0，4，8....

/**:
  ros__parameters:
    common:
      img_topic: "/image"
      lid_topic: "/hesai/ot32"
      imu_topic: "/chcnav/devpvt"
      img_en: 0 # 是否使用相机
      lidar_en: 1 # 是否使用激光雷达
      ros_driver_bug_fix: false # 是否修复ROS驱动器bug

    extrin_calib: # 外参 lidar->imu
      extrinsic_T: [0.864749, -0.00179501, 1.62208]
      extrinsic_R: [0.00935207,   -0.999951,  -0.0033473,
                    0.999946,  0.00936742, -0.00459907,
                    0.00463019, -0.00330411,    0.999984]
      Rcl: [-0.00387589,-0.999952, -0.00901998,
            0.0026016, 0.00900993, -0.999956,
            0.999989, -0.00389919, 0.00256655] 
      Pcl: [-0.0483692, -0.76351, -1.47161]

    time_offset: # 时间偏移 
      imu_time_offset: 0.0 # 激光和imu之间的时间差
      img_time_offset: 0.1 # 激光和相机之间的时间差
      exposure_time_init: 0.0 # 曝光时间

    preprocess: # 点云预处理
      point_filter_num: 3 #这里其实是选点的间隔，每搁多少个点选一个点处理，作为系统的待处理点云
      filter_size_surf: 0.1 # # 体素滤波设置 0.1*0.1*0.1 
      # 必须与 common_lib.h 中 LID_TYPE 一致：1=AVIA 2=VELO16 3=OUST64 4=L515 5=XT32 6=PANDAR128
      # 设为 0 会走 preprocess 的 default 分支，不填充点云，易导致空点云段错误（SIGSEGV, exit -11）
      lidar_type: 5
      scan_line: 32 
      blind: 0.1

    vio:
      max_iterations: 5 # ESIKF的最大迭代次数
      outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite 每个像素块的阈值
      img_point_cov: 100 # 100 1000 vio观测方程（光度误差）的协方差，也可以说是像素点噪声协方差
      patch_size: 8 # 图像块的尺寸
      patch_pyrimid_level: 4 # 金字塔层数
      normal_en: true # 是否细化法向量，决定是否能够从单应阵获得仿射变换阵
      raycast_en: false # 是否进行体素投射
      inverse_composition_en: false # 是否进行逆补偿，SVO 采用的，Fast-livo2 默认不使用（逆补偿下的状态更新考虑的没有 Fast-livo2 全面）
      exposure_estimate_en: true # 是否估计逆曝光时间
      inv_expo_cov: 0.1 # 逆曝光时间估计的协方差

    imu:
      imu_en: false # 是否使用imu
      imu_int_frame: 30 # 初始化所需要的 IMU 帧数（Fast-LIVO2 需要静止初始化）
      acc_cov: 0.5 # 0.2 加速度计的方差
      gyr_cov: 0.3 # 0.5 陀螺仪的方差
      b_acc_cov: 0.0001 # 0.1 加速度计的偏置协方差
      b_gyr_cov: 0.0001 # 0.1 陀螺仪的偏置协方差

    lio:
      max_iterations: 5 # ESIKF 中的更新迭代次数
      dept_err: 0.02 # 激光雷达的深度误差
      beam_err: 0.05 # 激光雷达的方向误差
      min_eigen_value: 0.0025 # 0.005 # 平面奇异值分解后的最小特征上限（阈值）
      voxel_size: 0.5 # 体素块的大小 0.5*0.5*0.5。有读者可能混淆 filter_size_surf 中设置的体素尺寸，具体就是：系统先将每帧点云用 0.1*0.1*0.1 的体素进行滤波，再用 0.5*0.5*0.5 的体素划分地图。
      max_layer: 2 # 八叉树的最大层数
      max_points_num: 50 # 每个节点的最大存点量
      layer_init_num: [5, 5, 5, 5, 5] # 每个节点的最小存点量

    local_map:  
      map_sliding_en: false # 是否进行滑动地图，当内存较小时使用
      half_map_size: 100 # 地图边长的一半
      sliding_thresh: 8.0 # 每 8 米滑动一次

    uav:
      imu_rate_odom: false
      gravity_align_en: true

    publish:
      dense_map_en: false # 是否进行稠密建图
      pub_effect_point_en: false  # 是否发布有效点
      pub_plane_en: false # 是否发布平面点
      pub_scan_num: 1 # 每次发布的累积点云帧
      blind_rgb_points: 0.0 # 盲区内的 RGB 点值为 0

    evo:
      seq_name: "CBD_Building_01"
      pose_output_en: true

    pcd_save:
      pcd_save_en: true # 是否保存 PCD
      colmap_output_en: false # need to set interval = -1 是否输出 COLMAP 格式
      filter_size_pcd: 0.15 # 对 PCD 文件进行降采样
      # interval: 每个 PCD 的累积帧数。
      # -1: 仅在程序退出时保存一个 all_raw_points.pcd（需 Ctrl+C 正常退出）；>0: 运行中按帧数保存 1.pcd, 2.pcd...
      interval: -1 # 建议用 -1 并在退出时 Ctrl+C，否则可能因无 VIO 帧或点云为空而看不到文件