【3D激光SLAM】LOAM源代码解析--laserMapping.cpp

系列文章目录

·【3D激光SLAM】LOAM源代码解析–.cpp

·【3D激光SLAM】LOAM源代码解析–.cpp

·【3D激光SLAM】LOAM源代码解析–.cpp

·【3D激光SLAM】LOAM源代码解析–.cpp

写在前面

本系列文章将对LOAM源代码进行讲解，在讲解过程中，涉及到论文中提到的部分，会结合论文以及我自己的理解进行解读，尤其是对于其中坐标变换的部分，将会进行详细的讲解。

本来是懒得写的，一个是怕自己以后忘了，另外是我在学习过程中，其实没有感觉哪一个博主能讲解的通篇都能让我很明白，特别是坐标变换部分的代码，所以想着自己学完之后，按照自己的理解，也写一个LOAM解读，希望能对后续学习LOAM的同学们有所帮助。

之后也打算录一个LOAM讲解的视频，大家可以关注一下。

文章目录 七、高斯牛顿优化与退化处理 八、将当前帧的特征点加入到地图中九、话题发布总结

整体框架

LOAM多牛逼就不用多说了，直接开始

先贴一下我详细注释的LOAM代码，在这个版本的代码上加入了我自己的理解。

我觉得最重要也是最恶心的一部分是其中的坐标变换，在代码里面真的看着头大，所以先明确一下坐标系(都是右手坐标系)：

坐标变换约定： 为了清晰，变换矩阵的形式与《SLAM十四讲中一样》，即： R A _ B R_{A\_B} RA_B​表示B坐标系相对于A坐标系的变换，B中一个向量通过 R A _ B R_{A\_B} RA_B​可以变换到A中的向量。

首先对照ros的节点图和论文中提到的算法框架来看一下：

可以看到节点图和论文中的框架是一一对应的，这几个模块的功能如下：

本文介绍模块，它相当于SLAM的后端，它维护了一个局部地图，它相当于一个scan-to-map的匹配过程，它的具体功能如下：

接收特征点话题、全部点云话题、IMU话题，并保存到对应的变量中将当前sweep与上几次sweep(局部地图)进行特征匹配，得到edge point在局部子图中KD-tree查找到的5个最近点以及 point在局部子图KD-tree查找到的5个最近点计算edge point对应5个点构成的均值和协方差矩阵，对协方差矩阵进行分解，求解匹配到的直线参数用 point对应5个点构建一个超定方程组，求解得到平面参数构建雅可比矩阵和残差项，求解优化后的位姿变换将当前帧的点云插入局部子图，维护一个局部地图，用于下一帧的匹配发布话题并更新tf变换 一、变量含义

首先，介绍一下本程序用到变量的含义，理解这一部分非常重要：

一些理解：虽然[6]我上面写的是 T m a p _ e n d T_{map\_end} ​，看起来好像是把坐标系换成了map坐标系，但是我觉得这里有两种理解都可以：

可以理解为经过模块优化后的里程计坐标系下的当前帧end相对于初始帧的坐标变换也可以理解为经过模块优化，变到了map坐标系 二、main()函数以及回调函数

main()函数是很简单的，就是定义了一系列的发布者和订阅者，订阅了来自节点发布的话题和imu数据话题；然后定义了一个tf发布器，发布经过模块优化后的当前帧(/)到初始帧(/)的坐标变换；然后定义了一些列下面会用到的变量。

其中有5个订阅者，分别看一下它们的回调函数。

int main(int argc, char** argv)
{ros::init(argc, argv, "laserMapping");ros::NodeHandle nh;ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/laser_odom_to_init", 5, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2, laserCloudFullResHandler);ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_registered", 2);ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);nav_msgs::Odometry odomAftMapped;odomAftMapped.header.frame_id = "/camera_init";odomAftMapped.child_frame_id = "/aft_mapped";tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform aftMappedTrans;aftMappedTrans.frame_id_ = "/camera_init";aftMappedTrans.child_frame_id_ = "/aft_mapped";std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;PointType pointOri, pointSel, pointProj, coeff;cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));bool isDegenerate = false;cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));//创建VoxelGrid滤波器（体素栅格滤波器）pcl::VoxelGrid<PointType> downSizeFilterCorner;//设置体素大小downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);pcl::VoxelGrid<PointType> downSizeFilterSurf;downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);pcl::VoxelGrid<PointType> downSizeFilterMap;downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);//指针初始化for (int i = 0; i < laserCloudNum; i++) {laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());laserCloudCornerArray2[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray2[i].reset(new pcl::PointCloud<PointType>());}int frameCount = stackFrameNum - 1;   //0     stackFrameNum = 1 这个改成10才是论文中的1Hz频率int mapFrameCount = mapFrameNum - 1;  //4     mapFrameNum = 5ros::Rate rate(100);bool status = ros::ok();

接收特征点的回调函数

下面这三个回调函数的作用和代码结构都类似，都是接收发布的话题，分别接收了less edge point、full cloud point。

对于接收到点云之后都是如下操作：

//接收边沿点
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2)
{timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec();laserCloudCornerLast->clear();pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast);newLaserCloudCornerLast = true;
}//接收平面点
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2)
{timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec();laserCloudSurfLast->clear();pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast);newLaserCloudSurfLast = true;
}//接收点云全部点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
{timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();laserCloudFullRes->clear();pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);newLaserCloudFullRes = true;
}

接收IMU原始数据/的消息

这个回调函数主要是接收了IMU原始数据话题，更新最新的IMU数据指针，记录下imu的时间戳，然后这里使用了roll和pitch两个角。

//接收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);imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;
}

接收计算得到的位姿变换

接收模块计算出的当前帧相end相对于初始帧init的坐标变换，并将其保存在变量中。

//接收旋转平移信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{timeLaserOdometry = laserOdometry->header.stamp.toSec();double roll, pitch, yaw;//四元数转换为欧拉角geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformSum[0] = -pitch;transformSum[1] = -yaw;transformSum[2] = roll;transformSum[3] = laserOdometry->pose.pose.position.x;transformSum[4] = laserOdometry->pose.pose.position.y;transformSum[5] = laserOdometry->pose.pose.position.z;newLaserOdometry = true;
}

三、求解初始猜测–Map()

1.求解位移增量

" - "的含义是上一帧相对于初始帧的位移量 与 当前帧相对于初始帧的位移量 的差值，得到的结果是初始帧init坐标系下的位移增量 t i n i t s t a r t − e n d t_{init}^{start-end} −end​。

然后将其变换到end时刻：

t i n i t s t a r t − e n d = R e n d _ i n i t ∗ t i n i t s t a r t − e n d = R i n i t _ e n d − 1 ∗ t i n i t s t a r t − e n d R i n i t _ e n d − 1 = R Z X Y − 1 = R − r z R − r x R − r y t_{init}^{start-end} = R_{end\_init} * t_{init}^{start-end} = R_{init\_end}^{-1} * t_{init}^{start-end} \\ R_{init\_end}^{-1} = R_{ZXY}^{-1} = R_{-rz} R_{-rx} R_{-ry} −end​=​∗−end​=−1​∗−end​−1​=RZXY−1​=R−rz​R−rx​R−ry​

对应于下面代码中所示的变换。

2.求解旋转部分的初始猜测

现在这里的变量含义分别表示为：

那么有如下坐标变换关系：

R ~ m a p _ e n d = R m a p _ s t a r t ∗ R i n i t _ s t a r t − 1 ∗ R i n i t _ e n d = R Z X Y ∗ R Z X Y − 1 ∗ R Z X Y \tilde R_{map\_end} = R_{map\} * R_{init\}^{-1} * R_{init\_end} = R_{ZXY} * R_{ZXY}^{-1} * R_{ZXY} R~​=​∗−1​∗​=RZXY​∗RZXY−1​∗RZXY​

这里的计算公式与模块中的IMU修正部分完全一样：

R ~ m a p _ e n d = [ c a c y c a c z + s a c x s a c y s a c z c a c y s a c z + s a c x s a c y c a c z c a c x s a c y c a c x s a c z c a c x c a c z − s a c x − s a c y c a c z + s a c x c a c y s a c z s a c y s a c z + s a c x c a c y c a c z c a c x c a c y ] \tilde R_{map\_end}=\left[ \begin{} +& +& \\ & & -sacx\\ -+& +& \\ \end{} \right] R~​=​+−+​++​−​​

R m a p _ s t a r t = [ c b c y c b c z + s b c x s b c y s b c z c b c y s b c z + s b c x s b c y c b c z c b c x s b c y c b c x s b c z c b c x c b c z − s b c x − s b c y c b c z + s b c x c b c y s b c z s b c y s b c z + s b c x c b c y c b c z c b c x c b c y ] R_{map\}=\left[ \begin{} +& +& \\ & & -sbcx\\ -+& +& \\ \end{} \right] ​=​+−+​++​−​​

R i n i t _ s t a r t − 1 = [ c b l y c b l z − s b l x s b l y s b l z − c b l x s b l z s b l y c b l z + s b l x c b l y s b l z − c b l y s b l z + s b l x s b l y c b l z c b l x c b l z s b l y s b l z − s b l x c b l y c b l z − c b l x s b l y s b l x c b l x c b l y ] R_{init\}^{-1}=\left[ \begin{} -& -& +\\ -+& & -\\ -& sblx& \\ \end{} \right] −1​=​−−+−​−​+−​​

R i n i t _ e n d = [ c a l y c a l z + s a l x s a l y s a l z c a l y s a l z + s a l x s a l y c a l z c a l x s a l y c a l x s a l z c a l x c a l z − s a l x − s a l y c a l z + s a l x c a l y s a l z s a l y s a l z + s a l x c a l y c a l z c a l x c a l y ] R_{init\_end}=\left[ \begin{} +& +& \\ & & -salx\\ -+& +& \\ \end{} \right] ​=​+−+​++​−​​

然后使用对应位置的值相等，就得到了修正后的累计变换acx、acy、acz，计算如下：

a c x = − a r c s i n ( R 2 , 3 ) = − a r c s i n ( − s b c x ∗ ( s a l x ∗ s b l x + c a l x ∗ c a l y ∗ c b l x ∗ c b l y + c a l x ∗ c b l x ∗ s a l y ∗ s b l y ) − c b c x ∗ c b c z ∗ ( c a l x ∗ s a l y ∗ ( c b l y ∗ s b l z − c b l z ∗ s b l x ∗ s b l y ) − c a l x ∗ c a l y ∗ ( s b l y ∗ s b l z + c b l y ∗ c b l z ∗ s b l x ) + c b l x ∗ c b l z ∗ s a l x ) − c b c x ∗ s b c z ∗ ( c a l x ∗ c a l y ∗ ( c b l z ∗ s b l y − c b l y ∗ s b l x ∗ s b l z ) − c a l x ∗ s a l y ∗ ( c b l y ∗ c b l z + s b l x ∗ s b l y ∗ s b l z ) + c b l x ∗ s a l x ∗ s b l z ) ) a c y = a r c t a n ( R 1 , 3 / R 3 , 3 ) a c z = a r c t a n ( R 2 , 1 / R 2 , 2 ) acx = -(R_{2,3}) = -(-sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) ) \\ acy = (R_{1,3}/R_{3,3}) \\ acz = (R_{2,1}/R_{2,2}) acx=−(R2,3​)=−(−sbcx∗(salx∗sblx+calx∗caly∗cblx∗cbly+calx∗cblx∗saly∗sbly)−cbcx∗cbcz∗(calx∗saly∗(cbly∗sblz−cblz∗sblx∗sbly)−calx∗caly∗(sbly∗sblz+cbly∗cblz∗sblx)+cblx∗cblz∗salx)−cbcx∗sbcz∗(calx∗caly∗(cblz∗sbly−cbly∗sblx∗sblz)−calx∗saly∗(cbly∗cblz+sblx∗sbly∗sblz)+cblx∗salx∗sblz))acy=(R1,3​/R3,3​)acz=(R2,1​/R2,2​)

3.将位移增量转换到map坐标系

t m a p i n c r e m e n t = R ~ m a p _ e n d ∗ t e n d i n c r e m e n t R ~ m a p _ e n d = R Z X Y = R y R x R z t_{map}^{} = \tilde R_{map\_end} * t_{end}^{} \\ \tilde R_{map\_end} = R_{ZXY} = R_y R_x R_z ​=R~​∗​R~​=RZXY​=Ry​Rx​Rz​

4.求解平移部分的初始猜测

这里注意一点：上面求出来的增量使用的事start时刻的累积位移减去end时刻的累计位移，所以这里在求解时也是减号，如下：

t ~ m a p _ e n d = t m a p _ s t a r t + t m a p e n d − s t a r t = t m a p _ s t a r t − t m a p s t a r t − e n d \tilde t_{map\_end} = t_{map\} + t_{map}^{end-start} = t_{map\} - t_{map}^{start-end} t~​=​+−start​=​−−end​

//基于匀速模型，根据上次微调的结果和odometry这次与上次计算的结果，猜测一个新的世界坐标系的转换矩阵transformTobeMapped
// 得到T_map_end的初始猜测transformTobeMapped
void transformAssociateToMap()
{// 这个写的很好：https://zhuanlan.zhihu.com/p/159525107// 这里用到的旋转矩阵：R_end_init = R_init_end^{-1} = R_ZXY^{-1} = R_-rz * R_-rx * R_-ry// 这里的运算把括号拆开看：transformIncre = t_end^{start-end} = R_end_init * t_init^{start-end}//                                    = R_end_init * (t_init_start - t_init_end)//绕y轴旋转 -ryfloat x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float y1 = transformBefMapped[4] - transformSum[4];float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);//绕x轴旋转 -rxfloat x2 = x1;float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;//绕z轴旋转 -rz//平移增量transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;transformIncre[5] = z2;// T_init_endfloat sbcx = sin(transformSum[0]);float cbcx = cos(transformSum[0]);float sbcy = sin(transformSum[1]);float cbcy = cos(transformSum[1]);float sbcz = sin(transformSum[2]);float cbcz = cos(transformSum[2]);// T_init_startfloat sblx = sin(transformBefMapped[0]);float cblx = cos(transformBefMapped[0]);float sbly = sin(transformBefMapped[1]);float cbly = cos(transformBefMapped[1]);float sblz = sin(transformBefMapped[2]);float cblz = cos(transformBefMapped[2]);//transformAftMapped是上一次构图经过mapping微调后的转换矩阵，//可以理解为上一次的构图帧最终解算的位姿 T_map_startfloat salx = sin(transformAftMapped[0]);float calx = cos(transformAftMapped[0]);float saly = sin(transformAftMapped[1]);float caly = cos(transformAftMapped[1]);float salz = sin(transformAftMapped[2]);float calz = cos(transformAftMapped[2]);// R_transformTobeMapped// = （初始猜测）R_map_end // = R_map_start * (R_init_start)^{-1} * R_init_end// = R_transformAftMapped * (R_transformBefMapped)^{-1} * R_transformSumfloat srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);transformTobeMapped[0] = -asin(srx);float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), crycrx / cos(transformTobeMapped[0]));float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), crzcrx / cos(transformTobeMapped[0]));//这样就得到了一个世界系到当前构图帧的不错的初始姿态，存入transformTobeMapped中，用作迭代匹配的初值// 转换到map坐标系// 这里用到的旋转矩阵：R_map_end = R_ZXY = R_rz * R_rx * R_ry// 把位移增量转换到map坐标系下：t_map^{start-end} = R_map_end * t_end^{start-end}//绕z轴旋转 +rzx1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];z1 = transformIncre[5];//绕x轴旋转 +rxx2 = x1;y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;//绕y轴旋转 +rytransformTobeMapped[3] = transformAftMapped[3] - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);transformTobeMapped[4] = transformAftMapped[4] - y2;transformTobeMapped[5] = transformAftMapped[5] - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);// t_map_end = t_map_start - t_map^{start-end}
}

三、将接收到的点转换到地图坐标系下

这一部分主要的功能就是判断一下时间戳和是否接受到该帧的特征点，如果接收到了的好，就根据初始猜测，将接收到的点从end时刻坐标系变换到map坐标系下，主要函数就是()函数

  while (status) {ros::spinOnce();if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {newLaserCloudCornerLast = false;newLaserCloudSurfLast = false;newLaserCloudFullRes = false;newLaserOdometry = false;frameCount++;//控制跳帧数，>=这里实际并没有跳帧，只取>或者增大stackFrameNum才能实现相应的跳帧处理if (frameCount >= stackFrameNum) {//获取T_map_end的初始猜测transformAssociateToMap();//将最新接收到的平面点和边沿点进行旋转平移转换到地图坐标系map下int laserCloudCornerLastNum = laserCloudCornerLast->points.size();for (int i = 0; i < laserCloudCornerLastNum; i++) {pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);laserCloudCornerStack2->push_back(pointSel);}int laserCloudSurfLastNum = laserCloudSurfLast->points.size();for (int i = 0; i < laserCloudSurfLastNum; i++) {pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);laserCloudSurfStack2->push_back(pointSel);}}

()函数

输入的pi是end时刻坐标系下的点，这个变换过程如下式：

p m a p = R ~ m a p _ e n d ∗ p e n d + t m a p _ e n d = R y R x R z ∗ p e n d + t m a p _ e n d p_{map} = \tilde R_{map\_end} * p_{end} + t_{map\_end} = R_y R_x R_z * p_{end} + t_{map\_end} pmap​=R~​∗pend​+​=Ry​Rx​Rz​∗pend​+​

//根据调整计算后的转移矩阵，将点注册到全局世界坐标系下
void pointAssociateToMap(PointType const * const pi, PointType * const po)
{//绕z轴旋转（transformTobeMapped[2]）float x1 = cos(transformTobeMapped[2]) * pi->x- sin(transformTobeMapped[2]) * pi->y;float y1 = sin(transformTobeMapped[2]) * pi->x+ cos(transformTobeMapped[2]) * pi->y;float z1 = pi->z;//绕x轴旋转（transformTobeMapped[0]）float x2 = x1;float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;//绕y轴旋转（transformTobeMapped[1]），再平移po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2+ transformTobeMapped[3];po->y = y2 + transformTobeMapped[4];po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2+ transformTobeMapped[5];po->intensity = pi->intensity;
}

四、维护局部地图

首先，前面求解了一个在y方向上10米高位置的点在世界坐标系下的坐标，这个主要是为了后面帮助判断周围选定的周围点云集合S`中的cube是否在激光雷达的视域内，可以先不用管它，之后我们再讲。

下面先声明一些局部地图中使用到的变量：

下面就是求解机器人位置的cube索引、、，下图源自：SLAM前端入门框架-源码解析-知乎

然后下面六个while循环，就是循环移位的过程，如果机器人的位置靠近了地图边缘，那就调整地图的中心靠近机器人的位置，比如第一个while循环，意思是如果

调整后需要满足：

3 < < 18， 3 < < 8, 3 < < 18

      if (frameCount >= stackFrameNum) {frameCount = 0;PointType pointOnYAxis;pointOnYAxis.x = 0.0;pointOnYAxis.y = 10.0;pointOnYAxis.z = 0.0;//获取y方向上10米高位置的点在世界坐标系下的坐标pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);//立方体的中点（原点）在世界坐标系下的位置//过半取一（以50米进行四舍五入的效果），由于数组下标只能为正数，而地图可能建立在原点前后，因此//每一维偏移一个laserCloudCenWidth（该值会动态调整，以使得数组利用最大化，初始值为该维数组长度1/2）的量int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;//由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;//调整之后取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18//如果处于下边界，表明地图向负方向延伸的可能性比较大，则循环移位，将数组中心点向上边界调整一个单位while (centerCubeI < 3) {for (int j = 0; j < laserCloudHeight; j++) {for (int k = 0; k < laserCloudDepth; k++) {//实现一次循环移位效果int i = laserCloudWidth - 1;//指针赋值，保存最后一个指针位置pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//that's [i + 21 * j + 231 * k]pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//循环移位，I维度上依次后移for (; i >= 1; i--) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];}//将开始点赋值为最后一个点laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeI++;laserCloudCenWidth++;}//如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位while (centerCubeI >= laserCloudWidth - 3) {//18for (int j = 0; j < laserCloudHeight; j++) {for (int k = 0; k < laserCloudDepth; k++) {int i = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//I维度上依次前移for (; i < laserCloudWidth - 1; i++) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeI--;laserCloudCenWidth--;}while (centerCubeJ < 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int k = 0; k < laserCloudDepth; k++) {int j = laserCloudHeight - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//J维度上，依次后移for (; j >= 1; j--) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeJ++;laserCloudCenHeight++;} while (centerCubeJ >= laserCloudHeight - 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int k = 0; k < laserCloudDepth; k++) {int j = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//J维度上一次前移for (; j < laserCloudHeight - 1; j++) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeJ--;laserCloudCenHeight--;}while (centerCubeK < 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int j = 0; j < laserCloudHeight; j++) {int k = laserCloudDepth - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//K维度上依次后移for (; k >= 1; k--) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeK++;laserCloudCenDepth++;}while (centerCubeK >= laserCloudDepth - 3) {for (int i = 0; i < laserCloudWidth; i++) {for (int j = 0; j < laserCloudHeight; j++) {int k = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//K维度上依次前移for (; k < laserCloudDepth - 1; k++) {laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeK--;laserCloudCenDepth--;}

五、构建周围点云地图S`

这一部分对应于论文中下面这一段：

以机器人当前位置所处的cube为中心，取宽度方向5个、深度方向5个、高度方向5个（前后250米范围内，总共500米范围），三个维度总共125个cube，然后在这125个cube里面进一步筛选在视域范围内的cube。

筛选方法如下：

先将周围125个cube的每一个cube索引换算成在世界坐标系下的坐标(以米为单位)取周围125个cube的每一个cube的上下左右八个顶点坐标求原点到顶点距离的平方和机器人上方10m一个点到顶点距离的平方应用余弦定理求解角度，如果在(30,150)范围内则认为在视域内，图示如下

如果，就说明图示中的θ角在30度到150度之间，判定为在激光雷达视域范围内。

如果某个cube在视域内，就将该cube的索引保存到[]数组中；然后不管是不是在视域内，都将cube的索引保存到d[]数组中

最后再通过一个for循环，使用在视域范围内的cube构建周围点云S`，代码中为Map和p点云

        //下面这个三层for循环用来寻找构成局部点云集合S`的cubeint laserCloudValidNum = 0;     // 局部地图有效(在视野范围内)cube数量int laserCloudSurroundNum = 0;  // 局部地图所有cube数量//在每一维附近5个cube(前2个，后2个，中间1个)里进行查找（前后250米范围内，总共500米范围），三个维度总共125个cube//在这125个cube里面进一步筛选在视域范围内的cubefor (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) {if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth) {//如果索引合法//换算成实际比例，在世界坐标系下的坐标float centerX = 50.0 * (i - laserCloudCenWidth);float centerY = 50.0 * (j - laserCloudCenHeight);float centerZ = 50.0 * (k - laserCloudCenDepth);//下面的代码是用来判断周围点云集合S`的cube是否在雷达的视线范围内bool isInLaserFOV = false;for (int ii = -1; ii <= 1; ii += 2) {for (int jj = -1; jj <= 1; jj += 2) {for (int kk = -1; kk <= 1; kk += 2) {//周围点云S`中的cube的上下左右八个顶点坐标float cornerX = centerX + 25.0 * ii;float cornerY = centerY + 25.0 * jj;float cornerZ = centerZ + 25.0 * kk;//原点到顶点距离的平方float squaredSide1 = (transformTobeMapped[3] - cornerX) * (transformTobeMapped[3] - cornerX) + (transformTobeMapped[4] - cornerY) * (transformTobeMapped[4] - cornerY)+ (transformTobeMapped[5] - cornerZ) * (transformTobeMapped[5] - cornerZ);//pointOnYAxis到顶点距离的平方float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)+ (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);//用了余弦定理，check1其实是：机器人原点位置与cube中点的连线 与 机器人上方10m线段 的夹角 的余弦 - cos30//如果check1<0说明：这个夹角大于30度，在视野范围内float check1 = 100.0 + squaredSide1 - squaredSide2- 10.0 * sqrt(3.0) * sqrt(squaredSide1);//check2其实是：机器人原点位置与cube中点的连线 与 机器人上方10m线段 的夹角 的余弦 - cos150//如果check2>0说明：这个夹角小于150度，在视野范围内float check2 = 100.0 + squaredSide1 - squaredSide2+ 10.0 * sqrt(3.0) * sqrt(squaredSide1);//其实就是激光雷达的视野(-30,30)if (check1 < 0 && check2 > 0) {isInLaserFOV = true;}}}}//记住视域范围内的cube索引，匹配用if (isInLaserFOV) {laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;laserCloudValidNum++;}//记住附近所有cube的索引，显示用laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;laserCloudSurroundNum++;}}}}laserCloudCornerFromMap->clear();laserCloudSurfFromMap->clear();//构建特征点地图，查找匹配使用，对应于论文中S`的部分for (int i = 0; i < laserCloudValidNum; i++) {*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];}

六、特征匹配

首先将点云又变回局部坐标系，也就是里程计坐标系下的end时刻，个人没看懂这个操作有什么意义，前面变到map坐标系中也没进行什么操作。

对视域范围内的周围点云S`进行降采样操作，以减少运算量，然后如果周围点云中edge point数量大于10并且 point数量大于100，则开始进行特征匹配。

6.1 edge point匹配

用S`构造KD-tree，搜索与待匹配点最近的5个点，求这5个点的均值保存在cx、cy、cz中，求这5个点构成的协方差矩阵，保存在matA1矩阵中，然后对matA1进行特征值分解。

接下来就是判断这五个点是否构成一条直线，按照论文所述，如果三个特征值中，有一个特征值明显大于其他两个，则认为这5个点构成一条直线，且该特征值对应的特征向量为构成直线的方向向量，在代码中当最大特征大于次大特征值3倍以上，则认为构成直线。

在均值点处沿着方向向量，乘以+0.1和-0.1分别构造出该直线上两个点，即代码中的(x1,y1,z1)和(x2,y2,z2)，然后类似于中的方法，求解待匹配点到直线的距离以及垂线向量在xyz轴三个方向的分量保存到coeff中，匹配成功的点放入中。

        int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();/***********************************************************************此处将特征点转移回local坐标系，是为了voxel grid filter的下采样操作不越界？好像不是！后面还会转移回世界坐标系，这里是前面的逆转换，和前面一样应无必要，可直接对laserCloudCornerLast和laserCloudSurfLast进行下采样***********************************************************************/int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();for (int i = 0; i < laserCloudCornerStackNum2; i++) {pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);}int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();for (int i = 0; i < laserCloudSurfStackNum2; i++) {pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);}// 为了减少运算量，对点云进行下采样laserCloudCornerStack->clear();downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);//设置滤波对象downSizeFilterCorner.filter(*laserCloudCornerStack);//执行滤波处理int laserCloudCornerStackNum = laserCloudCornerStack->points.size();//获取滤波后体素点尺寸laserCloudSurfStack->clear();downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);downSizeFilterSurf.filter(*laserCloudSurfStack);int laserCloudSurfStackNum = laserCloudSurfStack->points.size();laserCloudCornerStack2->clear();laserCloudSurfStack2->clear();//如果点云数量足够大if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 100) {kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);//构建kd-treekdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);for (int iterCount = 0; iterCount < 10; iterCount++) {//最多迭代10次laserCloudOri->clear();coeffSel->clear();for (int i = 0; i < laserCloudCornerStackNum; i++) {pointOri = laserCloudCornerStack->points[i];//转换回世界坐标系pointAssociateToMap(&pointOri, &pointSel);kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);//寻找最近距离五个点if (pointSearchSqDis[4] < 1.0) {//5个点中最大距离不超过1才处理//将五个最近点的坐标加和求平均float cx = 0;float cy = 0; float cz = 0;for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;}cx /= 5;cy /= 5; cz /= 5;//求均方差float a11 = 0;float a12 = 0; float a13 = 0;float a22 = 0;float a23 = 0; float a33 = 0;for (int j = 0; j < 5; j++) {float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;a11 += ax * ax;a12 += ax * ay;a13 += ax * az;a22 += ay * ay;a23 += ay * az;a33 += az * az;}a11 /= 5;a12 /= 5; a13 /= 5;a22 /= 5;a23 /= 5; a33 /= 5;//构建协方差矩阵matA1.at<float>(0, 0) = a11;matA1.at<float>(0, 1) = a12;matA1.at<float>(0, 2) = a13;matA1.at<float>(1, 0) = a12;matA1.at<float>(1, 1) = a22;matA1.at<float>(1, 2) = a23;matA1.at<float>(2, 0) = a13;matA1.at<float>(2, 1) = a23;matA1.at<float>(2, 2) = a33;//特征值分解cv::eigen(matA1, matD1, matV1);// 论文中：最大特征值大于次大特征值的3倍认为是线特征if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {//如果最大的特征值大于第二大的特征值三倍以上float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = cx + 0.1 * matV1.at<float>(0, 0);float y1 = cy + 0.1 * matV1.at<float>(0, 1);float z1 = cz + 0.1 * matV1.at<float>(0, 2);float x2 = cx - 0.1 * matV1.at<float>(0, 0);float y2 = cy - 0.1 * matV1.at<float>(0, 1);float z2 = cz - 0.1 * matV1.at<float>(0, 2);float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));    //OA、OB叉乘的模float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));    //AB的模//O到AB的垂线的方向向量在三个方向的分量float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float ld2 = a012 / l12;//unusedpointProj = pointSel;pointProj.x -= la * ld2;pointProj.y -= lb * ld2;pointProj.z -= lc * ld2;//权重系数计算float s = 1 - 0.9 * fabs(ld2);coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;if (s > 0.1) {//距离足够小才使用laserCloudOri->push_back(pointOri);coeffSel->push_back(coeff);}}}}

6.2 point匹配

用S`构造KD-tree，搜索与待匹配点最近的5个点，用这5个点构建了一个超定方程组，用来求解平面方程Ax + By +Cz + 1 = 0的ABCD系数，尝试去拟合一个平面。

接下来就是判断这五个点拟合的这个平面好不好，论文中的判断方法是如果有一个特征值远远小于其他两个特征值，那么这个特征值对应的特征向量为平面的法向量，但是在代码中并没有使用这种方式。

代码中的方法是：求解用于拟合的5个点到拟合出的平面的距离，如果有任何一个距离大于0.2米，则认为平面拟合的不好，将不进行匹配；否则，将记录下平面参数和待匹配点到平面的距离保存在中，将匹配成功的点保存到中。

            for (int i = 0; i < laserCloudSurfStackNum; i++) {pointOri = laserCloudSurfStack->points[i];pointAssociateToMap(&pointOri, &pointSel); kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);if (pointSearchSqDis[4] < 1.0) {// 构建平面方程Ax + By +Cz + 1 = 0// 通过构建一个超定方程来求解这个平面方程for (int j = 0; j < 5; j++) {matA0.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;}//求解matA0*matX0=matB0cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);float pa = matX0.at<float>(0, 0);float pb = matX0.at<float>(1, 0);float pc = matX0.at<float>(2, 0);float pd = 1;float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps;pb /= ps;pc /= ps;pd /= ps;bool planeValid = true;// 点如果距离平面太远，就认为这是一个拟合的不好的平面for (int j = 0; j < 5; j++) {if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x +pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y +pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2) {planeValid = false;break;}}if (planeValid) {float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;//unusedpointProj = pointSel;pointProj.x -= pa * pd2;pointProj.y -= pb * pd2;pointProj.z -= pc * pd2;float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1) {laserCloudOri->push_back(pointOri);coeffSel->push_back(coeff);}}}}

七、高斯牛顿优化与退化处理

这一部分与完全一样，直接复制过来啦！

7.1 雅克比矩阵求解与高斯牛顿优化

在代码中，作者是纯手推的高斯牛顿算法，这种方式相比于使用Ceres等工具，会提高运算速度，但是计算雅克比矩阵比较麻烦，需要清晰的思路和扎实的数学功底，下面我们一起来推导一下。

以edge point匹配为例， point是一样的。

设误差函数(点到直线的距离)为：

f ( X ) = D ( p s t a r t i , p s t a r t t ) f(X)=D(p_{start}^i,p_{start}^t) f(X)=D(​,​)

其中，X为待优化变量，也就是[6]中存储的变量，表示3轴旋转rx、ry、rz和3轴平移量tx、ty、tz；D()表示两点之间的距离，其计算公式为：

D ( p s t a r t i , p s t a r t t ) = ( p s t a r t i − p s t a r t t ) T ( p s t a r t i − p s t a r t t ) D(p_{start}^i,p_{start}^t) = \sqrt {(p_{start}^i-p_{start}^t)^T (p_{start}^i-p_{start}^t) } D(​,​)=(​−​)T(​−​)​

p s t a r t i p_{start}^i ​表示start时刻坐标系下待匹配点i； p s t a r t t p_{start}^t ​表示start时刻坐标系下点i到直线jl的垂点；另外根据之前()函数推导过的坐标变换有：

p s t a r t i = R c u r r _ s t a r t − 1 ∗ ( p c u r r − 1 − t c u r r _ s t a r t ) = [ c r y c r z − s r x s r y s r z − c r y s r z + s r x s r y c r z − c r x s r y − c r x s r z c r x c r z s r x s r y c r z + s r x c r y s r z s r y s r z − s r x c r y c r z c r x c r y ] ⋅ [ p c u r r − t x p c u r r − t y p c u r r − t z ] p_{start}^i = R_{curr\}^{-1}*(p_{curr}^{-1}-t_{curr\}) = \left[ \begin{} -& -+& -\\ -& & srx\\ +& -& \\ \end{} \right] \cdot \left[ \begin{array}{c} p_{curr}-tx\\ p_{curr}-ty\\ p_{curr}-tz\\ \end{array} \right] ​=−1​∗(pcurr−1​−​)=​−−+​−+z−​−​​⋅​pcurr​−​−​−tz​​

根据链式法则，f(x)对X求导有：

∂ f ( x ) ∂ X = ∂ f ( x ) ∂ D ( p s t a r t i , p s t a r t t ) ∗ ∂ D ( p s t a r t i , p s t a r t t ) ∂ p s t a r t i ∗ ∂ p s t a r t i ∂ X \frac{∂f(x)}{∂X} = \frac{∂f(x)}{∂D(p_{start}^i,p_{start}^t)} * \frac{∂D(p_{start}^i,p_{start}^t)}{∂p_{start}^i} * \frac{∂p_{start}^i}{∂X} ∂X∂f(x)​=∂D(​,​)∂f(x)​∗∂​∂D(​,​)​∗∂X∂​​

对其中每一项进行计算：

∂ f ( x ) ∂ D ( p s t a r t i , p s t a r t t ) = 1 ∂ D ( p s t a r t i , p s t a r t t ) ∂ p s t a r t i = ∂ ( p s t a r t i − p s t a r t t ) T ( p s t a r t i − p s t a r t t ) ∂ p s t a r t i = p s t a r t i − p s t a r t t D ( p s t a r t i , p s t a r t t ) \frac{∂f(x)}{∂D(p_{start}^i,p_{start}^t)} = 1 \\ \frac{∂D(p_{start}^i,p_{start}^t)}{∂p_{start}^i} = \frac{∂\sqrt {(p_{start}^i-p_{start}^t)^T (p_{start}^i-p_{start}^t) }}{∂p_{start}^i} = \frac{p_{start}^i-p_{start}^t}{D(p_{start}^i,p_{start}^t)} ∂D(​,​)∂f(x)​=1∂​∂D(​,​)​=∂​∂(​−​)T(​−​)​​=D(​,​)​−​​

D对 p s t a r t i p_{start}^i ​求导的结果其实就是 进行归一化后的点到直线向量，它在xyz三个轴的分量就是前面求解出来的la、lb、lc变量。

∂ p s t a r t i ∂ X = ∂ R c u r r _ s t a r t − 1 ∗ ( p c u r r − 1 − t c u r r _ s t a r t ) ∂ [ r x , r y , r z , t x , t y , t z ] T \frac{∂p_{start}^i}{∂X} = \frac{∂ R_{curr\}^{-1}*(p_{curr}^{-1}-t_{curr\}) }{∂[rx,ry,rz,tx,ty,tz]^T} ∂X∂​​=∂[rx,ry,rz,tx,ty,tz]T∂−1​∗(pcurr−1​−​)​

用上面 p s t a r t i p_{start}^i ​的结果，分别对rx,ry,rz,tx,ty,tz求导，将得到的结果(3x6矩阵)再与D对 p s t a r t i p_{start}^i ​求导的结果(1x3矩阵)相乘，就可以得到代码中显示的结果(1x6矩阵)，分别赋值到matA的6个位置，matB是残差项。

最后使用的QR分解求解增量X即可。

            float srx = sin(transformTobeMapped[0]);float crx = cos(transformTobeMapped[0]);float sry = sin(transformTobeMapped[1]);float cry = cos(transformTobeMapped[1]);float srz = sin(transformTobeMapped[2]);float crz = cos(transformTobeMapped[2]);int laserCloudSelNum = laserCloudOri->points.size();if (laserCloudSelNum < 50) {//如果特征点太少continue;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));//构建雅可比矩阵和残差for (int i = 0; i < laserCloudSelNum; i++) {pointOri = laserCloudOri->points[i];coeff = coeffSel->points[i];float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x+ ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;matA.at<float>(i, 0) = arx;matA.at<float>(i, 1) = ary;matA.at<float>(i, 2) = arz;matA.at<float>(i, 3) = coeff.x;matA.at<float>(i, 4) = coeff.y;matA.at<float>(i, 5) = coeff.z;matB.at<float>(i, 0) = -coeff.intensity;}cv::transpose(matA, matAt);matAtA = matAt * matA;      // H = J^T * JmatAtB = matAt * matB;      // g = -J^T * ecv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

7.2 退化处理

代码中使用的退化处理在Ji Zhang的这篇论文(《On of -based State 》)中有详细论述。

简单来说，作者认为退化只可能发生在第一次迭代时，先对 H = J T ∗ J H=J^T * J H=JT∗J矩阵求特征值，然后将得到的特征值与阈值(代码中为10)进行比较，如果小于阈值则认为该特征值对应的特征向量方向发生了退化，将对应的特征向量置为0，然后按照论文中所述，计算一个P矩阵：

P = V f − 1 ∗ V u P = V_f^{-1} * V_u P=Vf−1​∗Vu​

V f V_f Vf​是原来的特征向量矩阵， V u V_u Vu​是将退化方向置为0后的特征向量矩阵，然后用P矩阵与原来得到的解相乘，得到最终解：

X ‘ = P ∗ X ∗ X` = P * X^* X‘=P∗X∗

            //退化场景判断与处理if (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate) {cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}//积累每次的调整量transformTobeMapped[0] += matX.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) +pow(rad2deg(matX.at<float>(1, 0)), 2) +pow(rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));//旋转平移量足够小就停止迭代if (deltaR < 0.05 && deltaT < 0.05) {break;}}

7.3 更新变换矩阵–()函数

迭代优化完成后，使用()函数进行变换矩阵的优化，主要是完成对[6]和[6]的更新。

如果有IMU的话，就使用IMU信息中的roll和pitch角对结果中的变量进行微调，微调的原理我也没太看懂，感觉就是稍微加了一点点补偿。

然后将[6]更新为当前帧相对于初始帧的变换 T i n i t _ e n d T_{init\_end} ​；将[6]更新更新为优化后的当前帧相当于地图坐标系的变换 T m a p _ e n d T_{map\_end} ​

//优化后进行变换矩阵的更新，如果有IMU的，则使用IMU进行补偿
void transformUpdate()
{if (imuPointerLast >= 0) {float imuRollLast = 0, imuPitchLast = 0;//查找点云时间戳小于imu时间戳的imu位置while (imuPointerFront != imuPointerLast) {if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) {//未找到,此时imuPointerFront==imuPointerLastimuRollLast = imuRoll[imuPointerFront];imuPitchLast = imuPitch[imuPointerFront];} else {int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);//按时间比例求翻滚角和俯仰角imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;}//imu稍微补偿俯仰角和翻滚角transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;}//记录优化之前与之后的转移矩阵for (int i = 0; i < 6; i++) {transformBefMapped[i] = transformSum[i];transformAftMapped[i] = transformTobeMapped[i];}
}

八、将当前帧的特征点加入到地图中

下面两个for循环作用是：将当前帧的特征点按照cube索引号放入地图中，用于之后的匹配；然后对周围点云S`进行一次降采样，更新地图。

        /*下面两个for循环作用是：将当前帧的特征点放入地图中，用于之后的匹配*///将corner points按距离（比例尺缩小）归入相应的立方体for (int i = 0; i < laserCloudCornerStackNum; i++) {//转移到世界坐标系pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);//按50的比例尺缩小，四舍五入，偏移laserCloudCen*的量，计算索引int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;if (pointSel.x + 25.0 < 0) cubeI--;if (pointSel.y + 25.0 < 0) cubeJ--;if (pointSel.z + 25.0 < 0) cubeK--;if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth) {//只挑选-laserCloudCenWidth * 50.0 < point.x < laserCloudCenWidth * 50.0范围内的点，y和z同理//按照尺度放进不同的组，每个组的点数量各异int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;laserCloudCornerArray[cubeInd]->push_back(pointSel);}}//将surf points按距离（比例尺缩小）归入相应的立方体for (int i = 0; i < laserCloudSurfStackNum; i++) {pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;if (pointSel.x + 25.0 < 0) cubeI--;if (pointSel.y + 25.0 < 0) cubeJ--;if (pointSel.z + 25.0 < 0) cubeK--;if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth) {int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;laserCloudSurfArray[cubeInd]->push_back(pointSel);}}//因为加入了当前sweep的点，所以这个for循环对局部点云集合S`进行降采样，然后更新总cube集合for (int i = 0; i < laserCloudValidNum; i++) {int ind = laserCloudValidInd[i];  //局部点云集合S`的cube序号laserCloudCornerArray2[ind]->clear();downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]);//滤波输出到Array2laserCloudSurfArray2[ind]->clear();downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]);//Array与Array2交换，即滤波后自我更新pcl::PointCloud<PointType>::Ptr laserCloudTemp = laserCloudCornerArray[ind];laserCloudCornerArray[ind] = laserCloudCornerArray2[ind];laserCloudCornerArray2[ind] = laserCloudTemp;//laserCloudCornerArray更新成降采样之后的了laserCloudTemp = laserCloudSurfArray[ind];laserCloudSurfArray[ind] = laserCloudSurfArray2[ind];laserCloudSurfArray2[ind] = laserCloudTemp;}

九、话题发布

最后就是一些发提的发布，包括以下话题：

并且广播了/到/的坐标变换

        mapFrameCount++;//特征点汇总降采样，每隔五帧publish一次，从第一次开始if (mapFrameCount >= mapFrameNum) {// 每隔5帧发布S`集合mapFrameCount = 0;laserCloudSurround2->clear();for (int i = 0; i < laserCloudSurroundNum; i++) {int ind = laserCloudSurroundInd[i]; // 局部地图所有cube序号*laserCloudSurround2 += *laserCloudCornerArray[ind];*laserCloudSurround2 += *laserCloudSurfArray[ind];}laserCloudSurround->clear();downSizeFilterCorner.setInputCloud(laserCloudSurround2);downSizeFilterCorner.filter(*laserCloudSurround);sensor_msgs::PointCloud2 laserCloudSurround3;pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudSurround3.header.frame_id = "/camera_init";pubLaserCloudSurround.publish(laserCloudSurround3);}//将点云中全部点转移到世界坐标系下int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++) {pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudFullRes3.header.frame_id = "/camera_init";pubLaserCloudFullRes.publish(laserCloudFullRes3);geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);odomAftMapped.pose.pose.orientation.x = -geoQuat.y;odomAftMapped.pose.pose.orientation.y = -geoQuat.z;odomAftMapped.pose.pose.orientation.z = geoQuat.x;odomAftMapped.pose.pose.orientation.w = geoQuat.w;odomAftMapped.pose.pose.position.x = transformAftMapped[3];odomAftMapped.pose.pose.position.y = transformAftMapped[4];odomAftMapped.pose.pose.position.z = transformAftMapped[5];//扭转量odomAftMapped.twist.twist.angular.x = transformBefMapped[0];odomAftMapped.twist.twist.angular.y = transformBefMapped[1];odomAftMapped.twist.twist.angular.z = transformBefMapped[2];odomAftMapped.twist.twist.linear.x = transformBefMapped[3];odomAftMapped.twist.twist.linear.y = transformBefMapped[4];odomAftMapped.twist.twist.linear.z = transformBefMapped[5];pubOdomAftMapped.publish(odomAftMapped);//广播坐标系旋转平移参量aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], transformAftMapped[4], transformAftMapped[5]));tfBroadcaster.sendTransform(aftMappedTrans);}}status = ros::ok();rate.sleep();}return 0;
}

总结

我个人感觉这个模块是比模块简单很多的，主要是不涉及到IMU就省很多事，我觉得这部分需要仔细想一想的地方就是地图维护更新的那一部分，中间优化部分很简单，中的那部分弄懂了这部分就很容易看明白了。

论文中说的这部分是低频率高精度，按照这个代码，它的频率是5Hz，但是只要改变一下控制跳帧变量，就很容易得到文章中的1Hz；高精度我觉得主要是体现在它使用的是scan-to-map的方式进行匹配，用于匹配的数据量很大，从而提高了匹配精度，因为它的迭代次数是比少的。

下一篇将介绍最后一个模块.cpp，也就是和求解到的变量进行融合得到最终变换的模块。