使用livox horizon+内置imu建图时直接跑飞

[xxxkun]

您好， @JokerJohn ，使用livox horizon+内置imu建图时直接跑飞。请问有没有解决思路？
运行截图如下：

其中livox自定义数据转pointcloud2，代码如下：

void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
  livox_data.push_back(livox_msg_in);
  if (livox_data.size() < TO_MERGE_CNT) return;

  pcl::PointCloud<PointXYZIRT> pcl_in;

  for (size_t j = 0; j < livox_data.size(); j++) {
    auto& livox_msg = livox_data[j];
    auto time_end = livox_msg->points.back().offset_time;
    for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
      PointXYZIRT pt;
      pt.x = livox_msg->points[i].x;
      pt.y = livox_msg->points[i].y;
      pt.z = livox_msg->points[i].z;
      float s = livox_msg->points[i].offset_time / (float)time_end;
      pt.intensity = livox_msg->points[i].reflectivity;
      pt.ring = livox_msg->points[i].line;
      pt.time = livox_msg->points[i].offset_time / 10e8;
      pcl_in.push_back(pt);
    }

雷达与imu坐标关系如图：

config文件中设置如下：

# Extrinsics (lidar -> IMU)
  imuType: 0   # 0: 6axis, 1:9 axis
  extrinsicTrans: [ 0.05512, 0.02226, -0.0297 ]
  extrinsicRot: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]
  extrinsicRPY: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]

[JokerJohn]

您好， @JokerJohn ，使用livox horizon+内置imu建图时直接跑飞。请问有没有解决思路？ 运行截图如下：

其中livox自定义数据转pointcloud2，代码如下：

void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
  livox_data.push_back(livox_msg_in);
  if (livox_data.size() < TO_MERGE_CNT) return;

  pcl::PointCloud<PointXYZIRT> pcl_in;

  for (size_t j = 0; j < livox_data.size(); j++) {
    auto& livox_msg = livox_data[j];
    auto time_end = livox_msg->points.back().offset_time;
    for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
      PointXYZIRT pt;
      pt.x = livox_msg->points[i].x;
      pt.y = livox_msg->points[i].y;
      pt.z = livox_msg->points[i].z;
      float s = livox_msg->points[i].offset_time / (float)time_end;
      pt.intensity = livox_msg->points[i].reflectivity;
      pt.ring = livox_msg->points[i].line;
      pt.time = livox_msg->points[i].offset_time / 10e8;
      pcl_in.push_back(pt);
    }

雷达与imu坐标关系如图：

config文件中设置如下：

# Extrinsics (lidar -> IMU)
  imuType: 0   # 0: 6axis, 1:9 axis
  extrinsicTrans: [ 0.05512, 0.02226, -0.0297 ]
  extrinsicRot: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]
  extrinsicRPY: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]

外参看了下问题不大。点云的点的时间戳再检查下，应该要确保每个点的时间戳=相对于本帧起点的时间（pt.time = livox_msg->points[i].offset_time / 10e8;
），单位为秒。

[xxxkun]

您好， @JokerJohn ，使用livox horizon+内置imu建图时直接跑飞。请问有没有解决思路？ 运行截图如下：

其中livox自定义数据转pointcloud2，代码如下：

void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
  livox_data.push_back(livox_msg_in);
  if (livox_data.size() < TO_MERGE_CNT) return;

  pcl::PointCloud<PointXYZIRT> pcl_in;

  for (size_t j = 0; j < livox_data.size(); j++) {
    auto& livox_msg = livox_data[j];
    auto time_end = livox_msg->points.back().offset_time;
    for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
      PointXYZIRT pt;
      pt.x = livox_msg->points[i].x;
      pt.y = livox_msg->points[i].y;
      pt.z = livox_msg->points[i].z;
      float s = livox_msg->points[i].offset_time / (float)time_end;
      pt.intensity = livox_msg->points[i].reflectivity;
      pt.ring = livox_msg->points[i].line;
      pt.time = livox_msg->points[i].offset_time / 10e8;
      pcl_in.push_back(pt);
    }

雷达与imu坐标关系如图：

config文件中设置如下：

# Extrinsics (lidar -> IMU)
  imuType: 0   # 0: 6axis, 1:9 axis
  extrinsicTrans: [ 0.05512, 0.02226, -0.0297 ]
  extrinsicRot: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]
  extrinsicRPY: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]

外参看了下问题不大。点云的点的时间戳再检查下，应该要确保每个点的时间戳=相对于本帧起点的时间（pt.time = livox_msg->points[i].offset_time / 10e8; ），单位为秒。

感谢回复，关于时间戳问题，参考了livox_ros_driver中的定义：

览沃自定义数据包格式，详细说明如下 :

Header header             # ROS standard message header
uint64 timebase           # The time of first point
uint32 point_num          # Total number of pointclouds
uint8  lidar_id           # Lidar device id number
uint8[3]  rsvd            # Reserved use
CustomPoint[] points      # Pointcloud data

上述自定义数据包中的自定义点云（CustomＰoint）格式 :

uint32 offset_time      # offset time relative to the base time
float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
uint8 reflectivity      # reflectivity, 0~255
uint8 tag               # livox tag
uint8 line              # laser number in lidar

但这里没给出offset_set的单位，打印数据推断这里应该是：纳秒，故除以1e9。

另这里每个点的时间是不是仅用于单帧点云的运动畸变矫正，若屏蔽ImageProjection节点，直接对原数据进行后续操作。仍旧跑飞的话就可以排除点云数据问题，那么只剩下imu数据有问题？

[JokerJohn]

@xxxkun
1.确保pt.time的时间戳取值范围在0-0.1s即可。点的时间戳一般只用于畸变去除，对LIO非常重要，首先要确保这个。
2.如果点的时间戳正确，再检查IMU的外参（内参不重要，可以随便设置为0.1/0.01，放大或缩小10倍），确保IMU的三个轴经过旋转外参转换后与LIDAR系方向一致，并且转换后的z朝上（朝上朝下会影响到预积分函数的代码）。
3.最后就是确保数据其实阶段不要做过于剧烈的旋转运动。

[LiquoriceH]

@xxxkun 1.确保pt.time的时间戳取值范围在0-0.1s即可。点的时间戳一般只用于畸变去除，对LIO非常重要，首先要确保这个。 2.如果点的时间戳正确，再检查IMU的外参（内参不重要，可以随便设置为0.1/0.01，放大或缩小10倍），确保IMU的三个轴经过旋转外参转换后与LIDAR系方向一致，并且转换后的z朝上（朝上朝下会影响到预积分函数的代码）。 3.最后就是确保数据其实阶段不要做过于剧烈的旋转运动。

作者你好，我的mid360倒装，也就是z轴朝下，imu的z轴朝上，旋转矩阵是x轴的180度，请问这种情况下mid360倒装建图会有问题吗

[JokerJohn]

@LiquoriceH 只要外参正确设置，就没什么问题。

[LiquoriceH]

@LiquoriceH 只要外参正确设置，就没什么问题。

胡博，请问一下：

1. “并且转换后的z朝上（朝上朝下会影响到预积分函数的代码）。”这部分是什么意思呀。
2. 还有一个问题就是，param中的外参矩阵，指的是imu的姿态在lidar坐标系下的表示吗。举个例子雷达x(前)y(左)z(上)，IMU是x(右)y(前)z(上)，那么这个外参应该是z轴-90°的旋转矩阵吗

[JokerJohn]

@LiquoriceH

1. 预积分部分代码中，需要先把IMU方向转换到LiDAR坐标轴上(只是用外参将坐标轴对齐，在LiDAR坐标轴上去做积分，求解出了相对运动才去考虑外参平移)。下面这行代码初始化预积分的类，MakeSharedU，U代表z朝上，还有MakeSharedD代表z朝下，详细使用可以点进去看看代码注释。
   boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);

2.我记得是LiDAR-to-IMU的外参。在下面的IMU convert函数中，把每个IMU的加速度角速度数据转换到LiDAR坐标轴上(与LiDAR坐标系缺个平移)，然后去做积分，也没有考虑平移。参数文件里说是说外参指的是LiDAR-to-IMU的变换矩阵，使用的是左乘。 按照你的描述，我习惯这么画一下，你可以先用我这个矩阵试试坐标系对不对，不对的话取它的逆就行。

[JokerJohn]

@LiquoriceH 想起来你用的是mid360，这里旋转设置为单位矩阵就行，平移参考我下面的数据。因为你的雷达和IMU是装在一起的，动也是刚性连接整体运动。
extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]

[LiquoriceH]

@LiquoriceH 想起来你用的是mid360，这里旋转设置为单位矩阵就行，平移参考我下面的数据。因为你的雷达和IMU是装在一起的，动也是刚性连接整体运动。 extrinsic_T: [ -0.011, -0.02329, 0.04412 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1]

胡博，根据你写的应该是-90°的矩阵。我用的是外置的IMU，MID360的imu数据没有四元数。可以加个vx嘛胡博😀