论文理论部分
特征提取不是从原始点云中进行提取，而是从点云分割中分割出的地面点和分割点中进行提取。参考：LeGO-LOAM论文翻译（内容精简)
过程如下：
只看核心理论部分还是很好理解的。总体流程：（特征提取部分）订阅传感器数据->运动畸变去除->曲率计算->去除瑕点->提取边、平面特征->发布特征点云；（特征关联部分）将IMU信息作为先验->根据线特征、面特征计算位姿变换->联合IMU数据进行位姿更新->发布里程计信息
下面来看看源码，剖析下该部分所用到的方法。
源码部分
首先看main函数，结构比较简单，要注意的是处理函数并没有以回调函数的方式运行，而是以一定的频率在循环中运行。
ros::Rate rate(200)表示循环的频率，
ros::spinOnce()与ros::spin()不同，ros::spinOnce()仅仅运行一次。
rate.sleep()为了控制时间对齐。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");
FeatureAssociation FA;
ros::Rate rate(200);
while (ros::ok())
{
ros::spinOnce();
FA.runFeatureAssociation();
rate.sleep();
}
ros::spin();
return 0;
}
查看 FeatureAssociation的私有对象以及变量定义（参考LeGO-LOAM源码解析3： featureAssociation（一））：
订阅者：
ros::NodeHandle nh;
ros::Subscriber subLaserCloud;
ros::Subscriber subLaserCloudInfo;
ros::Subscriber subOutlierCloud;
ros::Subscriber subImu;
点云发布者：
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
pcl储存变量：
pcl::PointCloud<PointType>::Ptr segmentedCloud;
pcl::PointCloud<PointType>::Ptr outlierCloud;
pcl::PointCloud<PointType>::Ptr cornerPointsSharp;
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp;
pcl::PointCloud<PointType>::Ptr surfPointsFlat;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
定义相关时间：
double timeScanCur;
double timeNewSegmentedCloud;
double timeNewSegmentedCloudInfo;
double timeNewOutlierCloud;
接受点云的标志：
bool newSegmentedCloud;
bool newSegmentedCloudInfo;
bool newOutlierCloud;
定义时间戳以及系统初始化标志：
cloud_msgs::cloud_info segInfo;
std_msgs::Header cloudHeader;
int systemInitCount;
bool systemInited;
点云平滑度及相关标签定义：
std::vector<smoothness_t> cloudSmoothness;
float cloudCurvature[N_SCAN*Horizon_SCAN];
int cloudNeighborPicked[N_SCAN*Horizon_SCAN];
int cloudLabel[N_SCAN*Horizon_SCAN];
imu相关数据：
int imuPointerFront;
int imuPointerLast;
int imuPointerLastIteration;
float imuRollStart, imuPitchStart, imuYawStart;
float cosImuRollStart, cosImuPitchStart, cosImuYawStart, sinImuRollStart, sinImuPitchStart, sinImuYawStart;
float imuRollCur, imuPitchCur, imuYawCur;
float imuVeloXStart, imuVeloYStart, imuVeloZStart;
float imuShiftXStart, imuShiftYStart, imuShiftZStart;
float imuVeloXCur, imuVeloYCur, imuVeloZCur;
float imuShiftXCur, imuShiftYCur, imuShiftZCur;
float imuShiftFromStartXCur, imuShiftFromStartYCur, imuShiftFromStartZCur;
float imuVeloFromStartXCur, imuVeloFromStartYCur, imuVeloFromStartZCur;
float imuAngularRotationXCur, imuAngularRotationYCur, imuAngularRotationZCur;
float imuAngularRotationXLast, imuAngularRotationYLast, imuAngularRotationZLast;
float imuAngularFromStartX, imuAngularFromStartY, imuAngularFromStartZ;
//旋转角
double imuTime[imuQueLength];
float imuRoll[imuQueLength];
float imuPitch[imuQueLength];
float imuYaw[imuQueLength];
//加速度
float imuAccX[imuQueLength];
float imuAccY[imuQueLength];
float imuAccZ[imuQueLength];
//速度
float imuVeloX[imuQueLength];
float imuVeloY[imuQueLength];
float imuVeloZ[imuQueLength];
//偏移量
float imuShiftX[imuQueLength];
float imuShiftY[imuQueLength];
float imuShiftZ[imuQueLength];
//角速度
float imuAngularVeloX[imuQueLength];
float imuAngularVeloY[imuQueLength];
float imuAngularVeloZ[imuQueLength];
//角度
float imuAngularRotationX[imuQueLength];
float imuAngularRotationY[imuQueLength];
float imuAngularRotationZ[imuQueLength];
里程计相关发布器：
ros::Publisher pubLaserCloudCornerLast;
ros::Publisher pubLaserCloudSurfLast;
ros::Publisher pubLaserOdometry;
ros::Publisher pubOutlierCloudLast;
int skipFrameNum;
bool systemInitedLM;
int laserCloudCornerLastNum;
int laserCloudSurfLastNum;
int pointSelCornerInd[N_SCAN*Horizon_SCAN];
float pointSearchCornerInd1[N_SCAN*Horizon_SCAN];
float pointSearchCornerInd2[N_SCAN*Horizon_SCAN];
int pointSelSurfInd[N_SCAN*Horizon_SCAN];
float pointSearchSurfInd1[N_SCAN*Horizon_SCAN];
float pointSearchSurfInd2[N_SCAN*Horizon_SCAN];
float pointSearchSurfInd3[N_SCAN*Horizon_SCAN];
float transformCur[6];
float transformSum[6];
float imuRollLast, imuPitchLast, imuYawLast;
float imuShiftFromStartX, imuShiftFromStartY, imuShiftFromStartZ;
float imuVeloFromStartX, imuVeloFromStartY, imuVeloFromStartZ;
保存上帧点云的定义：
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast;
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast;
里程计和tf发布：
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff;
nav_msgs::Odometry laserOdometry;
tf::TransformBroadcaster tfBroadcaster;
tf::StampedTransform laserOdometryTrans;
退化相关：
bool isDegenerate;
cv::Mat matP;
int frameCount;
了解完一些变量后，看一下消息回调。
segmented_cloud 分割好的点云（分块点云：包括地面点和分割点）。
segmented_cloud_info 分割好的点云，和segmented_cloud的区别是，cloud_info的强度信息是距离，而segmented_cloud的是range image的行列信息。
outlier_cloud 离群点，也就是分割失败的点（界外点）。
imuTopic 接收imu的消息。
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
三个点云的回调主要是将ros通信转化为pcl点云信息
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
cloudHeader = laserCloudMsg->header;
timeScanCur = cloudHeader.stamp.toSec();
timeNewSegmentedCloud = timeScanCur;
segmentedCloud->clear();
pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);
newSegmentedCloud = true;
}
// 注意这里没有转换为PCL点云，而是直接保存的消息cloud_msgs::cloud_info segInfo
void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn)
{
timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
segInfo = *msgIn;
newSegmentedCloudInfo = true;
}
void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){
timeNewOutlierCloud = msgIn->header.stamp.toSec();
outlierCloud->clear();
pcl::fromROSMsg(*msgIn, *outlierCloud);
newOutlierCloud = true;
}
接下来看IMU回调函数，接收到IMU消息之后保存到数组。IMU还做了一些角度的转换
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
// 加速度去除重力影响，同时坐标轴进行变换
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;
AccumulateIMUShiftAndRotation();
}
上面首先是对重力加速度的去除，然后进行坐标转化。这块我也是懵逼了好久，后来慢慢看了一下三维旋转理解了。
防止文章过长，不知道原因的可以去看我的这篇博客：Logo-loam中imu消除重力影响
知道的可以跳过。
坐标转换主要过程是首先将重力加速度转换到当前imu坐标系，然后imu加速度坐标转换到相机坐标系上。主要是因为相机坐标系和imu坐标系并不统一，然后将欧拉角，加速度，速度保存到循环队列中；AccumulateIMUShiftAndRotation()对速度，角速度，加速度进行积分，得到位移，角度和速度；
赋值操作：
void AccumulateIMUShiftAndRotation()
{
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
经过上面的处理Imu数据已经变换到相机坐标系，下面首先要将数据变换到世界坐标系，也就是相机初始坐标系。需要注意的是这里的角度还是原来坐标系的角度，也就是说新坐标系的roll, pitch, yaw并不对应着数据上的roll, pitch, yaw（原坐标系里的roll, pitch, yaw），因为上一步赋值的时候没有变换roll, pitch, yaw，这个理解了后面的坐标转换就会迎刃而解。
绕z轴旋转（原来的x轴），注意看角度：
// 先绕Z轴(原x轴)旋转,下方坐标系示意imuHandler()中加速度的坐标轴交换
// z->Y
// ^
// | ^ y->X
// | /
// | /
// | /
// -----> x->Z
//
// |cosrz -sinrz 0|
// Rz=|sinrz cosrz 0|
// |0 0 1|
// [x1,y1,z1]^T=Rz*[accX,accY,accZ]
// 因为在imuHandler中进行过坐标变换，
// 所以下面的roll其实已经对应于新坐标系中(X-Y-Z)的yaw
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
绕x轴旋转（原来的y轴），注意看角度
// 绕X轴(原y轴)旋转
// [x2,y2,z2]^T=Rx*[x1,y1,z1]
// |1 0 0|
// Rx=|0 cosrx -sinrx|
// |0 sinrx cosrx|
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
绕y轴旋转（原来的z轴），注意看角度:
// 最后再绕Y轴(原z轴)旋转
// |cosry 0 sinry|
// Ry=|0 1 0|
// |-sinry 0 cosry|
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
计算位移，速度，角度：
// 进行位移，速度，角度量的累加
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
if (timeDiff < scanPeriod) {
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;
imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;
imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;
}
}
回到主函数中，主体函数是runFeatureAssociation()，紧接着下面查看该函数的结构以及功能，新数据指的是点云分割随后发布的三种数据，确保它们是同一时间的数据后才进行下一步的处理。
主要过程为：
1、新数据进来进行坐标转换和插补等工作.
2、进行光滑度计算。
3、标记阻塞点。
4、特征抽取。
5、发布四种点云信息。
6、预测位姿，
7、更新变换位姿。
8、积分总变换。
9、发布里程计及上一帧点云信息。
1、新数据进来进行坐标转换和插补等工作.
void runFeatureAssociation()
{
// 如果有新数据进来则执行，否则不执行任何操作
if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){
newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
}else{
return;
}
// 主要进行的处理是将点云数据进行坐标变换，进行插补等工作
adjustDistortion();
// 不完全按照公式进行光滑性计算，并保存结果
calculateSmoothness();
// 标记阻塞点??? 阻塞点是什么点???
// 参考了csdn若愚maimai大佬的博客，这里的阻塞点指过近的点
// 指在点云中可能出现的互相遮挡的情况
markOccludedPoints();
// 特征抽取，然后分别保存到cornerPointsSharp等等队列中去
// 保存到不同的队列是不同类型的点云，进行了标记的工作，
// 这一步中减少了点云数量，使计算量减少
extractFeatures();
// 发布cornerPointsSharp等4种类型的点云数据
publishCloud();
if (!systemInitedLM) {
checkSystemInitialization();
return;
}
// 预测位姿
updateInitialGuess();
// 更新变换
updateTransformation();
// 积分总变换
integrateTransformation();
publishOdometry();
publishCloudsLast();
}
};
如果不是新数据或者数据不同步则不进行处理，直接返回。
如果是新数据，将数据标志记为0，顺序执行操作。
首先是adjustDistortion()函数处理点云数据的坐标转换、插补等工作。
遍历点云坐标转换：
void adjustDistortion()
{
bool halfPassed = false;
int cloudSize = segmentedCloud->points.size();
PointType point;
for (int i = 0; i < cloudSize; i++) {
// 这里xyz与laboshin_loam代码中的一样经过坐标轴变换
// imuhandler() 中相同的坐标变换
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
针对每个点计算偏航角yaw：
// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw，因为前面有负号，ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI)，在2PI附近
// 下面过程的主要作用是调整ori大小，满足start<ori<end
float ori = -atan2(point.x, point.z);
对角度进行矫正，和点云分割中的操作类似。
四种情况：
没有转过一半，但是start-ori>M_PI/2
没有转过一半，但是ori-start>3/2M_PI,说明ori太大，不合理（正常情况在前半圈的话，ori-start范围[0,M_PI]）
转过一半，end-ori>3/2PI,ori太小
转过一半，ori-end>M_PI/2,太大
if (!halfPassed) {
if (ori < segInfo.startOrientation - M_PI / 2)
// start-ori>M_PI/2，说明ori小于start，不合理，
// 正常情况在前半圈的话，ori-stat范围[0,M_PI]
ori += 2 * M_PI;
else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
// ori-start>3/2*M_PI,说明ori太大，不合理
ori -= 2 * M_PI;
if (ori - segInfo.startOrientation > M_PI)
halfPassed = true;
} else {
ori += 2 * M_PI;
if (ori < segInfo.endOrientation - M_PI * 3 / 2)
// end-ori>3/2*PI,ori太小
ori += 2 * M_PI;
else if (ori > segInfo.endOrientation + M_PI / 2)
// ori-end>M_PI/2,太大
ori -= 2 * M_PI;
}
然后进行imu数据与激光数据的时间轴对齐操作。我不是很理解，可以看看这篇博客LeGo-LOAM源码阅读笔记(三)—featureAssociation.cpp
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
imuPointerFront = imuPointerLastIteration;
// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
// 该条件内imu数据比激光数据早，但是没有更后面的数据
// (打个比方,激光在9点时出现，imu现在只有8点的)
// 这种情况上面while循环是以imuPointerFront == imuPointerLast结束的
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
} else {
// 在imu数据充足的情况下可以进行插补
// 当前timeScanCur + pointTime < imuTime[imuPointerFront]，
// 而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
// 通过上面计算的ratioFront以及ratioBack进行插补
// 因为imuRollCur和imuPitchCur通常都在0度左右，变化不会很大，因此不需要考虑超过2M_PI的情况
// imuYaw转的角度比较大，需要考虑超过2*M_PI的情况
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
// imu速度插补
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
// imu位置插补
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
if (i == 0) {
// 此处更新过的角度值主要用在updateImuRollPitchYawStartSinCos()中,
// 更新每个角的正余弦值
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
// 该条件内imu数据比激光数据早，但是没有更后面的数据
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
}else{
// 在imu数据充足的情况下可以进行插补
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;
}
// 距离上一次插补，旋转过的角度变化值
imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;
imuAngularRotationXLast = imuAngularRotationXCur;
imuAngularRotationYLast = imuAngularRotationYCur;
imuAngularRotationZLast = imuAngularRotationZCur;
// 这里更新的是i=0时刻的rpy角，后面将速度坐标投影过来会用到i=0时刻的值
updateImuRollPitchYawStartSinCos();
} else {
// 速度投影到初始i=0时刻
VeloToStartIMU();
// 将点的坐标变换到初始i=0时刻
TransformToStartIMU(&point);
}
}
以上插值过程就是根据imuPointerFront和imuPointerBack对imu数据进行插值。然后将相对速度数据从世界坐标系转换到当前坐标系，然后在坐标转换到最后统一的激光坐标系。不懂得可以参考：LeGO-LOAM源码解析4： featureAssociation（二）
更新imu开始角度正余弦指：
void updateImuRollPitchYawStartSinCos(){
cosImuRollStart = cos(imuRollStart);
cosImuPitchStart = cos(imuPitchStart);
cosImuYawStart = cos(imuYawStart);
sinImuRollStart = sin(imuRollStart);
sinImuPitchStart = sin(imuPitchStart);
sinImuYawStart = sin(imuYawStart);
}
更新imu初始数据，此时i=0，也就是将imu的本来的世界坐标系（i=0的初始坐标系就是世界坐标系）更新到新的世界坐标系（插值的新的初始角度，这个时间在i=0之前）：
void VeloToStartIMU()
{
// imuVeloXStart,imuVeloYStart,imuVeloZStart是点云索引i=0时刻的速度
// 此处计算的是相对于初始时刻i=0时的相对速度
// 这个相对速度在世界坐标系下
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
// ！！！下面从世界坐标系转换到start坐标系，roll,pitch,yaw要取负值
// 首先绕y轴进行旋转
// |cosry 0 sinry|
// Ry=|0 1 0|
// |-sinry 0 cosry|
float x1 = cosImuYawStart * imuVeloFromStartXCur - sinImuYawStart * imuVeloFromStartZCur;
float y1 = imuVeloFromStartYCur;
float z1 = sinImuYawStart * imuVeloFromStartXCur + cosImuYawStart * imuVeloFromStartZCur;
// 绕当前x轴旋转(-pitch)的角度
// |1 0 0|
// Rx=|0 cosrx -sinrx|
// |0 sinrx cosrx|
float x2 = x1;
float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;
float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;
// 绕当前z轴旋转(-roll)的角度
// |cosrz -sinrz 0|
// Rz=|sinrz cosrz 0|
// |0 0 1|
imuVeloFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;
imuVeloFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;
imuVeloFromStartZCur = z2;
}
将所有点云数据转换到imu的新的初始时刻，首先变换到世界坐标系（i=0）：
void TransformToStartIMU(PointType *p)
{
// 因为在adjustDistortion函数中有对xyz的坐标进行交换的过程
// 交换的过程是x=原来的y，y=原来的z，z=原来的x
// 所以下面其实是绕Z轴(原先的x轴)旋转，对应的是roll角
//
// |cosrz -sinrz 0|
// Rz=|sinrz cosrz 0|
// |0 0 1|
// [x1,y1,z1]^T=Rz*[x,y,z]
//
// 因为在imuHandler中进行过坐标变换，
// 所以下面的roll其实已经对应于新坐标系中(X-Y-Z)的yaw
float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;
float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
float z1 = p->z;
// 绕X轴(原先的y轴)旋转
//
// [x2,y2,z2]^T=Rx*[x1,y1,z1]
// |1 0 0|
// Rx=|0 cosrx -sinrx|
// |0 sinrx cosrx|
float x2 = x1;
float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;
// 最后再绕Y轴(原先的Z轴)旋转
// |cosry 0 sinry|
// Ry=|0 1 0|
// |-sinry 0 cosry|
float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
float y3 = y2;
float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;
然后变换到新的start坐标系：
// 下面部分的代码功能是从imu坐标的原点变换到i=0时imu的初始时刻(从世界坐标系变换到start坐标系)
// 变换方式和函数VeloToStartIMU()中的类似
// 变换顺序：Cur-->世界坐标系-->Start，这两次变换中，
// 前一次是正变换，角度为正，后一次是逆变换，角度应该为负
// 可以参考：
// https://blog.csdn.net/wykxwyc/article/details/101712524
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;
// 绕z轴(原先的x轴)变换角度到初始imu时刻，另外需要加上imu的位移漂移
// 后面加上的 imuShiftFromStart.. 表示从start时刻到cur时刻的漂移，
// (imuShiftFromStart.. 在start坐标系下)
p->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;
p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;
p->z = z5 + imuShiftFromStartZCur;
}
以上坐标变换以及插值工作完成。
以下工作下一篇文章再分享：
2、进行光滑度计算
3、标记阻塞点。
4、特征抽取。
5、发布四种点云信息。
6、预测位姿，
7、更新变换位姿。
8、积分总变换。
9、发布里程计及上一帧点云信息。
参考：
LeGO-LOAM论文翻译（内容精简)
Lego-LOAM IMU坐标系变换的详细记录
Logo-loam中imu消除重力影响
LeGo-LOAM源码阅读笔记(三)—featureAssociation.cpp
LeGo-LOAM源码篇:源码分析(2)
LeGO-LOAM源码解析3： featureAssociation（一）
loam源码解析1 ： scanRegistration（一）
LeGO-LOAM源码解析4： featureAssociation（二）