Lego-Loam TransformToStartIMU TransformToStart TransformToEnd的区别
Lego-Loam中FeatureAssociation中，有三处点云去畸变的操作。为什么要对点云去畸变？因为雷达是在运动，所以每帧点云最后一个点的参考坐标系与该帧第一个点的点云坐标系不重合。所以需要，将点云统一到同一个参考系中。其实，点云两帧之间的位姿变换，就是求每帧点云点云最后一个点的参考坐标系与该帧的第一个点的参考坐标系之间的位姿变换。因为，每帧最后一个点的参考坐标系，就是下一帧的第一个点的参考坐标系。Lego-Loam中是以每帧的最后一个点的参考坐标系作为该帧的参考坐标系。
TransformToStartIMU
void TransformToStartIMU(PointType *p){float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;float x4 = cosImuYawStart * x3 - sinImuYawStart * z3;float y4 = y3;float z4 = sinImuYawStart * x3 + cosImuYawStart * z3;float x5 = x4;float y5 = cosImuPitchStart * y4 + sinImuPitchStart * z4;float z5 = -sinImuPitchStart * y4 + cosImuPitchStart * z4;p->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;}
TransformToStartIMU是通过IMU的姿态变换对雷达点云进行旋转去畸变，这里并没有对雷达的位移进行去畸变。我猜可能是IMU的加速度积分漂移比较大，对位移去畸变可能误差比之前还大的原因。这个函数就是将每帧中每个点的参考坐标系的旋转姿态都转换为该帧第一个点的参考坐标系的旋转姿态。
- TransformToStart
void TransformToStart(PointType const * const pi, PointType * const po){float s = 10 * (pi->intensity - int(pi->intensity));float rx = s * transformCur[0];float ry = s * transformCur[1];float rz = s * transformCur[2];float tx = s * transformCur[3];float ty = s * transformCur[4];float tz = s * transformCur[5];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);float x2 = x1;float y2 = cos(rx) * y1 + sin(rx) * z1;float z2 = -sin(rx) * y1 + cos(rx) * z1;po->x = cos(ry) * x2 - sin(ry) * z2;po->y = y2;po->z = sin(ry) * x2 + cos(ry) * z2;po->intensity = pi->intensity;}
这个函数的作用是将每个点云都转换到该帧第一个点的参考坐标系下，包括位移和旋转。假设当前帧是第k+1帧，这里里面transformCur表示第k帧与第k-1帧之间的位姿变换。这里作者应该是做了一个匀速运动假设，即k+1帧与第k帧之间的位姿变换，等于第k帧与第k-1帧之间的位姿变换。根据前面介绍，k+1帧与第k帧之间的位姿变换，就是k+1帧最后一个点的参考坐标系与第一个点的参考坐标系之间的位姿变换。因此就可以利用transformCur来做插值，将每个点都转换到第一帧下面。
为什么作者要将每个点都转换到第一个点下面？
主要是为了做数据关联，因为k+1帧第一个点的参考坐标系，就是第k帧最后一个点的参考坐标系。Lego-Loam是以每帧的最后一个点的参考坐标系作为该帧的参考坐标系。所以就可以将k+1帧点云与k帧点云对齐，这样就可以更好匹配两帧之间的角特征点和线特征点。
不过作者在做位姿估计的时候，并没有将k+1帧点云都转换到k+1帧最后一个点的参考坐标系下。因为你要算k+1帧最后一个点的参考坐标系与k帧最后一个点的参考坐标系之间的位姿变换，将k+1帧点云都转换到k+1帧最后一个点的参考坐标系下才更精准。源码里面用的还是TransformToStartIMU去畸变后的点云。
3.TransformToEnd
void TransformToEnd(PointType const * const pi, PointType * const po){float s = 10 * (pi->intensity - int(pi->intensity));float rx = s * transformCur[0];float ry = s * transformCur[1];float rz = s * transformCur[2];float tx = s * transformCur[3];float ty = s * transformCur[4];float tz = s * transformCur[5];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);float x2 = x1;float y2 = cos(rx) * y1 + sin(rx) * z1;float z2 = -sin(rx) * y1 + cos(rx) * z1;float x3 = cos(ry) * x2 - sin(ry) * z2;float y3 = y2;float z3 = sin(ry) * x2 + cos(ry) * z2;rx = transformCur[0];ry = transformCur[1];rz = transformCur[2];tx = transformCur[3];ty = transformCur[4];tz = transformCur[5];float x4 = cos(ry) * x3 + sin(ry) * z3;float y4 = y3;float z4 = -sin(ry) * x3 + cos(ry) * z3;float x5 = x4;float y5 = cos(rx) * y4 - sin(rx) * z4;float z5 = sin(rx) * y4 + cos(rx) * z4;float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;float z6 = z5 + tz;float x7 = cosImuRollStart * (x6 - imuShiftFromStartX) - sinImuRollStart * (y6 - imuShiftFromStartY);float y7 = sinImuRollStart * (x6 - imuShiftFromStartX) + cosImuRollStart * (y6 - imuShiftFromStartY);float z7 = z6 - imuShiftFromStartZ;float x8 = x7;float y8 = cosImuPitchStart * y7 - sinImuPitchStart * z7;float z8 = sinImuPitchStart * y7 + cosImuPitchStart * z7;float x9 = cosImuYawStart * x8 + sinImuYawStart * z8;float y9 = y8;float z9 = -sinImuYawStart * x8 + cosImuYawStart * z8;float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;float y10 = y9;float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;float x11 = x10;float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;po->z = z11;po->intensity = int(pi->intensity);}
假设当前是第k+1帧，这里的transformCur表示的是第k+1帧与k帧之间的位姿变换，即k+1帧最后一个点的参考坐标系与第一个点的参考坐标系之间的位姿变换。因为用每帧的最后一个点的参考坐标系作为该帧的参考坐标系，所以
TransformToEnd就是将每个点都转换到最后一个点的参考坐标系下。