hku-mars / FAST_LIO

A computationally efficient and robust LiDAR-inertial odometry (LIO) package

GNU General Public License v2.0

2.54k stars 860 forks source link

【坐标系转换】适配其它品牌Lidar和IMU #141

Closed liang0724s closed 2 years ago

liang0724s commented 2 years ago

前提：已标定好Lidar（32线）和IMU（6轴）外参，，rosbag topic类型已转成livox_ros_driver/CustomMsg和 sensor_msgs/Imu, 但是坐标系和livox avia不同。请问在坐标转换的时候，那些地方需要注意或者要修改的？

liang0724s commented 2 years ago

成功适配某品牌的Lidar（32线）和IMU/INS（6轴），IMU坐标系为前右下，Lidar坐标系为x向左、y向后、z向上，激光雷达点云通过外参矩阵统一到IMU坐标系，演示平台为车载。适配注意事项： 1.Lidar和IMU时间对齐 2.注意Lidar和IMU时间戳单位是否一致 3.如果是INS，注意加速度计输出单位g or m/s2 4.基于Lidar坐标系，注意点云方位角计算方式

JokerJohn commented 2 years ago

大哥您好，有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线，它的y朝向是向后的。我现在也是采用这款激光雷达，加一个博世的BMI085（6 轴 MEMS）IMU，它的朝向是xyz前左下，我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试，在fastlio2中，开始一小段直线路段是正常的，但是在墙角的地方有一个不太大旋转，系统就飞了，终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外，我在另外一个室内测试场景中，也是走到墙角的地方飞掉了。此外，时间戳部分，在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s，应该也是正确的。请教一下，这可能是什么原因导致的呢？

common:
  lid_topic: "/hesai/pandar"
  imu_topic: "/alphasense/imu"
  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

preprocess:
  lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai
  scan_line: 32
  blind: 0.3

mapping:
  acc_cov: 0.0011501915187049582
  gyr_cov: 5.084312924828687e-05
  b_acc_cov: 0.06080652138668933
  b_gyr_cov: -0.0015351229643790084
  fov_degree: 180
  det_range: 150.0
  extrinsic_est_en: false      # true: enable the online estimation of IMU-LiDAR extrinsic
  extrinsic_T: [ -0.001, -0.00855, 0.055 ]
  extrinsic_R: [ 0, -1,  0,
                 -1, 0,  0,
                 0,  0, -1 ]

 double time_begin = pl_orig.points[0].timestamp;
    for (uint i = 0; i < plsize; i++) {
      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
          + pl_orig.points[i].z * pl_orig.points[i].z;
      if (range < (blind * blind)) continue;
      Eigen::Vector3d pt_vec;
      PointType added_pt;
      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.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
      if (yaw_angle >= 180.0)
        yaw_angle -= 360.0;
      if (yaw_angle <= -180.0)
        yaw_angle += 360.0;

      added_pt.curvature = pl_orig.points[i].timestamp - time_begin;

      if (pl_orig.points[i].ring < N_SCANS) {
        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
      }
    }

XW-HKU commented 2 years ago

大哥您好，有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线，它的y朝向是向后的。我现在也是采用这款激光雷达，加一个博世的BMI085（6 轴 MEMS）IMU，它的朝向是xyz前左下，我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试，在fastlio2中，开始一小段直线路段是正常的，但是在墙角的地方有一个不太大旋转，系统就飞了，终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外，我在另外一个室内测试场景中，也是走到墙角的地方飞掉了。此外，时间戳部分，在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s，应该也是正确的。请教一下，这可能是什么原因导致的呢？
common:
  lid_topic: "/hesai/pandar"
  imu_topic: "/alphasense/imu"
  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

preprocess:
  lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai
  scan_line: 32
  blind: 0.3

mapping:
  acc_cov: 0.0011501915187049582
  gyr_cov: 5.084312924828687e-05
  b_acc_cov: 0.06080652138668933
  b_gyr_cov: -0.0015351229643790084
  fov_degree: 180
  det_range: 150.0
  extrinsic_est_en: false      # true: enable the online estimation of IMU-LiDAR extrinsic
  extrinsic_T: [ -0.001, -0.00855, 0.055 ]
  extrinsic_R: [ 0, -1,  0,
                 -1, 0,  0,
                 0,  0, -1 ]
 double time_begin = pl_orig.points[0].timestamp;
    for (uint i = 0; i < plsize; i++) {
      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
          + pl_orig.points[i].z * pl_orig.points[i].z;
      if (range < (blind * blind)) continue;
      Eigen::Vector3d pt_vec;
      PointType added_pt;
      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.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
      if (yaw_angle >= 180.0)
        yaw_angle -= 360.0;
      if (yaw_angle <= -180.0)
        yaw_angle += 360.0;

      added_pt.curvature = pl_orig.points[i].timestamp - time_begin;

      if (pl_orig.points[i].ring < N_SCANS) {
        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
      }
    }

你怎么判断时间戳是对的，详细说一下，你引用的这段代码是不对的。还有，你这个代码不是最新版。

JokerJohn commented 2 years ago

是这样的，我新增了pandar_handler来处理禾赛点云，其中禾赛点云结构的定义如下：

namespace pandar_ros{
struct Point {
  PCL_ADD_POINT4D
  float intensity;
  double timestamp;
  uint16_t ring;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

}

POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(double, timestamp, timestamp)
(uint16_t, ring, ring)
)

其时间通道名字为timestamp，时间戳的定义是utc时间，所以在求取点云中每个点的时间戳的时候需要减掉起始点的时间，保证每个点的时间是相对于点云起始点的相对时间，范围在0-0.1s内，我把这些点的时间打印出来，发现时间数值是符合要求的。

void Preprocess::pandar_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
  pl_surf.clear();
  pl_corn.clear();
  pl_full.clear();
  pcl::PointCloud<pandar_ros::Point> pl_orig;
  pcl::fromROSMsg(*msg, pl_orig);

  int plsize = pl_orig.size();
  pl_corn.reserve(plsize);
  pl_surf.reserve(plsize);
  if (feature_enabled) {
    for (int i = 0; i < N_SCANS; i++) {
      pl_buff[i].clear();
      pl_buff[i].reserve(plsize);
    }
    double timeScanCur = msg->header.stamp.toSec();
    double time_begin = pl_orig.points[0].timestamp;
    for (uint i = 0; i < plsize; i++) {
      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
          + pl_orig.points[i].z * pl_orig.points[i].z;
      if (range < (blind * blind)) continue;
      Eigen::Vector3d pt_vec;
      PointType added_pt;
      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.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
      if (yaw_angle >= 180.0)
        yaw_angle -= 360.0;
      if (yaw_angle <= -180.0)
        yaw_angle += 360.0;

      added_pt.curvature = pl_orig.points[i].timestamp - time_begin;

      if (i % 2000) {
        std::cout << std::fixed << std::setprecision(12) << "time: " << added_pt.curvature << std::endl;
        std::cout << std::fixed << std::setprecision(12) << "point time: " << pl_orig.points[i].timestamp << std::endl;
      }
      if (pl_orig.points[i].ring < N_SCANS) {
        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
      }
    }

    for (int j = 0; j < N_SCANS; j++) {
      PointCloudXYZI &pl = pl_buff[j];
      int linesize = pl.size();
      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 {
    double time_stamp = msg->header.stamp.toSec();
    double time_begin = pl_orig.points[0].timestamp;
    // cout << "===================================" << endl;
    // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
    for (int i = 0; i < pl_orig.points.size(); i++) {
      if (i % point_filter_num != 0) continue;

      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
          + pl_orig.points[i].z * pl_orig.points[i].z;

      if (range < (blind * blind)) continue;

      Eigen::Vector3d pt_vec;
      PointType added_pt;
      // change the axis
      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.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      added_pt.curvature = pl_orig.points[i].timestamp - time_begin;

      if (i % 2000) {
        std::cout << std::fixed << std::setprecision(12) << "curvature: " << added_pt.curvature << std::endl;
        std::cout << std::fixed << std::setprecision(12) << "point time: " << pl_orig.points[i].timestamp << std::endl;
      }

      pl_surf.points.push_back(added_pt);
    }

    ROS_WARN("GET HESAI POINT CLOUD %d: ", pl_surf.points.size());

  }
//  pub_func(pl_surf, pub_full, msg->header.stamp);
//  pub_func(pl_surf, pub_corn, msg->header.stamp);
}

因为这里点的时间戳我采用的是秒为单位，所以在laserMapping.cpp中sync_packages的时候判定点云结束时间部分代码我也进行了相应调整，统一时间单位为s.

  /*** push a lidar scan ***/
  if (!lidar_pushed) {
    meas.lidar = lidar_buffer.front();
    meas.lidar_beg_time = time_buffer.front();
    if (meas.lidar->points.size() <= 1) // time too little
    {
      lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
      ROS_WARN("Too few input point cloud!\n");
    } else if (meas.lidar->points.back().curvature < 0.5 * lidar_mean_scantime) {
//    } else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime) {
//      std::cout << "lidar end time: " << lidar_end_time << std::endl;
    } else {
      scan_num++;
      lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
//      lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
      lidar_mean_scantime += (meas.lidar->points.back().curvature - lidar_mean_scantime) / scan_num;
      std::cout << "lidar end time: " << lidar_end_time << std::endl;
    }
    lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;

    meas.lidar_end_time = lidar_end_time;

    lidar_pushed = true;
  }

终端的输出结果如下，点云中点的时间基本保持在0-0.1s之间，这与我10hz的激光雷达是相匹配的。我这个代码是两个月前下载的，我也观察到您在20天前更新了代码，增加了时间单位设置的内容。其他的核心算法部分有更改吗？我的坐标系定义如下：

XW-HKU commented 2 years ago

added_pt.curvature的单位需要是毫秒，也就是需要在0-100 .0之间。

JokerJohn commented 2 years ago

您好，我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定，加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题，我换成了默认的bias（比我标定的结果小2个数量级），就正常跑完。此外，在室内这种场景中，要想达到fastlio的最佳效果，有哪些参数需要格外注意的吗？

XW-HKU commented 2 years ago

您好，我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定，加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题，我换成了默认的bias（比我标定的结果小2个数量级），就正常跑完。此外，在室内这种场景中，要想达到fastlio的最佳效果，有哪些参数需要格外注意的吗？

你是学术实验还是？

JokerJohn commented 2 years ago

您好，我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定，加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题，我换成了默认的bias（比我标定的结果小2个数量级），就正常跑完。此外，在室内这种场景中，要想达到fastlio的最佳效果，有哪些参数需要格外注意的吗？

你是学术实验还是？

对，我是用于学术实验，室内场景居多。

liang0724s commented 2 years ago

您好，我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定，加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题，我换成了默认的bias（比我标定的结果小2个数量级），就正常跑完。此外，在室内这种场景中，要想达到fastlio的最佳效果，有哪些参数需要格外注意的吗？

你是学术实验还是？

对，我是用于学术实验，室内场景居多。

我用的数据室外场景居多，包括退化场景，目前从适配结果看，容易发生180度转弯（慢速）匹配失败，退化场景比如长隧道也容易失败

XW-HKU commented 2 years ago

您好，我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定，加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题，我换成了默认的bias（比我标定的结果小2个数量级），就正常跑完。此外，在室内这种场景中，要想达到fastlio的最佳效果，有哪些参数需要格外注意的吗？

你是学术实验还是？

对，我是用于学术实验，室内场景居多。

我用的数据室外场景居多，包括退化场景，目前从适配结果看，容易发生180度转弯（慢速）匹配失败，退化场景比如长隧道也容易失败

你这个现象听上去就像是时间戳单位不对。

liang0724s commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2

mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999]

publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame

pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file;

-1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的 Selection_023

ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

liang0724s commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2

mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999]

publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame

pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的

ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

180度掉头后的Attitude曲线应该如下图红线所示： Selection_027

怀疑问题是不是出现在ICP匹配上？

XW-HKU commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2

mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999]

publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame

pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的

ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

你的旋转外参是怎么搞到的？你这个旋转外参的旋转矩阵的行列式是-1，这是不对的。

liang0724s commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的 ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

你的旋转外参是怎么搞到的？你这个旋转外参的旋转矩阵的行列式是-1，这是不对的。

仔细检查了下，粗心大意，旋转矩阵的(0,0)项少了”-“号。这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转，我最终把他俩转到IMU坐标系下【左乘IMU（前-右-下）到车体坐标系(右-前-上)的转换矩阵的逆（x,y,z,r,p,y）=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2)）】，再传给IMU，同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配，最终到得到的pose也是在IMU坐标系。更正后又跑了一下，还是得到上面的错误轨迹

注：我的这段数据有动态交通流，180度转弯发生在内地小区门口（结构特征还挺明显），当时转弯时有一辆货车经过。掉头后也有车辆经过，但从数据看，也能匹配上。

XW-HKU commented 2 years ago

launch file 里面的 point_filter_num 改成1试试
你可以检查下转弯的时候路边建筑的点云细节，如果墙面有模糊，那大概率是imu外参或者数据转换的问题（比如xy轴反了这种可能）。

XW-HKU commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的 ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

你的旋转外参是怎么搞到的？你这个旋转外参的旋转矩阵的行列式是-1，这是不对的。

仔细检查了下，粗心大意，旋转矩阵的(0,0)项少了”-“号。这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转，我最终把他俩转到IMU坐标系下【左乘IMU（前-右-下）到车体坐标系(右-前-上)的转换矩阵的逆（x,y,z,r,p,y）=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2)）】，再传给IMU，同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配，最终到得到的pose也是在IMU坐标系。更正后又跑了一下，还是得到上面的错误轨迹

注：我的这段数据有动态交通流，180度转弯发生在内地小区门口（结构特征还挺明显），当时转弯时有一辆货车经过。掉头后也有车辆经过，但从数据看，也能匹配上。

你这个旋转的说法我没看懂，既然你的imu和lidar已经align到了xyz朝向一致，那你的外参应该使用单位阵而不是对角线元素-1 -1 1。而且，我说的时间戳单位不是指lidar rostopic header的时间戳单位，而是指lidar一帧里面每个点的相对时间戳的单位。

liang0724s commented 2 years ago

launch file 里面的 point_filter_num 改成1试试

你可以检查下转弯的时候路边建筑的点云细节，如果墙面有模糊，那大概率是imu外参或者数据转换的问题（比如xy轴反了这种可能）。

@XW-HKU 太感谢啦，根据您的思路仔细梳理一下坐标系的转换过程，确实是数据转换出现了问题，首先您在后向传播过程中其实已经利用外参对点云进行了坐标转换，但是我又在处理点云ros节点的时候又对点云进行了坐标转换。

注意到一个问题，对于后向传播阶段对点云进行校准的公式，您注释说公式不准确，是因为只用到旋转，就像 Duplicate of #113 所以提到的那样。

liang0724s commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的 ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

你的旋转外参是怎么搞到的？你这个旋转外参的旋转矩阵的行列式是-1，这是不对的。

仔细检查了下，粗心大意，旋转矩阵的(0,0)项少了”-“号。这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转，我最终把他俩转到IMU坐标系下【左乘IMU（前-右-下）到车体坐标系(右-前-上)的转换矩阵的逆（x,y,z,r,p,y）=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2)）】，再传给IMU，同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配，最终到得到的pose也是在IMU坐标系。更正后又跑了一下，还是得到上面的错误轨迹注：我的这段数据有动态交通流，180度转弯发生在内地小区门口（结构特征还挺明显），当时转弯时有一辆货车经过。掉头后也有车辆经过，但从数据看，也能匹配上。

你这个旋转的说法我没看懂，既然你的imu和lidar已经align到了xyz朝向一致，那你的外参应该使用单位阵而不是对角线元素-1 -1 1。而且，我说的时间戳单位不是指lidar rostopic header的时间戳单位，而是指lidar一帧里面每个点的相对时间戳的单位。

我的imu和lidar的坐标系需要需要我设定的旋转矩阵才能align到了xyz朝向一致，您的这行代码也说明了这点： V3D P_compensate = imu_state.offset_R_L_I.conjugate() (imu_state.rot.conjugate() (R_i (imu_state.offset_R_L_I P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!

lidar一帧里面每个点的相对时间戳的单位=s, 然后基于time_unit_scale换算成ms进行后面的计算，您的代码里也是这么写的，这部分的逻辑我没有改动。

XW-HKU commented 2 years ago

确定Lidar输出的时间戳UTM时间，单位是秒，所以对应的配置项timestamp_unit =0, 最终换算成ms，处理lidar的句柄函数已根据velodyne句柄做了适配修改。下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况，旋涡左右两边车道应该是重合的 ikfom结果输出： IMU结果输出：从图片看，在转弯时刻陀螺仪数据特征很明显，可以看出经过了三个转弯，前面两个是90度转弯。后面是180度掉头

你的旋转外参是怎么搞到的？你这个旋转外参的旋转矩阵的行列式是-1，这是不对的。

仔细检查了下，粗心大意，旋转矩阵的(0,0)项少了”-“号。这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转，我最终把他俩转到IMU坐标系下【左乘IMU（前-右-下）到车体坐标系(右-前-上)的转换矩阵的逆（x,y,z,r,p,y）=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2)）】，再传给IMU，同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配，最终到得到的pose也是在IMU坐标系。更正后又跑了一下，还是得到上面的错误轨迹注：我的这段数据有动态交通流，180度转弯发生在内地小区门口（结构特征还挺明显），当时转弯时有一辆货车经过。掉头后也有车辆经过，但从数据看，也能匹配上。

你这个旋转的说法我没看懂，既然你的imu和lidar已经align到了xyz朝向一致，那你的外参应该使用单位阵而不是对角线元素-1 -1 1。而且，我说的时间戳单位不是指lidar rostopic header的时间戳单位，而是指lidar一帧里面每个点的相对时间戳的单位。

我的imu和lidar的坐标系需要需要我设定的旋转矩阵才能align到了xyz朝向一致，您的这行代码也说明了这点： V3D P_compensate = imu_state.offset_R_L_I.conjugate() (imu_state.rot.conjugate() (R_i (imu_state.offset_R_L_I P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!

lidar一帧里面每个点的相对时间戳的单位=s, 然后基于time_unit_scale换算成ms进行后面的计算，您的代码里也是这么写的，这部分的逻辑我没有改动。

后向传播的公司是正确的，注释应该是忘删了。不过问题解决了就行。

JiangxinkeTao commented 1 year ago

大哥您好，有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线，它的y朝向是向后的。我现在也是采用这款激光雷达，加一个博世的BMI085（6 轴 MEMS）IMU，它的朝向是xyz前左下，我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试，在fastlio2中，开始一小段直线路段是正常的，但是在墙角的地方有一个不太大旋转，系统就飞了，终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外，我在另外一个室内测试场景中，也是走到墙角的地方飞掉了。此外，时间戳部分，在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s，应该也是正确的。请教一下，这可能是什么原因导致的呢？
common:
  lid_topic: "/hesai/pandar"
  imu_topic: "/alphasense/imu"
  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

preprocess:
  lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai
  scan_line: 32
  blind: 0.3

mapping:
  acc_cov: 0.0011501915187049582
  gyr_cov: 5.084312924828687e-05
  b_acc_cov: 0.06080652138668933
  b_gyr_cov: -0.0015351229643790084
  fov_degree: 180
  det_range: 150.0
  extrinsic_est_en: false      # true: enable the online estimation of IMU-LiDAR extrinsic
  extrinsic_T: [ -0.001, -0.00855, 0.055 ]
  extrinsic_R: [ 0, -1,  0,
                 -1, 0,  0,
                 0,  0, -1 ]
 double time_begin = pl_orig.points[0].timestamp;
    for (uint i = 0; i < plsize; i++) {
      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
          + pl_orig.points[i].z * pl_orig.points[i].z;
      if (range < (blind * blind)) continue;
      Eigen::Vector3d pt_vec;
      PointType added_pt;
      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.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
      if (yaw_angle >= 180.0)
        yaw_angle -= 360.0;
      if (yaw_angle <= -180.0)
        yaw_angle += 360.0;

      added_pt.curvature = pl_orig.points[i].timestamp - time_begin;

      if (pl_orig.points[i].ring < N_SCANS) {
        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
      }
    }

大佬，您好，我使用的是禾赛Panda人QT64，禾赛64线的激光雷达，没办法适配，我按照您说的一些内容进行配置，在终端打印的是 No point, skip this scan! 我想咨询你关于适配的问题，诚盼您的回复，感谢

JiangxinkeTao commented 1 year ago

大哥您好，有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线，它的y朝向是向后的。我现在也是采用这款激光雷达，加一个博世的BMI085（6 轴 MEMS）IMU，它的朝向是xyz前左下，我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试，在fastlio2中，开始一小段直线路段是正常的，但是在墙角的地方有一个不太大旋转，系统就飞了，终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外，我在另外一个室内测试场景中，也是走到墙角的地方飞掉了。此外，时间戳部分，在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s，应该也是正确的。请教一下，这可能是什么原因导致的呢？
common:
  lid_topic: "/hesai/pandar"
  imu_topic: "/alphasense/imu"
  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

preprocess:
  lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai
  scan_line: 32
  blind: 0.3

mapping:
  acc_cov: 0.0011501915187049582
  gyr_cov: 5.084312924828687e-05
  b_acc_cov: 0.06080652138668933
  b_gyr_cov: -0.0015351229643790084
  fov_degree: 180
  det_range: 150.0
  extrinsic_est_en: false      # true: enable the online estimation of IMU-LiDAR extrinsic
  extrinsic_T: [ -0.001, -0.00855, 0.055 ]
  extrinsic_R: [ 0, -1,  0,
                 -1, 0,  0,
                 0,  0, -1 ]
 double time_begin = pl_orig.points[0].timestamp;
    for (uint i = 0; i < plsize; i++) {
      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
          + pl_orig.points[i].z * pl_orig.points[i].z;
      if (range < (blind * blind)) continue;
      Eigen::Vector3d pt_vec;
      PointType added_pt;
      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.normal_x = 0;
      added_pt.normal_y = 0;
      added_pt.normal_z = 0;
      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
      if (yaw_angle >= 180.0)
        yaw_angle -= 360.0;
      if (yaw_angle <= -180.0)
        yaw_angle += 360.0;

      added_pt.curvature = pl_orig.points[i].timestamp - time_begin;

      if (pl_orig.points[i].ring < N_SCANS) {
        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
      }
    }

大哥，如果可以的话，我想您能不能将您进行修改用于适配的文件发给我学习一下，感激不尽！