LIO-SAM代码解析：mapOptmization.cpp（二）

---

1. cornerOptimization

void cornerOptimization() 
{
    // 更新点的位姿变换参数，使点从当前坐标系映射到全局地图坐标系
    updatePointAssociateToMap();

    // 使用多线程并行优化处理每个角点
    #pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < laserCloudCornerLastDSNum; i++) 
    {
        PointType pointOri, pointSel, coeff;          // 定义原始点、选中的点和优化系数
        std::vector<int> pointSearchInd;             // 存储最近邻点的索引
        std::vector<float> pointSearchSqDis;         // 存储最近邻点的距离平方

        pointOri = laserCloudCornerLastDS->points[i]; // 获取当前点
        pointAssociateToMap(&pointOri, &pointSel);   // 将当前点映射到全局坐标系
        kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); // 查找最近的5个邻点

        // 初始化协方差矩阵相关变量
        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)); // 特征向量矩阵
                    
        // 如果第五个最近邻点的距离小于1.0，则认为该点具有一定的局部平面结构
        if (pointSearchSqDis[4] < 1.0) {
            float cx = 0, cy = 0, cz = 0; // 初始化质心坐标
            for (int j = 0; j < 5; j++) {
                // 计算5个最近邻点的质心
                cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
            }
            cx /= 5; cy /= 5; cz /= 5; // 计算质心坐标

            // 初始化协方差矩阵元素
            float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
            for (int j = 0; j < 5; j++) {
                // 计算每个邻点相对于质心的偏移量
                float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                float az = laserCloudCornerFromMapDS->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);

            // 判断主方向特征值是否显著大于次方向特征值
            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)));

                // 计算直线的长度
                float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                // 计算法向量的分量
                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;

                // 计算权重因子
                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) {
                    laserCloudOriCornerVec[i] = pointOri;
                    coeffSelCornerVec[i] = coeff;
                    laserCloudOriCornerFlag[i] = true;
                }
            }
        }
    }
}

1. 点集中心计算

从给定点的 5 个最近邻点中，计算点的中心位置：
$$c_x=\frac{1}{5}\sum _{j=1}^5{x}_j,c_y=\frac{1}{5}\sum _{j=1}^5{y}_j,c_z=\frac{1}{5}\sum _{j=1}^5{z}_j$$

            float cx = 0, cy = 0, cz = 0; // 初始化质心坐标
            for (int j = 0; j < 5; j++) {
                // 计算5个最近邻点的质心
                cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
            }
            cx /= 5; cy /= 5; cz /= 5; // 计算质心坐标

2. 协方差矩阵计算

对最近邻点的分布计算协方差矩阵 $\mathbf{A}$：
$$a_{11}=\frac{1}{5}\sum _{j=1}^5(x_j-c_x{)}^2,a_{12}=\frac{1}{5}\sum _{j=1}^5(x_j-c_x)(y_j-c_y),a_{13}=\frac{1}{5}\sum _{j=1}^5(x_j-c_x)(z_j-c_z)$$
$$a_{22}=\frac{1}{5}\sum _{j=1}^5(y_j-c_y{)}^2,a_{23}=\frac{1}{5}\sum _{j=1}^5(y_j-c_y)(z_j-c_z),a_{33}=\frac{1}{5}\sum _{j=1}^5(z_j-c_z{)}^2$$

协方差矩阵表示为：
$$\mathbf{A}=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{12}& a_{22}& a_{23}\\ a_{13}& a_{23}& a_{33}\end{array}\right]$$

            // 初始化协方差矩阵元素
            float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
            for (int j = 0; j < 5; j++) {
                // 计算每个邻点相对于质心的偏移量
                float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                float az = laserCloudCornerFromMapDS->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;

3. 特征值分解

对协方差矩阵 $\mathbf{A}$ 进行特征值分解：
$$\mathbf{A}=\mathbf{VD}{\mathbf{V}}^{\top }$$
其中，$\mathbf{D}$ 是对角矩阵，其对角线上的值是特征值，$\mathbf{V}$ 是对应的特征向量。

---

补充： （1） 特征值与特征向量的定义
对于一个对称矩阵 $\mathbf{A}\in {\mathbb{R}}^{n\times n}$，特征值 $\lambda$ 和对应特征向量 $\mathbf{v}$ 满足以下关系：
$$\mathbf{Av}=\lambda \mathbf{v}$$
其中：

• $\mathbf{A}$ 是输入矩阵（如 matA1）。
• $\lambda$ 是特征值。
• $\mathbf{v}$ 是与 $\lambda$ 对应的特征向量。

（2）特征值分解的公式
特征值分解可以表示为：
$$\mathbf{A}=\mathbf{V\Lambda }{\mathbf{V}}^{\top }$$
其中：

• $\mathbf{\Lambda }$ 是对角矩阵，对角线上元素为特征值：
  $$\mathbf{\Lambda }=\left[\begin{array}{cccc}{\lambda }_1& 0& \cdots & 0\\ 0& {\lambda }_2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & {\lambda }_n\end{array}\right]$$
• $\mathbf{V}$ 是正交矩阵，其列为特征向量：
  $$\mathbf{V}=\left[\begin{array}{cccc}{\mathbf{v}}_1& {\mathbf{v}}_2& \cdots & {\mathbf{v}}_n\end{array}\right]$$
  满足 ${\mathbf{V}}^{\top }\mathbf{V}=\mathbf{I}$，其中 $\mathbf{I}$ 是单位矩阵。

（3） 数学推导
特征值分解的步骤：

1. 特征值求解：
   求解特征值 $\lambda$，需要解以下特征方程：
   $$\det (\mathbf{A}-\lambda \mathbf{I})=0$$
   其中，$\det (\cdot )$ 表示行列式。

2. 特征向量求解：
   对于每个特征值 ${\lambda }_i$，通过解以下方程得到对应的特征向量 ${\mathbf{v}}_i$：
   $$(\mathbf{A}-{\lambda }_i\mathbf{I}){\mathbf{v}}_i=0$$

（4）代码中分解结果的含义
在代码 cv::eigen(matA1, matD1, matV1) 中：

• matA1: 输入矩阵 $\mathbf{A}$。
• matD1: 输出的特征值矩阵 $\mathbf{\Lambda }$，表示为：
  $$\mathbf{\Lambda }=[{\lambda }_1,{\lambda }_2,{\lambda }_3]$$
• matV1: 输出的特征向量矩阵 $\mathbf{V}$，每列是一个特征向量：
  $$\mathbf{V}=\left[\begin{array}{ccc}{v}_{11}& v_{12}& v_{13}\\ v_{21}& v_{22}& v_{23}\\ v_{31}& v_{32}& v_{33}\end{array}\right]$$
  其中，${\mathbf{v}}_1,{\mathbf{v}}_2,{\mathbf{v}}_3$ 是 ${\lambda }_1,{\lambda }_2,{\lambda }_3$ 的特征向量。

---

对应代码：

            // 对协方差矩阵进行特征值分解，提取特征值和特征向量
            cv::eigen(matA1, matD1, matV1);

4. 主方向选择

主方向对应的特征值满足以下条件：
$${\lambda }_1>3{\lambda }_2$$

原因分析：矩阵 $\mathbf{A}$ 的特征值 ${\lambda }_1,{\lambda }_2,{\lambda }_3$ 表示局部点云分布在三个主方向上的方差大小。其中：

• ${\lambda }_1$ 表示点云在主方向上的扩展程度。
• ${\lambda }_2,{\lambda }_3$ 表示点云在次方向上的扩展程度。

条件解释： 如果 ${\lambda }_1$ 显著大于 ${\lambda }_2$ 和 ${\lambda }_3$，例如满足 ${\lambda }_1>3{\lambda }_2$：

• 说明点云在 ${\mathbf{v}}_1$ 主方向上延展显著，而在其他方向上分布紧凑。
• 这种特征说明局部点云的形状更接近一条直线。在满足 ${\lambda }_1>3{\lambda }_2$ 的条件下，局部点云形状可以近似为一条直线：
• 直线的方向：由主方向的特征向量 ${\mathbf{v}}_1=(v_{x1},v_{y1},v_{z1})$ 确定。
• 直线的位置：由局部点云的质心 $(c_x,c_y,c_z)$ 确定。

直线的表达形式为：
$$\mathbf{P}(t)=(c_x,c_y,c_z)+t\cdot (v_{x1},v_{y1},v_{z1})$$
其中 $t$ 是直线上点的参数，表示点沿着方向向量 ${\mathbf{v}}_1$ 偏移的程度。

端点的计算逻辑： 在实际计算中，为了定义有限长度的直线段：

• 选择点云质心作为直线的中心点。

• 通过在质心两侧沿 ${\mathbf{v}}_1$ 方向各偏移固定距离 $0.1$ 来定义两个端点 $P_1$ 和 $P_2$。

• 端点 $P_1$：在质心处沿 ${\mathbf{v}}_1$ 方向偏移正方向距离 $0.1$：
  $$P_1=(c_x+0.1v_{x1},c_y+0.1v_{y1},c_z+0.1v_{z1})$$

• 端点 $P_2$：在质心处沿 ${\mathbf{v}}_1$ 方向偏移负方向距离 $0.1$：
  $$P_2=(c_x-0.1v_{x1},c_y-0.1v_{y1},c_z-0.1v_{z1})$$
  几何意义：

• ${\mathbf{v}}_1$ 表示直线的方向向量。

• 通过质心加减一定的偏移量（$0.1\cdot {\mathbf{v}}_1$）即可定义直线的两个端点，从而表示局部点云拟合的直线。

• ${\lambda }_1>3{\lambda }_2$ 表明点云的形状趋向于一维分布。

• 使用主方向的特征向量 ${\mathbf{v}}_1$ 和质心 $(c_x,c_y,c_z)$，可以定义直线的方向和位置。

---

                    float x0 = pointSel.x;  //中心点
                    float y0 = pointSel.y;  //中心点
                    float z0 = pointSel.z;  //中心点
                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);//端点P1
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);//端点P1
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);//端点P1
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);//端点P2
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);//端点P2
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);//端点P2

5. 距离与权重计算

这里将点线的距离转化为点到面的距离 ：参考博客
平面由三个特定的点确定：

• 中心点 ${\mathbf{p}}_0$：这是当前处理的角点，经过关联变换后的点，坐标为 $(x_0,y_0,z_0)$。
• 端点1 ${\mathbf{p}}_1$：这是基于协方差矩阵的主特征向量方向延伸一定距离得到的点，坐标为 $(x_1,y_1,z_1)$。
• 端点2 ${\mathbf{p}}_2$：这是与端点1 相反方向延伸得到的点，坐标为 $(x_2,y_2,z_2)$。

这三个点共同确定了一个几何平面，该平面用于后续的优化过程。具体来说：

• ${\mathbf{p}}_0$ 是当前处理的角点。
• ${\mathbf{p}}_1$ 和 ${\mathbf{p}}_2$ 是沿着主方向（由协方差矩阵的主特征向量确定）的两个端点，用于定义该平面的方向和位置。

通过这三个点，可以唯一确定一个平面，该平面用于评估和优化角点在地图中的位置。代码计算了由三个点 ${\mathbf{p}}_0$, ${\mathbf{p}}_1$, ${\mathbf{p}}_2$ 构成的三角形的面积和端点之间的距离。

1. 计算三角形的面积 $a_{012}$：

   计算公式：

   $$a_{012}=\sqrt{{\left[(x_0-x_1)(y_0-y_2)-(x_0-x_2)(y_0-y_1)\right]}^2+{\left[(x_0-x_1)(z_0-z_2)-(x_0-x_2)(z_0-z_1)\right]}^2+{\left[(y_0-y_1)(z_0-z_2)-(y_0-y_2)(z_0-z_1)\right]}^2}$$

   原因与用途：

   这个公式实际上计算的是三角形 ${\mathbf{p}}_0$, ${\mathbf{p}}_1$, ${\mathbf{p}}_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)));

2. 计算两端点之间的距离 $l_{12}$：

   计算公式：

   $$l_{12}=\sqrt{(x_1-x_2{)}^2+(y_1-y_2{)}^2+(z_1-z_2{)}^2}$$

   原因与用途：

   这个距离表示端点1和端点2之间的欧几里得距离。它用于归一化平面参数，确保平面方程的标准化，使得优化过程中的权重计算更加合理。平面的法向量 $(a,b,c)$ 可以通过两个向量的叉积得到，这两个向量分别是 ${\mathbf{p}}_1-{\mathbf{p}}_0$ 和 ${\mathbf{p}}_2-{\mathbf{p}}_0$。通过叉积，可以得到垂直于这两个向量的法向量。

   向量 $\mathbf{u}={\mathbf{p}}_1-{\mathbf{p}}_0=(x_1-x_0,y_1-y_0,z_1-z_0)$

   向量 $\mathbf{v}={\mathbf{p}}_2-{\mathbf{p}}_0=(x_2-x_0,y_2-y_0,z_2-z_0)$

   则法向量为：

   $$\mathbf{n}=\mathbf{u}\times \mathbf{v}=\left|\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{k}\\ x_1-x_0& y_1-y_0& z_1-z_0\\ x_2-x_0& y_2-y_0& z_2-z_0\end{array}\right|$$

   展开后得到：

   $$\mathbf{n}=\left[(y_1-y_0)(z_2-z_0)-(z_1-z_0)(y_2-y_0)\right]\mathbf{i}-\left[(x_1-x_0)(z_2-z_0)-(z_1-z_0)(x_2-x_0)\right]\mathbf{j}+\left[(x_1-x_0)(y_2-y_0)-(y_1-y_0)(x_2-x_0)\right]\mathbf{k}$$

   由此可得：

   $$\begin{gathered} a=(y_1-y_0)(z_2-z_0)-(z_1-z_0)(y_2-y_0) \\ b=-[(x_1-x_0)(z_2-z_0)-(z_1-z_0)(x_2-x_0)] \\ c=(x_1-x_0)(y_2-y_0)-(y_1-y_0)(x_2-x_0) \end{gathered}$$

标准化平面参数： 为了确保平面方程的标准化，使得优化过程中不同参数的影响力一致，需要将平面参数进行归一化。这里使用了之前计算的面积 $a_{012}$ 和距离 $l_{12}$ 来进行归一化处理。

标准化后的平面参数为：

$$a^{\prime }=\frac{a}{s},b^{\prime }=\frac{b}{s},c^{\prime }=\frac{c}{s},d^{\prime }=\frac{d}{s}$$

其中，$s$ 是归一化因子，用于确保法向量的单位长度或其他标准化需求。

                    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;

4 . 计算权重因子

计算权重因子 $s$，用于调整平面参数在优化过程中的影响力：

$$s=1-0.9\cdot \frac{|d|}{\sqrt[4]{x_{\text{ori}}^2+y_{\text{ori}}^2+z_{\text{ori}}^2}}$$

6. 优化目标

如果 $s>0.1$，则更新点的优化目标系数：
$$\text{coeff}=(s\cdot l_a,s\cdot l_b,s\cdot l_c,s\cdot l_d^2)$$

2. surfOptimization

void surfOptimization() 
{
    // 更新点的位姿变换参数，将点从当前坐标系映射到全局地图坐标系
    updatePointAssociateToMap();

    // 使用多线程并行优化每个平面点
    #pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < laserCloudSurfLastDSNum; i++) 
    {
        PointType pointOri, pointSel, coeff;         // 定义原始点、映射点和优化系数
        std::vector<int> pointSearchInd;            // 最近邻点的索引
        std::vector<float> pointSearchSqDis;        // 最近邻点的平方距离

        // 获取当前点并映射到全局坐标系
        pointOri = laserCloudSurfLastDS->points[i];
        pointAssociateToMap(&pointOri, &pointSel);

        // 查找当前点的 5 个最近邻点
        kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

        // 初始化平面拟合矩阵
        Eigen::Matrix<float, 5, 3> matA0;           // 最近邻点的坐标矩阵
        Eigen::Matrix<float, 5, 1> matB0;           // 方程右侧常数项
        Eigen::Vector3f matX0;                      // 解向量，表示平面法向量

        matA0.setZero();
        matB0.fill(-1);                             // 初始化常数项为 -1
        matX0.setZero();

        // 如果第五个邻点的距离小于阈值，进行平面拟合
        if (pointSearchSqDis[4] < 1.0) 
        {
            // 构造邻点的坐标矩阵 matA0
            for (int j = 0; j < 5; j++) {
                matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
            }

            // 求解平面方程的系数 pa, pb, pc
            matX0 = matA0.colPivHouseholderQr().solve(matB0);

            float pa = matX0(0, 0);                 // 平面方程法向量的 x 分量
            float pb = matX0(1, 0);                 // 平面方程法向量的 y 分量
            float pc = matX0(2, 0);                 // 平面方程法向量的 z 分量
            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 * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                         pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                         pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
                    planeValid = false;
                    break;
                }
            }

            // 如果平面有效，计算当前点到平面的距离
            if (planeValid) {
                float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                // 计算点的权重因子
                float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x
                        + pointOri.y * pointOri.y + pointOri.z * pointOri.z));

                // 计算优化系数
                coeff.x = s * pa;
                coeff.y = s * pb;
                coeff.z = s * pc;
                coeff.intensity = s * pd2;

                // 如果权重因子大于阈值，记录该点的优化信息
                if (s > 0.1) {
                    laserCloudOriSurfVec[i] = pointOri;
                    coeffSelSurfVec[i] = coeff;
                    laserCloudOriSurfFlag[i] = true;
                }
            }
        }
    }
}

1. 平面方程拟合

通过邻点构造线性方程组：
$$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\\ x_4& y_4& z_4\\ x_5& y_5& z_5\end{array}\right]\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\left[\begin{array}{c}-1\\ -1\\ -1\\ -1\\ -1\end{array}\right]$$

记为 $\mathbf{AX}=\mathbf{B}$，使用最小二乘法求解：
$$\mathbf{X}=({\mathbf{A}}^{\top }\mathbf{A}{)}^{-1}{\mathbf{A}}^{\top }\mathbf{B}$$

---

2. 平面方程归一化

求得的平面法向量 $\mathbf{X}=[a,b,c{]}^{\top }$，需进行归一化：
$$p_a=\frac{a}{\sqrt{a^2+b^2+c^2}},p_b=\frac{b}{\sqrt{a^2+b^2+c^2}},p_c=\frac{c}{\sqrt{a^2+b^2+c^2}},p_d=\frac{1}{\sqrt{a^2+b^2+c^2}}$$

---

3. 点到平面的距离

点 $P(x,y,z)$ 到平面 $p_{a}x+p_{b}y+p_{c}z+p_d=0$ 的距离：
$$d=p_{a}x+p_{b}y+p_{c}z+p_d$$

---

4. 权重因子计算

定义权重因子 $s$，根据点到平面的距离和点的深度调整权重：
$$s=1-0.9\cdot \frac{|d|}{\sqrt{\sqrt{x^2+y^2+z^2}}}$$

---

5. 优化系数

优化系数 $(\text{coeff.x},\text{coeff.y},\text{coeff.z},\text{coeff.intensity})$：
$$\text{coeff.x}=s\cdot p_a,\text{coeff.y}=s\cdot p_b,\text{coeff.z}=s\cdot p_c,\text{coeff.intensity}=s\cdot d$$