---

---

前言

// 下面这是计算当前帧位置t_w_curr（在上图中用红色五角星表示的位置）IJK坐标（见上图中的坐标轴），
			// 参照LOAM_NOTED的注释，下面有关25呀，50啥的运算，等效于以50m为单位进行缩放，因为LOAM用1维数组
			// 进行cube的管理，而数组的index只用是正数，所以要保证IJK坐标都是正数，所以加了laserCloudCenWidth/Heigh/Depth
			// 的偏移，来使得当前位置尽量位于submap的中心处，也就是使得上图中的五角星位置尽量处于所有格子的中心附近，
			// 偏移laserCloudCenWidth/Heigh/Depth会动态调整，来保证当前位置尽量位于submap的中心处。
			int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
			int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
			int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;

			// 由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一
			if (t_w_curr.x() + 25.0 < 0)
				centerCubeI--;
			if (t_w_curr.y() + 25.0 < 0)
				centerCubeJ--;
			if (t_w_curr.z() + 25.0 < 0)
				centerCubeK--;

			// 以下注释部分参照LOAM_NOTED，结合我画的submap的示意图说明下面的6个while loop的作用：要
			// 注意世界坐标系下的点云地图是固定的，但是IJK坐标系我们是可以移动的，所以这6个while loop
			// 的作用就是调整IJK坐标系（也就是调整所有cube位置），使得五角星在IJK坐标系的坐标范围处于
		    // 3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18，目的是为了防止后续向
			// 四周拓展cube（图中的黄色cube就是拓展的cube）时，index（即IJK坐标）成为负数。
			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]; 
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						for (; i >= 1; i--)// 在I方向上，将cube[I] = cube[I-1],最后一个空出来的cube清空点云，实现IJK坐标系向I轴负方向移动一个cube的
										   // 效果，从相对运动的角度看，就是图中的五角星在IJK坐标系下向I轴正方向移动了一个cube，如下面的动图所示，所
				                           // 以centerCubeI最后++，laserCloudCenWidth也会++，为下一帧Mapping时计算五角星的IJK坐标做准备。
						{
    
    
							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)
			{
    
     
				for (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];
						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];
						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];
						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];
						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];
						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--;
			}

			int laserCloudValidNum = 0;
			int laserCloudSurroundNum = 0;

			// 向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube，上图中五角星所在的蓝色cube就是当前位置
			// 所处的cube，拓展的cube就是黄色的cube，这些cube就是submap的范围
			for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
			{
    
    
				for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
				{
    
    
					for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
					{
    
    
						if (i >= 0 && i < laserCloudWidth &&
							j >= 0 && j < laserCloudHeight &&
							k >= 0 && k < laserCloudDepth)// 如果坐标合法
						{
    
     
							// 记录submap中的所有cube的index，记为有效index
							laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
							laserCloudValidNum++;
							laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
							laserCloudSurroundNum++;
						}
					}
				}
			}

			laserCloudCornerFromMap->clear();
			laserCloudSurfFromMap->clear();
			for (int i = 0; i < laserCloudValidNum; i++)
			{
    
    
				// 将有效index的cube中的点云叠加到一起组成submap的特征点云
				*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
				*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
			}
			int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
			int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();

1.计算当前帧位置 的 IJK 坐标

段代码的主要功能是计算当前帧在世界坐标系中的位置，并将其映射到一个局部的 IJK 坐标系中。通过动态调整 IJK 坐标系的位置和大小，使得当前帧的位置始终处于 submap 的中心，避免拓展 cube 时索引越界。此外，代码还通过拓展 IJK 坐标系的范围来实现对当前帧周围区域的特征点云提取和更新。

1. 坐标转换与偏移：
   由于 LOAM 使用 $1$ 维数组来管理 cubes，而数组索引需要是正数，因此将坐标进行偏移，保证 IJK 坐标系的所有索引都是正数。

   设当前帧位置为 $t_{\text{wcurr}}=(x,y,z)$，将该位置转换为 IJK 坐标系中的索引，并加入偏移量：
   $$I=\lfloor \frac{x+25}{50}\rfloor +\text{laserCloudCenWidth}$$
   $$J=\lfloor \frac{y+25}{50}\rfloor +\text{laserCloudCenHeight}$$
   $$K=\lfloor \frac{z+25}{50}\rfloor +\text{laserCloudCenDepth}$$

2. 负数索引调整：
   由于计算机的取余操作向零取整，需要调整负数索引，确保索引不会偏移。对于每个坐标轴 $x$, $y$, $z$，当偏移后的值小于 $0$ 时，需要调整索引：
   $$\text{If }x+25<0,\text{ then }I\text{ decrement by 1}$$
   同理，$J$ 和 $K$ 也做相同调整。

3. IJK坐标系的扩展与移动：
   通过多个 while 循环动态调整 IJK 坐标系，使得当前位置（五角星标记的点）尽量处于 submap 的中心附近，避免后续拓展时索引为负数。

   • 每次循环将 IJK 坐标系向负方向移动一个 cube 或向正方向移动一个 cube，确保当前点不会越过 submap 边界。

4. 扩展范围的 cube 记录：
   对于中心点周围的 $3\times 3\times 2$ 立方体区域（I 轴扩展 2 个，J 轴扩展 2 个，K 轴扩展 1 个），记录有效的 cube 索引。
   Valid Indices = { i + laserCloudWidth ⋅ j + laserCloudWidth ⋅ laserCloudHeight ⋅ k } \text{Valid Indices} = \{i + \text{laserCloudWidth} \cdot j + \text{laserCloudWidth} \cdot \text{laserCloudHeight} \cdot k\} Valid Indices={ i+laserCloudWidth⋅j+laserCloudWidth⋅laserCloudHeight⋅k}

5. 特征点云提取：
   从有效的 cubes 中提取点云数据，并将其合并到 submap 的特征点云中：
   $$\text{Submap Feature}=\bigcup \text{laserCloudCornerFromMap},\bigcup \text{laserCloudSurfFromMap}$$

			for (int i = 0; i < laserCloudValidNum; i++)
			{
    
    
				// 将有效index的cube中的点云叠加到一起组成submap的特征点云
				*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
				*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
			}

目的：

• IJK 坐标系转换与调整：确保当前位置始终位于 submap 中心，避免索引越界。
• 特征点云构建：根据有效的 cube 索引提取并合并特征点云，更新 submap。

2.与地图特征点与线段拟合及残差计算

2.1. 变换点云坐标系

由于在地图中，submap 的点云数据是基于世界坐标系（world frame），而当前帧的点云是基于激光雷达坐标系（Lidar frame），因此需要先将当前帧的特征点从 Lidar 坐标系变换到世界坐标系。

变换公式：
$${\mathbf{p}}_{\text{world}}=T_{\text{Lidar}\to \text{world}}({\mathbf{p}}_{\text{Lidar}})$$
其中，$T_{\text{Lidar}\to \text{world}}$ 是从激光雷达坐标系到世界坐标系的变换矩阵。

// 用Mapping的位姿w_curr，将Lidar坐标系下的点变换到world坐标系下
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
    
    
	Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
	Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
	po->x = point_w.x();
	po->y = point_w.y();
	po->z = point_w.z();
	po->intensity = pi->intensity;
	//po->intensity = 1.0;
}
// 需要注意的是submap中的点云都是world坐标系，而当前帧的点云都是Lidar坐标系，所以
// 在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到world坐标系下
pointAssociateToMap(&pointOri, &pointSel);

2.2. 寻找最近邻点

在submap的 corner 特征点云中，使用 k-d 树算法从当前帧的特征点中寻找最近邻的 5 个点：

// 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 

$$\text{nearest\_neighbors}({\mathbf{p}}_{\text{curr}})=\arg \underset{{\mathbf{p}}_{\text{map}}}{\min }\Vert {\mathbf{p}}_{\text{curr}}-{\mathbf{p}}_{\text{map}}\Vert$$

2.3. 计算最近邻点的中心

将找到的 5 个最近邻点的坐标求平均，得到这些点的几何中心：

$$\mathbf{center}=\frac{1}{5}\sum _{i=1}^5{\mathbf{p}}_i$$

for (int j = 0; j < 5; j++)
{
    
    
	Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
						laserCloudCornerFromMap->points[pointSearchInd[j]].y,
						laserCloudCornerFromMap->points[pointSearchInd[j]].z);
	center = center + tmp;
	nearCorners.push_back(tmp);
}
// 计算这个5个最近邻点的中心
center = center / 5.0;

2.4. 计算协方差矩阵

计算这 5 个点的协方差矩阵，用于判断这些点是否呈线性分布。协方差矩阵的计算公式为：

$$\mathbf{Cov}=\frac{1}{5}\sum _{i=1}^5({\mathbf{p}}_i-\mathbf{center})({\mathbf{p}}_i-\mathbf{center}{)}^T$$

// 协方差矩阵
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++)
{
    
    
	Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
	covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}

2.5. 特征值与特征向量分析

对协方差矩阵进行特征值分解，通过比较最大特征值与第二大特征值来判断这 5 个点是否呈线状分布。如果最大的特征值显著大于其他特征值，则认为这 5 个点呈线状分布。

特征值分解：
$$\mathbf{Cov}\cdot {\mathbf{v}}_i={\lambda }_i\cdot {\mathbf{v}}_i\text{where }{\lambda }_i\text{ is the eigenvalue, and }{\mathbf{v}}_i\text{ is the corresponding eigenvector}$$
其中，${\mathbf{v}}_2$ 对应最大特征值 ${\lambda }_2$，即线段的方向向量。

2.6. 判断是否为线特征

如果最大特征值大于 3 倍的次大特征值，则认为这 5 个点是线特征，并计算点到线的残差。点到直线的距离 $d$ 为：
$$d=\frac{|(\mathbf{p}-{\mathbf{p}}_a)\times (\mathbf{p}-{\mathbf{p}}_b)|}{\Vert {\mathbf{p}}_a-{\mathbf{p}}_b\Vert }$$
其中，${\mathbf{p}}_a$ 和 ${\mathbf{p}}_b$ 是直线上的两个点，$\mathbf{p}$ 是当前点。

// 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);

// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);// 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])// 如果最大的特征值 >> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直”
{
    
     
	Eigen::Vector3d point_on_line = center;
	Eigen::Vector3d point_a, point_b;
	// 从中心点沿着方向向量向两端移动0.1m，构造线上的两个点
	point_a = 0.1 * unit_direction + point_on_line;
	point_b = -0.1 * unit_direction + point_on_line;

	// 然后残差函数的形式就跟Odometry一样了，残差距离即点到线的距离，到介绍lidarFactor.cpp时再说明具体计算方法
	ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
	problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
	corner_num++;	
}	

2.7. 添加残差函数

如果这 5 个点呈线性分布，则构造一个残差函数，表示当前点到直线的距离，并将其加入到优化问题中：

$$\text{Cost Function}=\text{LidarEdgeFactor}({\mathbf{p}}_{\text{curr}},{\mathbf{p}}_a,{\mathbf{p}}_b,s)$$
其中，$s$ 是运动补偿系数，${\mathbf{p}}_{\text{curr}}$ 是当前点的位置，${\mathbf{p}}_a$ 和 ${\mathbf{p}}_b$ 是线段的两个端点。

3.点到平面拟合与残差计算（LidarPlaneNormFactor）

3.1. 变换点云坐标系

将当前帧的激光雷达特征点从激光雷达坐标系（Lidar frame）变换到世界坐标系（World frame），使用预测的位姿。

变换公式：
$${\mathbf{p}}_{\text{world}}=T_{\text{Lidar}\to \text{world}}({\mathbf{p}}_{\text{Lidar}})$$
其中，$T_{\text{Lidar}\to \text{world}}$ 是从激光雷达坐标系到世界坐标系的变换矩阵。

3.2. 寻找最近邻点

在submap的 surface 特征点云中，使用k-d树算法从当前帧的特征点中寻找最近邻的 5 个点：

$$\text{nearest\_neighbors}({\mathbf{p}}_{\text{curr}})=\arg \underset{{\mathbf{p}}_{\text{map}}}{\min }\Vert {\mathbf{p}}_{\text{curr}}-{\mathbf{p}}_{\text{map}}\Vert$$

3.3. 最小二乘法拟合平面

平面方程的标准形式为：
$$Ax+By+Cz+D=0$$
假设平面不通过原点，则方程简化为：
$$Ax+By+Cz+1=0$$
使用最小二乘法来求解平面的法向量 $(A,B,C)$，通过求解超定方程组：
$${\mathbf{A}}_0\cdot \mathbf{n}={\mathbf{B}}_0$$
其中：
$${\mathbf{A}}_0=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ ⋮& ⋮& ⋮\\ x_5& y_5& z_5\end{array}\right],{\mathbf{B}}_0=\left[\begin{array}{c}-1\\ -1\\ -1\\ -1\\ -1\end{array}\right]$$
通过解这个线性方程组可以得到平面的法向量 $\mathbf{n}=(A,B,C)$。

for (int j = 0; j < 5; j++)
{
    
    
	matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
	matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
	matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
}
// 求解这个最小二乘问题，可得平面的法向量，find the norm of plane
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);

3.4. 归一化法向量

计算得到的法向量可以归一化，确保其模长为 1：
$${\mathbf{n}}_{\text{normalized}}=\frac{\mathbf{n}}{\Vert \mathbf{n}\Vert }$$

3.5. 判断平面拟合的有效性

使用点到平面的距离公式来验证平面是否拟合良好。点 $(x_0,y_0,z_0)$ 到平面 $Ax+By+Cz+D=0$ 的距离为：
$$d=\frac{|A{x}_0+B{y}_0+C{z}_0+D|}{\sqrt{A^2+B^2+C^2}}$$
如果距离大于某个阈值（如 $0.2$），则认为该平面拟合不好。

	// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
	if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
			 norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
			 norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
	{
    
    
		planeValid = false;// 平面没有拟合好，平面“不够平”
		break;
	}

3.6. 添加残差函数

如果平面拟合成功，构造点到平面的残差函数，用于优化过程。点到平面的距离作为残差项：
$$\text{residual}=\frac{|A{x}_0+B{y}_0+C{z}_0+D|}{\Vert \mathbf{n}\Vert }$$

Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (planeValid)
{
    
    
	// 构造点到面的距离残差项，同样的，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
	ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
	problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
	surf_num++;
}

然后将残差函数添加到优化问题中：
$$\text{Cost Function}=\text{LidarPlaneNormFactor}({\mathbf{p}}_{\text{curr}},\mathbf{n},D)$$

3.7. 最终优化

通过 Ceres 优化库最小化残差，从而优化位姿估计。