LOAM laserOdom

该文描述了laserOdom节点如何处理点云数据,通过点云匹配计算位姿估计,并采用插值方法去除匀速运动导致的畸变。TransformToStart和TransformToEnd函数分别用于将点转换到起点和终点姿态,以消除运动影响,最终转换到对应时刻的IMU坐标系。
摘要由CSDN通过智能技术生成

laserOdom

该节点主要接收scanRegistration的corner Point和surf Point,当前帧与上一帧做点云匹配,计算估计位姿,lsaerOdom。

去畸变(匀速运动产生的畸变)

坐标转换关键是下面的两个函数,论文中的公式:

T ( k + 1 , i ) L = t i − t k + 1 t − t k + 1 T k + 1 L T_{(k+1,i)}^{L}=\frac{t_{i}-t_{k+1}}{t-t_{k+1}}T_{k+1}^{L} T(k+1,i)L=ttk+1titk+1Tk+1L

  • TransformToStart
  • TransformToEnd
    个人理解,toStart,这里进行插值,将点转换到第一个点的位姿。
    toEnd,将点转换到最后一个点的姿态,然后转到开始时刻的世界坐标系,最后转到最后时刻IMU的相对位姿坐标系下。具体大家仔细看看代码,其实我自己也有点迷糊。
//当前点云中的点相对第一个点去除因匀速运动产生的畸变,效果相当于得到在点云扫描开始位置静止扫描得到的点云
void TransformToStart(PointType const * const pi, PointType * const po)
{
  //插值系数计算,云中每个点的相对时间/点云周期10
  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)
  po->x = cos(ry) * x2 - sin(ry) * z2;
  po->y = y2;
  po->z = sin(ry) * x2 + cos(ry) * z2;
  po->intensity = pi->intensity;
}

//将上一帧点云中的点相对结束位置去除因匀速运动产生的畸变,效果相当于得到在点云扫描结束位置静止扫描得到的点云
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;

  //绕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);
}

优化部分

优化部分与laserMapping部分的优化部分重叠。集中一起写。

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值