LOAM 融合 IMU 细节之 TransformToEnd 函数-CSDN博客

本文链接：https://blog.csdn.net/weixin_43959160/article/details/123743694

LOAM 融合 IMU 细节之 TransformToEnd 函数

啃个老骨头, 一个几乎要被我忘记的问题, 今天突然灵机一动有了思路, 仔细一追溯, 一不小心就理清楚了

TransformToEnd 问题追溯

void TransformToEnd(PointType const * const pi, PointType * const po)
{
  //插值系数计算
  float s = 10 * (pi->intensity - int(pi->intensity));

  float rx = s * transform[0];
  float ry = s * transform[1];
  float rz = s * transform[2];
  float tx = s * transform[3];
  float ty = s * transform[4];
  float tz = s * transform[5];

  //平移后绕z轴旋转（-rz）
  float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
  float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
  float z1 = (pi->z - tz);

  //绕x轴旋转（-rx）
  float x2 = x1;
  float y2 = cos(rx) * y1 + sin(rx) * z1;
  float z2 = -sin(rx) * y1 + cos(rx) * z1;

  //绕y轴旋转（-ry）
  float x3 = cos(ry) * x2 - sin(ry) * z2;
  float y3 = y2;
  float z3 = sin(ry) * x2 + cos(ry) * z2;//求出了相对于起始点校正的坐标

  rx = transform[0];
  ry = transform[1];
  rz = transform[2];
  tx = transform[3];
  ty = transform[4];
  tz = transform[5];

  // *********根据插值比例将点转换到点云第一个点所在坐标系
  // 绕y轴旋转（ry）
  float x4 = cos(ry) * x3 + sin(ry) * z3;
  float y4 = y3;
  float z4 = -sin(ry) * x3 + cos(ry) * z3;

  // 绕x轴旋转（rx）
  float x5 = x4;
  float y5 = cos(rx) * y4 - sin(rx) * z4;
  float z5 = sin(rx) * y4 + cos(rx) * z4;

  // 绕z轴旋转（rz），再平移
  float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;
  float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;
  float z6 = z5 + tz;
  // *********截至到这里转换到了点云第一个点

  // 由于加减速运动只考虑平移,不考虑旋转,故减掉因加减速产生的后, 得到的坐标是匀速运动
  // 后点的坐标, 匀速运动不涉及旋转,故此时认为点的角度为点云起点的角度
  
  // *********** 去除非匀速直线运动畸变, 然后转换到IMU世界坐标系起点
  // 平移后绕z轴旋转（imuRollStart）
  float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) 
           - sin(imuRollStart) * (y6 - imuShiftFromStartY);
  float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) 
           + cos(imuRollStart) * (y6 - imuShiftFromStartY);
  float z7 = z6 - imuShiftFromStartZ;

  //绕x轴旋转（imuPitchStart）
  float x8 = x7;
  float y8 = cos(imuPitchStart) * y7 - sin(imuPitchStart) * z7;
  float z8 = sin(imuPitchStart) * y7 + cos(imuPitchStart) * z7;

  //绕y轴旋转（imuYawStart）
  float x9 = cos(imuYawStart) * x8 + sin(imuYawStart) * z8;
  float y9 = y8;
  float z9 = -sin(imuYawStart) * x8 + cos(imuYawStart) * z8;
  // *********** 已转换到IMU世界坐标系起点
  
  // *********** 转换到点云最后一个点所在的坐标系
  //绕y轴旋转（-imuYawLast）
  float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;
  float y10 = y9;
  float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;

  //绕x轴旋转（-imuPitchLast）
  float x11 = x10;
  float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;
  float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;

  //绕z轴旋转（-imuRollLast）
  po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;
  po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;
  po->z = z11;
  // *********** 已转换到点云最后一个点所在的坐标系
  
  
  //只保留线号
  po->intensity = int(pi->intensity);
}

关于这个函数, 其中转来转去一共转了四次, 很长一段时间我都处于懵圈的状态, 知其然而不知其所以然. 其实这块理清楚了也并不难, 关键在与有没有注意到去除非匀速运动畸变的部分, 即第三次旋转之初所做的去畸变平移.

追溯到点云配准模块中, 这里的非匀速畸变来源如下:

imuShiftFromStartX, imuShiftFromStartY, imuShiftFromStartZ 数据来自订阅消息 “/imu_trans”

void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2)
{
  timeImuTrans = imuTrans2->header.stamp.toSec();
  ...

  imuShiftFromStartX = imuTrans->points[2].x;
  imuShiftFromStartY = imuTrans->points[2].y;
  imuShiftFromStartZ = imuTrans->points[2].z;

  ...
}

点云配准模块中该数据来自于imuShiftFromStartXCur等, 代码如下:

imuTrans.points[2].x = imuShiftFromStartXCur;
imuTrans.points[2].y = imuShiftFromStartYCur;
imuTrans.points[2].z = imuShiftFromStartZCur;

imuShiftFromStartXCur等的值来自 ShiftToStartIMU 函数, 代码如下:
```
void ShiftToStartIMU(float pointTime){
  //imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
  imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
  imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
  imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;

... ...
}
```
问题的关键就在这里, 从这里代码我们可以看到, imuShiftFromStartCur实际求得的畸变量, 指的是除了匀速直线运动之外的所有运动带来的畸变,这里面包括匀速和非匀速旋转畸变和加减速直线运动畸变, 也就是说, 去除这个畸变量, 就是去除了加减速运动量, 也同时去除了旋转量.
看到这里, 我们的问题就能说得通了.

问题解决, 最终解释

在使用优化方法得到当前帧点云和上一帧点云之间的变换后, 对其进行线性插值估计出该点相对于第一个点的运动畸变量.
然后据此转换到第一个点所在坐标系, 再使用该变换将其转换到该帧点云最后一个点坐在坐标系
但是由于前面的线性插值估计方式并不准确, 只有其中的线性部分是相对精确的
因此, 针对非线性部分, 我们引入IMU记录相对精确的值进行校准
先去除非线性畸变, 这时点处于点云起点坐标系下, 然后使用IMU数据将其重新转换到点云终点, 其转换方式为先转到IMU世界坐标系原点, 再转到IMU记录的点云最后一个点所在坐标系