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=t−tk+1ti−tk+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部分的优化部分重叠。集中一起写。