你好，首先要感谢贵实验室开源出来的slam项目，然后我现在用手里头的禾赛32线雷达，在跑的时候出现了一些问题，想请教一下。前一段时间我用这个激光器跑了fast-lio，效果很好，可是没有颜色信息，注意到你们在此基础上开源了FAST-LIVO，能结合相机对地图进行着色，所以就想跑着看一下禾赛雷达的适配效果如何。在使用禾赛雷达的过程中，有一个问题我始终无法解决，就是在静止状态下，系统整个运行是稳定状态，只要使其运动一下，轨迹就会乱飞，我试着修改过config中的配置参数，始终达不到一个稳定的结果，所以想请教一下这个问题该如何定位并解决呢？

喔，现在的fast-livo1好像还不支持，你可以把bag发我zhengcr@connect.hku.hk，我把对应的禾赛32线LiDAR适配代码更新到仓库里。

感谢你的回答，最新的情况是这样的，我昨天重启了系统后，嗯嗯，他就又好了。我注意到有不同的地方是禾赛ros驱动输出的十分频的时间不是对齐到100毫秒的，之前出图轨迹会飘的时候，他是对齐到50毫秒或者60毫秒这个值的。不知道现在fast-livo1是不是对各个传感器的同步要求在这呢，而且我感觉，如果雷达输出不是对齐到100毫秒的十分频的话，是不是后面和相机的时间同步就会出问题？我这边可以先确认一下是不是禾赛驱动输出的不是对齐到整秒的十分频导致系统运行不稳定，然后我尽可能的提供一份运行良好的bag和一份异常的bag到您的邮箱。 —再次感谢您的支持

rviz_screenshot_2024_06_19-16_02_40 我这边LIO部分没什么问题，可以把handler函数改成这个。


void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &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)
      {
        if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind_sqr)
        {
          pl_surf.points.push_back(added_pt);
          // printf("time mode: %d time: %d \n", given_offset_time,
          // pl_orig.points[i].t);
        }
      }
    }
  }
  // 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);
}

你好，非常感谢您的支持，让我明白现在搭建的这套硬件对FAST-LIVO1是适配的，我针对您给出的建议做出了相应的修改，可是效果出来的并不一致，对此，我有几个问题想跟您请教一下： 1、首先是hesai_handle的疑问，我们对禾赛雷达数据的处理和您给出的是一致的(在不打开feature_enabled)的时候，但是对于feature_enabled打开时处理是不一致，所以想请问一下，在你哪里跑的时候是打开feature_enabled的吗？另外given_offset_time这个值是否被其他地方引用？ [2、效果差异大，为了排除是我以前修改的地方的影响，我针对您提出的修改意见，重新从仓库拉了一份代码，这是我之前跑的数据] before 然后现在修改之后，轨迹从一开始就直接起飞，就是这样的 now 不知道会不会是由于yaml文件的差异造成的，以下是我使用的时候的yaml文件，不知道和您使用的参数是否一致 `feature_extract_enable : 0 point_filter_num : 2 max_iteration : 10 dense_map_enable : 1 filter_size_surf : 0.5 filter_size_map : 0.5 cube_side_length : 20 debug : 0 grid_size : 40 patch_size : 8 img_enable : 1 lidar_enable : 1 outlier_threshold : 300 # 78 100 156 ncc_en: false ncc_thre: 0 img_point_cov : 100 # 1000 laser_point_cov : 0.001 # 0.001 cam_fx: 3526.162900 cam_fy: 3523.153465 cam_cx: 1209.620345 cam_cy: 1028.169618

common: lid_topic: "/hesai/pandar" imu_topic: "/imu/data_raw"

preprocess: lidar_type: 4 # Livox Avia LiDAR scan_line: 32 blind: 2 # blind x m disable

mapping: acc_cov_scale: 100 gyr_cov_scale: 10000 fov_degree: 360 extrinsic_T: [ -0.046534, -0.026502, -0.028807 ] extrinsic_R: [ 0.021476 ,-0.999723, -0.009677 , 0.999701 , 0.021587 ,-0.011501 , 0.011707 ,-0.009427 , 0.999887 ]

camera:

img_topic: /usb_cam/image_raw

# img_topic:  /camera/image_color
img_topic: /rgb_img
#xiyuan
Rcl: [0.0234089,-0.999419,-0.0247768,
     -0.0248415,0.0241945,-0.999399,
      0.999417,0.0240103,-0.0242607]
Pcl: [-0.0738111, 0.0236786, 0.135707]

` 再次感谢您的支持！

你好，非常感谢您的支持，让我明白现在搭建的这套硬件对FAST-LIVO1是适配的，我针对您给出的建议做出了相应的修改，可是效果出来的并不一致，对此，我有几个问题想跟您请教一下： 1、首先是hesai_handle的疑问，我们对禾赛雷达数据的处理和您给出的是一致的(在不打开feature_enabled)的时候，但是对于feature_enabled打开时处理是不一致，所以想请问一下，在你哪里跑的时候是打开feature_enabled的吗？另外given_offset_time这个值是否被其他地方引用？ [2、效果差异大，为了排除是我以前修改的地方的影响，我针对您提出的修改意见，重新从仓库拉了一份代码，这是我之前跑的数据] 然后现在修改之后，轨迹从一开始就直接起飞，就是这样的不知道会不会是由于yaml文件的差异造成的，以下是我使用的时候的yaml文件，不知道和您使用的参数是否一致 `feature_extract_enable : 0 point_filter_num : 2 max_iteration : 10 dense_map_enable : 1 filter_size_surf : 0.5 filter_size_map : 0.5 cube_side_length : 20 debug : 0 grid_size : 40 patch_size : 8 img_enable : 1 lidar_enable : 1 outlier_threshold : 300 # 78 100 156 ncc_en: false ncc_thre: 0 img_point_cov : 100 # 1000 laser_point_cov : 0.001 # 0.001 cam_fx: 3526.162900 cam_fy: 3523.153465 cam_cx: 1209.620345 cam_cy: 1028.169618

common: lid_topic: "/hesai/pandar" imu_topic: "/imu/data_raw"

preprocess: lidar_type: 4 # Livox Avia LiDAR scan_line: 32 blind: 2 # blind x m disable

mapping: acc_cov_scale: 100 gyr_cov_scale: 10000 fov_degree: 360 extrinsic_T: [ -0.046534, -0.026502, -0.028807 ] extrinsic_R: [ 0.021476 ,-0.999723, -0.009677 , 0.999701 , 0.021587 ,-0.011501 , 0.011707 ,-0.009427 , 0.999887 ]

camera: # img_topic: /usb_cam/image_raw # img_topic: /camera/image_color img_topic: /rgb_img #xiyuan Rcl: [0.0234089,-0.999419,-0.0247768, -0.0248415,0.0241945,-0.999399, 0.999417,0.0240103,-0.0242607] Pcl: [-0.0738111, 0.0236786, 0.135707]

` 再次感谢您的支持！

``

今天我也遇到这个bug，并且debug完成。主要问题有以下两块：

原版本的velodyne提取特征方法太差，换成XT32处理后会好不少。
这版FASTLVIO基于的是初代FASTLIO，也就是FASTLIO1针对Livox的版本，缺少了外参相关转换。加上这个后，效果就直接起来了。

hku-mars / FAST-LIVO

禾赛雷达适配问题 #99

img_topic: /usb_cam/image_raw