《激光-视觉-IMU-GPS融合SLAM算法》笔记（二）

激光-惯性子系统（LIS）代码精读
SLAM

1.用的LVI-SAM的代码，首先分析的是激光-IMU部分，即激光里程计lidar_odometry包。这个包还可以自行添加融合GPS进一步优化。

2.imageProjection.cpp
去畸变代码在cloudHandler函数中，cachePointCloud具体先找IMU和里程计的数据,deskewInfo()函数计算畸变信息，projectPointCloud将每个点计算行列投影到矩阵中，并且投影的过程中对每个点去畸变，最终具体执行完成在deskewPoint()；cloudExtraction把点塞到数据结构里（可给到featureExtraction.cpp提角点和面点特征），最终publishClouds,然后resetParameters()重设参数.旋转引起的畸变远大于平移引起的畸变。

3.featureExtraction.cpp提角点和面点特征，具体在laserCloudInfoHandler,其中calculateSmoothness为计算曲率，然后markOccludedPoints标记一些不好的点（如与入射线几乎平行的斜面上的点或易受遮挡的点），之后extractFeatures提取角点和面点特征，最后PUB给mapping线程进行匹配（即转到mapOptimization.cpp)。

4.mapOptimization.cpp
updateInitialGuess为预测得到初始pose；
extractSurroundingKeyFrames得到降采样后的局部角点地图和局部面点地图；
downsampleCurrentScan对当前观测到的角点和面点进行降采样，使得它的分辨率和上一步得到的地图的分辨率一致；
scan2MapOptimazation为用当前pose预测值与地图进行角点和面点匹配，角点匹配得到直线的参数，面点匹配得到平面的参数，用LM算法进行优化；