NV-LIO:基于法向量的激光雷达-惯性里程计以实现多层环境中的鲁棒SLAM及核心代码解释

---

摘要

代码：github
原文：原文

摘要—在过去的几十年中，已经开发了许多激光雷达惯性里程计（LIO）算法，证明其在各种环境中表现令人满意。然而，这些算法主要是在开放的户外环境中进行验证，而在封闭的室内环境中往往面临挑战。在这样的室内环境中，由于激光雷达扫描的快速变化和重复的结构特征（如墙壁和楼梯），尤其是在多层建筑中，可靠的点云配准变得非常困难。本文提出了NV-LIO，一种基于法向量的LIO框架，旨在实现多层结构室内环境的同时定位与地图构建（SLAM）。我们的方法从激光雷达扫描中提取法向量，并利用这些法向量进行对应搜索，以增强点云配准的性能。为确保稳健的配准，分析法向量方向的分布，并检查退化情况以调整匹配不确定性。此外，实现了一种基于视点的回环闭合模块，以避免被墙壁遮挡的错误对应。所提出的方法通过公共数据集和我们自己的数据集进行了验证。

图 1：使用NV-LIO对KAIST的五层建筑KI大楼的地图构建结果。（a）显示了整体地图构建结果和激光雷达轨迹。（b）展示了建筑内部摄影与地图构建结果的比较。

一、介绍

移动机器人领域通过传感器、执行器和控制算法的进步，扩展了其操作环境。这些进步正在重塑机器人技术的格局，将其应用从自动驾驶扩展到各种环境，包括消防机器人、安全机器人、配送机器人等，这些机器人需要在封闭的室内环境中克服各种障碍。为了使这些机器人能够在未知的室内环境中导航，可靠的同时定位与地图构建（SLAM）技术是必需的。
激光雷达传感器直接测量周围环境的深度信息，能够提供稳定的测量结果，无论环境光照条件如何。利用这些能力，三维（3D）激光雷达主要用于通过对连续的激光雷达点云进行配准，实现移动机器人的定位和地图构建。在过去几十年中，已发布了许多用于自动驾驶应用的公共数据集，这促进了激光雷达惯性里程计算法技术的显著进步，通过利用这些数据集进行算法的开发和验证。
然而，这些算法在室内空间中表现出性能下降的趋势，并且在多层环境中往往会出现显著的失败。与户外环境不同，室内环境的特点是空间狭小且有薄墙壁，形成多个分隔区域。在这些区域中，激光雷达扫描捕获的场景可能因墙壁和楼梯等重复结构元素而快速变化。由于这些环境因素，现有算法在点云配准中经常遇到失败。其中一个注册失败的原因是对墙壁两侧点云对齐的挑战，称为双面问题 [1]。尽管墙壁有一定厚度，这些不对齐可能在地图中错误地将墙壁表示为无厚度的结构。另一个问题是固定参数的问题。在狭窄空间中，激光雷达扫描往往在近距离产生密集的点云。然而，当使用固定参数进行下采样匹配时，可用于匹配的点数减少，从而导致对齐过程中的潜在不准确或不匹配 [2][3]。
本文提出了NV-LIO，一种基于法向量的紧耦合LIO框架，旨在实现多层室内SLAM。该框架利用将密集旋转型三维激光雷达扫描投影到范围图像的能力，从中提取法向量。扫描之间的配准不仅考虑最近邻，还考虑法向量的角度差异，从而提高了配准过程中的对应搜索精度。在匹配过程中，分析法向量方向的分布，以评估退化的发生，并调整匹配的不确定性。为了确保准确的回环闭合，在扫描和子地图之间的匹配过程中采用了可见性分析，防止不同房间或楼层之间的错误对应。
本文的主要贡献可以总结如下：

• 我们引入了一种紧耦合的LIO框架，该框架利用来自密集机械激光雷达点云的法向量，以实现狭窄室内环境中的稳健配准。
• 为了解决点云配准中的退化情况，特别是在长走廊或楼梯场景中，提出了退化检测算法及相应的配准不确定性协方差矩阵计算方法。
• 我们在各种数据集上对所提算法进行了充分验证，包括对公共数据集和我们自己收集的数据集的评估，如图1所示。验证结果证明了所提方法的有效性和多样性。

二、相关工作

激光雷达（惯性）里程计的关键组成部分依赖于点云配准的有效性。为了处理每次扫描中数万个点的计算负载，开发了多种方法来减少计算时间。基于特征的方法从点云中提取关键特征以进行匹配，而直接方法则涉及对点云进行下采样，并使用诸如迭代最近点（ICP） [4] 或广义ICP（GICP） [5] 的技术。
在基于特征的算法中，LOAM [6] 利用带有点头运动的二维激光雷达扫描，基于相邻点之间的关系提取角点和表面点。LeGO-LOAM [7] 采用LOAM的特征提取技术用于三维激光雷达，从每个环中提取特征，并结合地面分割以提高准确性。LIO-SAM [8] 使用类似的特征提取方法，但引入了紧耦合的LIO与IMU预积分 [9]，显示出高性能。Fast-LIO [10] 采用相同的特征提取方法，但引入了一种新的卡尔曼增益公式以加快计算速度。
利用其速度优势，Fast-LIO2 [11] 引入了一种直接匹配方法，使用ikd-tree [12] 进行快速最近邻搜索，从而实现高效的扫描到地图匹配，而不是传统的扫描到扫描匹配， resulting in 更具全球一致性的性能。另一方面，Faster-LIO [13] 用增量体素化（iVox）系统替代Fast-LIO2的树结构，以实现更快的计算速度。
虽然这些直接方法在户外环境中表现优异，但观察到它们在点云密集堆积的狭窄室内环境中往往失败。这些算法的快速计算依赖于点云的下采样，这可能未能保留可靠配准所需的有意义点。为了解决这个问题，提出了根据激光雷达扫描分析调整参数的自适应方法。基于Faster-LIO，AdaLIO [2] 通过分析给定的激光雷达扫描，自适应地调整体素化大小、搜索半径和平面选择的残差边界，以确保在退化情况下的稳定配准性能。在DLIO [14] 和文献 [3] 中，提出了自适应关键帧插入方法，以平衡高效计算时间和可靠的点云配准。这些方法根据扫描分析插入关键帧，在狭窄空间中产生密集的关键帧，在宽广区域中产生稀疏的关键帧。
SuMa [15] 采用两步过程，首先将激光雷达点云投影到深度图像中，然后提取法向量以形成表面点（surfels）。SLAM通过表面点地图和测量的表面点之间的匹配进行，基于匹配结果进行表面点更新。为了加快对应搜索，使用渲染技术将表面点地图投影到当前帧上以找到图像中的对应关系。LIPS [16] 利用室内场景中平面的普遍性，如办公室。它从激光雷达扫描中提取平面原语，并利用一种匹配这些原语的技术。在 [1] 中，该方法首先提取每个点的法向量，并使用聚类识别平面。引入了一种称为前向ICP流的方法，利用点到平面的距离找到与现有平面对应的新扫描点，而不是在每次扫描中查找平面。在匹配过程中，如果平面的法向量与现有平面之间的角度差超过某个阈值，则避免匹配，有效解决了双面问题。

三、方法

A. 系统概述

系统X的状态如下所示：

$$X=\left[R,t,v,b_a,b_g\right](1)$$

其中 $R,t,v$ 分别是全局参考坐标系中的旋转矩阵、位置和速度，$b_a$ 和 $b_g$ 是IMU坐标系中的IMU加速度和陀螺仪偏置。当设置一个与IMU坐标系不同的机体坐标系时，需要转换IMU加速度和角速度。需要根据坐标变换执行IMU加速度和角速度的转换。为了方便并减少额外的计算，系统的机体坐标系与IMU坐标系对齐。关键帧K表示如下：
K = { x ( R , t ) , N ( P 0 ( p 0 , n 0 ) , ⋯   , P k − 1 ( p k − 1 , n k − 1 ) ) } ( 2 ) K=\left\{x(R,t),N\left(P_{0}\left(p_{0},n_{0}\right),\cdots,P_{k-1}\left(p_{k-1},n_{k-1}\right)\right)\right\} \qquad(2) K={x(R,t),N(P0​(p0​,n0​),⋯,Pk−1​(pk−1​,nk−1​))}(2)

其中x是全局参考坐标系中的机体姿态，当扫描时，N是包含法线点P_{i}的正常云，这些点由点坐标$p_i$和法线向量$n_i$组成。与室外环境不同，在室外环境中，地图的很大一部分通常在激光雷达的范围内，而在狭窄的室内空间中，当前扫描中地图点的可见性可能极其有限。由于这一特点，直接将扫描与地图匹配的方法可能会导致漂移，特别是在狭窄的走廊或楼层间过渡时，使得返回同一位置时的校正变得困难。因此，在本研究中，我们采用了基于关键帧的姿态图SLAM框架。地图被构建为关键帧的集合，允许它根据校正动态调整。
图2展示了所提出算法的概述。通过使用积分IMU陀螺仪测量得到的姿态来补偿扫描中的运动。通过球面投影提取运动补偿后的点云。在使用惯性测量对提取的正常云进行对齐后，我们通过前关键帧组成的子图之间的正常云注册来确定相对姿态。此外，还通过视点基础的闭环来获得校正测量。这些注册结果被包含在图中作为相对姿态因子，IMU测量也通过IMU预积分被添加到图中。通过这种姿态图优化，估计当前姿态和IMU偏置。

B. 预处理

为了获得准确的法线向量和注册结果，对激光雷达扫描进行运动补偿至关重要。为此，我们使用带有估计偏置的IMU获得的角速度。考虑到激光雷达接收点的频率（超过10000 Hz）与IMU频率（100 Hz）相比更高，我们根据初始接收点的时间戳，使用IMU估计的旋转进行时间插值。这个过程在激光雷达坐标系中进行，以便于后续的球面投影步骤。

C. 球面投影和法线提取

对运动补偿后的激光雷达点云进行球面投影以生成深度图像。在此过程中，根据激光雷达点云的特性，如激光雷达通道数、水平分辨率和视场角（FoV），手动选择深度图像的大小。给定最大垂直$FoV(fo{v}_{max})$、最小垂直$FoV(fo{v}_{min})$、深度图像高度$(h)$和宽度$(w)$，垂直分辨率为$ve{r}_{res}=(fo{v}_{max}-fo{v}_{min})/h$，水平分辨率为$ho{r}_{res}=2\pi /w$。每个点$(p(x,y,z))$的图像坐标$(u,v)$如下：
$$\begin{array}{rl}u& =(\pi -\mathrm{atan}2(y,x))/{\mathrm{hor}}_{\text{res}}\\ v& =(fo{v}_{\max }-\mathrm{atan}2\left(z,\sqrt{x^2+y^2}\right)/\left(ve{r}_{\text{res}}\right).\end{array}(3)$$
法线向量可以通过对范围图像的水平方向$（\Delta r/\Delta \psi ）$和垂直方向$（\Delta r/\Delta \theta ）$的深度值$(r(u,v))$进行微分来计算：
$$n^s(u,v)=\left[\begin{array}{c}(\Delta r/\Delta \psi )/c\\ -(\Delta r/\Delta \theta )/c\\ 1/c\end{array}\right](4)$$
其中$\theta =\pi /2-(fo{v}_{max}-v\times ve{r}_{res})$表示极角，$\psi =\pi -u\times ho{r}_{res}$表示方位角，和$c$是一个缩放变量，使得法线向量成为一个单位向量。在广阔的室外环境中，相邻像素覆盖的区域更广，减轻了距离测量噪声对法线向量计算的影响。相反，在狭窄的室内环境中，相同区域要小得多，放大了距离测量噪声对计算结果的影响。考虑到这些因素，我们没有简单地使用相邻像素之间的微分，而是采用了基于窗口的方法，假设窗口内的导数值是相似的。计算并平均窗口内每个水平和垂直方向上的导数值对，以减轻距离测量噪声的影响。在球坐标系中表示的法线向量$n^{\prime }$，根据每个像素的方位角和仰角，转换为笛卡尔坐标系中的$n^{\prime }$，如下所示：
$$n^{\prime }(u,v)=T(e,w)n(u,v)$$
其中 $T(e,y)$ 是变换矩阵，如下所示：
$$T(O,\omega )=\left[\begin{array}{cc}-\sin (\omega )& \cos (\omega )\cos (\theta )\\ \cos (\omega )& \sin (\omega )\cos (\theta )\\ 0& -\sin (\theta )\end{array}\right](5)$$
由于所有像素的变换矩阵保持不变，通过预先计算并存储所有像素的变换矩阵，减少了计算时间。
由于检测到的表面不能与射线平行或朝向射线方向$(r=p/p)$，当法线向量和射线方向$(n°.r)$的点积为正时，法线向量会被反转。最后，为了验证法线向量是否与窗口中的邻近点形成共识，计算了点与由它们的法线向量和邻近点形成的平面之间的距离。如果点到平面距离在5厘米内的点的数量少于窗口大小的三分之一，则被认为是无效的。通过这个过程，得到了包含法线点P的正常云N。
图3显示了从输入云中提取法线的整个过程。

计算法向量代码：

void NormalExtraction(const pcl::PointCloud<ProjectedPoint>::Ptr &projected_cloud, 
                      pcl::PointCloud<PointsWithNormals>::Ptr &normal_cloud) 
{
    // 初始化图像（如果需要显示）
    if (show_img == 1) {
        normal_img = cv::Mat(ver_pixel_num, hor_pixel_num, CV_8UC3, cv::Scalar(0, 0, 0));
        normal_img_sp = cv::Mat(ver_pixel_num, hor_pixel_num, CV_8UC3, cv::Scalar(0, 0, 0));
    }

    #pragma omp parallel for num_threads(4)
    for (int i = 0; i < projected_cloud->size(); ++i) {
        int u, v;
        index2uv(i, u, v); // 将点云索引转换为图像坐标
        
        ProjectedPoint &curr_pt = projected_cloud->points[i];
        // 复制点坐标和强度等信息到输出点云
        normal_cloud->points[i].x = curr_pt.x;
        normal_cloud->points[i].y = curr_pt.y;
        normal_cloud->points[i].z = curr_pt.z;
        normal_cloud->points[i].intensity = curr_pt.intensity;
        normal_cloud->points[i].range = curr_pt.range;

        // 跳过无效点
        if (curr_pt.valid == 0) {
            continue;
        }

        // 检查边界条件，避免越界访问
        if (u + normal_neighbor >= hor_pixel_num || (u - normal_neighbor < 0) ||
            v + normal_neighbor >= ver_pixel_num || (v - normal_neighbor < 0)) {
            continue;
        }

        // 初始化用于计算法线的变量
        double dzdpsi_sum = 0.0; // 对应水平方向的变化量累加（类似于公式中的∂z/∂ψ积分）
        double dzdtheta_sum = 0.0; // 对应垂直方向的变化量累加（类似于公式中的∂z/∂θ积分）
        int dpsi_sample_no = 0; // 水平方向采样点数
        int dtheta_sample_no = 0; // 垂直方向采样点数

        // 遍历邻域点
        for (int j = -normal_neighbor; j <= normal_neighbor; ++j) {
            for (int k = -normal_neighbor; k <= normal_neighbor; ++k) {
                int query_i = uv2index(u + j, v + k);
                ProjectedPoint &query_pt = projected_cloud->points[query_i];
                // 跳过无效点
                if (query_pt.valid == 0) {
                    continue;
                }

                // 遍历水平方向邻域点，计算dz/dψ（公式4的离散形式，但注意这里的实现与公式不完全对应，因为它是在邻域内进行的差分）
                for (int l = j + 1; l <= normal_neighbor; ++l) {
                    int target_i = uv2index(u + l, v + k);
                    ProjectedPoint &target_pt = projected_cloud->points[target_i];
                    if (target_pt.valid == 0) {
                        continue;
                    }
                    dzdpsi_sum += (target_pt.range - query_pt.range) / (float(l - j) * hor_resolution * curr_pt.range); // 注意这里的实现与公式不完全一致，但思路相近
                    ++dpsi_sample_no;
                }

                // 遍历垂直方向邻域点，计算dz/dθ（同上，公式5的离散形式）
                for (int l = k + 1; l <= normal_neighbor; ++l) {
                    int target_i = uv2index(u + j, v + l);
                    ProjectedPoint &target_pt = projected_cloud->points[target_i];
                    if (target_pt.valid == 0) {
                        continue;
                    }
                    dzdtheta_sum += (target_pt.range - query_pt.range) / (float(l - k) * ver_resolution * curr_pt.range); // 同上
                    ++dtheta_sample_no;
                }
            }
        }

        // 检查采样点数是否足够
        if (dpsi_sample_no < normal_neighbor * 2 || dtheta_sample_no < normal_neighbor * 2) {
            continue;
        }

        // 计算平均变化率
        float dzdpsi_mean = dzdpsi_sum / float(dpsi_sample_no);
        float dzdtheta_mean = dzdtheta_sum / float(dtheta_sample_no);

        // 构建球面法线向量（注意这里的实现与直接计算法线向量不完全对应，但它是基于上述变化率的估计）
        Eigen::Vector3f normal_sp {dzdpsi_mean, -dzdtheta_mean, 1};
        normal_sp.normalize();

        // 获取射线方向（从原点指向当前点的向量）
        Eigen::Vector3f ray_dir {curr_pt.x, curr_pt.y, curr_pt.z};
        ray_dir.normalize();

        // 将球面法线映射到笛卡尔坐标系（这里缺少具体的映射逻辑，因为sp2cart_map未给出定义）
        Eigen::Vector3f normal = sp2cart_map[i] * normal_sp;
        normal.normalize();

        // 确保法线与射线方向不共线（通过点积判断）
        if (normal.dot(ray_dir) > 0) {
            normal = -normal;
        }

        // 计算点到平面的距离d（这里似乎没有直接使用公式，但逻辑上是基于法线和点坐标计算点到平面的距离）
        float d = -(normal.x() * curr_pt.x + normal.y() * curr_pt.y + normal.z() * curr_pt.z);

        // 验证法线的有效性（通过检查邻域点是否满足法线方向）
        bool valid_normal = true;
        int valid_neighbors = 0;
        for (int j = -normal_neighbor; j <= normal_neighbor; ++j) {
            for (int k = -normal_neighbor; k <= normal_neighbor; ++k) {
                int query_i = uv2index(u + j, v + k);
                ProjectedPoint &target_pt = projected_cloud->points[query_i];
                float dist = std::abs(d + normal.x() * target_pt.x + normal.y() * target_pt.y + normal.z() * target_pt.z);
                if (dist < 0.05) {
                    ++valid_neighbors;
                }
            }
        }
        if (valid_neighbors < (2 * normal_neighbor + 1) * (2 * normal_neighbor + 1) / 3) {
            continue;
        }

        // 设置输出点云的法线信息
        normal_cloud->points[i].valid = 1;
        normal_cloud->points[i].normal_x = normal.x();
        normal_cloud->points[i].normal_y = normal.y();
        normal_cloud->points[i].normal_z = normal.z();

        // 如果需要显示，则更新图像
        if (show_img == 1) {
            // ...（图像更新代码，与公式无直接关联）
        }
    }

    // 显示图像（如果需要）
    if (show_img == 1) {
        cv::vconcat(normal_img, normal_img_sp, normal_img);
        cv::imshow("Normal Img", normal_img);
        cv::waitKey(1);
    }
}

D. 法线云注册

扫描到扫描的匹配可能导致不可靠的注册结果，特别是对于激光雷达在滚动或俯仰旋转以及扫描旋转轴上的平移运动。类似于LIO-SAM，我们采用了扫描到子图匹配方法来解决这个问题。子图是通过在上一个关键帧坐标系中累积前一个关键帧的正常云生成的。对于上一个关键帧$K_n$，增加前j个关键帧的子图Nn如下：
$$N_n=N_n\cup N_{n-1}\cup \cdots \cup N_{n-j}$$
其中Nk表示关键帧$K_i$中转换到关键帧$K_k$坐标系中的法线云，U表示法线云的增加。

为了进行准确的对应搜索和快速匹配，当前查询帧$K_q$通过IMU积分从其最后获得的姿态转换到初始姿态。知道目标和查询帧的世界坐标系，使我们能够确定两个帧之间的初始相对姿态。利用这些信息，目标帧被转换到查询帧的坐标系中，并继续进行匹配过程。之后，为了加快匹配速度，当前法线云$N_q$和子图$N_n$使用体素网格滤波器进行下采样。为了实现结果法线云之间的稳定匹配，建立满足以下两个标准的对之间的对应关系：首先，点对点距离在距离阈值内的对。其次，法线向量方向差异在角度阈值内的对。为了完成这一点，我们首先使用kd树为当前法线云中的每个查询点选择在距离阈值内的子图点。然后对于按最近顺序选择的子图点，计算选定点和查询点之间法线向量方向的角度差异。如果角度差异在角度阈值内，这两个点被选为对应对。
之后，使用高斯-牛顿方法通过利用对应对计算相对姿态。每对的残差代价函数计算为点到平面的距离，目标帧相对于查询帧的相对姿态可以通过解决以下优化问题来计算：
$$(R,t)=\arg \underset{(R,t)}{\min }\sum _{k=1}^m(n{g}_k-(R{p}_{ik}+t-P_{qk}){)}^2(7)$$
其中 $(R,t{)}_k=0$。然后，得到的相对姿态被转换为相对姿态因子，并添加到因子图中。
点云高斯牛顿法：

// 遍历点云cloud_i中的每一个点
for(size_t point_id = 0; point_id < cloud_i->size(); ++point_id) {
    // 获取当前点point_i
    PointsWithNormals point_i = cloud_i->points[point_id];
    // 初始化一个用于存储变换后点的变量
    PointsWithNormals point_ij;
    // 使用相对位姿rel_poseT变换point_i到point_ij
    TransformPoints(point_i, point_ij, rel_poseT);

    // 用于存储最近邻搜索结果的索引和距离
    std::vector<int> knn_index;
    std::vector<float> knn_distance;

    // 初始化匹配标志和匹配点的ID
    bool found_match = false;
    int match_id = -1;

    // 在点云cloud_j的kdtree中搜索point_ij的3个最近邻
    kdtreeNormalPoint->nearestKSearch(point_ij, 3, knn_index, knn_distance);

    // 遍历这3个最近邻点
    for(int i = 0; i < 3; ++i) {
        // 如果最近邻距离超过阈值，则跳出循环
        if(knn_distance[i] > nearest_point_range) {
            break;
        }
        // 获取当前最近邻点point_j
        PointsWithNormals & point_j = cloud_j->points[knn_index[i]];
        // 计算point_ij和point_j的法向量之间的点积，用于估计它们之间的夹角
        float dot_product = point_ij.normal_x * point_j.normal_x + 
                            point_ij.normal_y * point_j.normal_y + 
                            point_ij.normal_z * point_j.normal_z;

        // 如果法向量之间的夹角大于10度（大约），则继续搜索下一个最近邻
        if(dot_product < 0.9848) { // Approximately 10 degrees
            continue;
        }

        // 找到匹配点，跳出循环
        found_match = true;
        match_id = knn_index[i];
        break;
    }

    // 如果没有找到匹配点，则继续下一个点的处理
    if(!found_match) {
        continue;
    }
    // 将匹配关系存储到match_vec中
    match_vec[point_id] = match_id;
}

// 清除之前的匹配记录，准备存储新的匹配对
matches.clear();
// 遍历match_vec，将有效的匹配对存储到matches中
for(int i = 0; i < match_vec.size(); ++i) {
    if(match_vec[i] != -1) {
        std::pair<int, int> match_pair{i, match_vec[i]};
        matches.push_back(match_pair);
    }
}

// 如果匹配对数太少，则认为配准失败
if(matches.size() < 15) {
    printf("Match Number: %d\n", int(matches.size()));
    return false;
}

// 优化部分：使用高斯-牛顿方法
// 初始化A矩阵和b向量
Eigen::MatrixXd A(6, 6);
Eigen::VectorXd b(6);
A.setZero();
b.setZero();

// 遍历所有匹配对，构建误差方程
for(int match_id = 0; match_id < matches.size(); ++match_id) {
    // 获取匹配对中的两个点
    PointsWithNormals point_i = cloud_i->points[matches[match_id].first];
    PointsWithNormals point_ij;
    TransformPoints(point_i, point_ij, rel_poseT);
    PointsWithNormals & point_j = cloud_j->points[matches[match_id].second];

    // 将点的坐标和法向量转换为Eigen的Vector3d类型
    Eigen::Vector3d X_ij(point_ij.x, point_ij.y, point_ij.z);
    Eigen::Vector3d X_j(point_j.x, point_j.y, point_j.z);
    Eigen::Vector3d N_j(point_j.normal_x, point_j.normal_y, point_j.normal_z);

    // 计算点到平面的距离作为残差
    double residual = N_j.dot(X_ij - X_j);
    // 计算雅可比矩阵
    Eigen::Matrix<double, 1, 6> jacobian;
    jacobian << N_j[2] * X_ij[1] - N_j[1] * X_ij[2],
                N_j[0] * X_ij[2] - N_j[2] * X_ij[0],
                N_j[1] * X_ij[0] - N_j[0] * X_ij[1],
                N_j[0],
                N_j[1],
                N_j[2];

    // 更新A矩阵和b向量
    A += jacobian.transpose() * jacobian;
    b += jacobian.transpose() * residual;

    // 累加残差平方和，用于后续评估
    residual_sum += residual* residual;
}

// 计算平均残差平方和
residual_sum /= static_cast<float>(matches.size());

// 解误差方程，得到位姿增量delta
Eigen::VectorXd delta = A.ldlt().solve(-b);
// 构建更新矩阵update
Eigen::Matrix4d update = Eigen::Matrix4d::Identity();
update(0, 3) = delta(3);
update(1, 3) = delta(4);
update(2, 3) = delta(5);
// 更新旋转部分
update.block(0, 0, 3, 3) = (Eigen::AngleAxisd(delta(2), Eigen::Vector3d::UnitZ()) *
                            Eigen::AngleAxisd(delta(1), Eigen::Vector3d::UnitY()) *
                            Eigen::AngleAxisd(delta(0), Eigen::Vector3d::UnitX())).matrix();
// 应用更新到相对位姿上
rel_poseT = update * rel_poseT;

// 计算旋转和平移的增量，用于判断收敛
float deltaR = delta.segment<3>(0).norm()*180.0/M_PI;
float deltaT = delta.segment<3>(3).norm();

// 如果旋转和平移的增量都小于阈值，则认为已经收敛，跳出循环
if(deltaR < 0.01 && deltaT < 0.01) {
    break;
}

E、回环检测

全局回环检测算法在多层室内环境中常常面临挑战，特别是在具有重复结构特征的环境中。这种挑战在楼梯间尤为明显，因为其特征的重复性可能导致在楼层间过渡时与来自不同楼层的点云产生错误关联。因此，在本研究中，我们采用了一种专注于匹配当前位置信息附近关键帧的回环检测方法，而不是进行全局搜索。

尽管像ICP或GICP这样的技术，通常使用半径搜索来找到最近点作为对应关系，频繁用于局部回环检测方法，但在狭窄的室内空间中，它们往往会导致对齐不准确。这主要是因为室内环境通常由多个分隔区域组成，即使在激光雷达位置发生微小变化时，LiDAR扫描也会出现显著差异。为了解决这个问题，我们引入了一种基于视点的回环闭合检测方法，灵感来自于文献 [15] 中的投影技术，以实现更好的对应搜索。

图4：基于视点的楼层间回环闭合检测示例

图4展示了基于视点的回环检测的一个示例。首先，使用每个关键帧的位置构建kd-tree，然后选择与当前帧最接近的关键帧。在此过程中，紧接着当前帧的关键帧被排除在kd-tree之外。一旦确定了一个回环闭合候选关键帧，该候选关键帧的法向云会被转换到当前帧的LiDAR位姿。随后，如图4c所示，该法向云被投影以匹配当前的LiDAR视点。在球面投影过程中，分配给同一像素的点中，仅分配距离最近的点。
然而，这种投影方法可能无法排除由于激光雷达点云的稀疏性质，从当前视点本不应看到的点。为了解决这个问题，我们利用位于射线方向90度角内的法线点，记作N+。由于这些点代表墙的对面，我们希望排除任何超过90度角到射线方向的法线点，记作N-，这些点穿过射线方向。因此，对于N+点的每个像素，如果N-点比N+点更远，则排除N-点附近的任何像素。随后，通过基于径向距离偏差和与当前法线云投影图像（图4b）的角偏差过滤投影点，获得对应对（图4d）。类似于lll-D中的法线云注册方法，这些匹配的点对应对被优化以估计相对姿态。然后，这些被作为闭环因子插入到姿态图中。

图5：楼梯间和长走廊环境中的退化情况

F、退化检测

在室内环境中，许多表面常常彼此平行排列，导致退化情况的发生。如图5所示的楼梯井或长走廊等环境就是这样的例子，其中表面的法线向量仅分布在两个方向，导致在剩余方向上的平移歧义。例如，在楼梯井的情况下，构成楼梯井的墙壁的法线向量水平分布，导致水平方向上的定位精度高，但垂直方向上可能存在歧义。

为了计算适合这种退化情况的匹配不确定性，可以利用匹配法线点中法线向量的分布。这可以通过法线向量的主成分分析来获得，如下所示：首先，计算法线向量的协方差矩阵C为：

$$C=\left(\frac{1}{m}\sum _{k=0}^{m-1}\left(n_k{n}_k^{\top }\right)\right).(8)$$

随后，使用特征值分解将协方差矩阵C分解为 $C=V\Lambda V^{-1}$，其中V是由特征向量组成的矩阵，$\Lambda$ 是对角元素为特征值的矩阵，如下所示：

$$\begin{array}{rl}& V=\left[v_0,v_1,v_2\right]\\ & \Lambda =\mathrm{diag}\left({\lambda }_0,{\lambda }_1,{\lambda }_2\right)\end{array}(9)$$

计算向量的特征值和特征向量：

    for(int i = 0; i < matches.size(); ++i) {
        PointsWithNormals & pt = cloud_i->points[matches[i].first];
        Eigen::Vector3d normal(pt.normal_x, pt.normal_y, pt.normal_z);
        covMatrix += normal * normal.transpose();
    }
    covMatrix /= static_cast<float>(matches.size());
    covMatrix = imu_to_lidar.block(0, 0, 3, 3) * covMatrix * lidar_to_imu.block(0, 0, 3, 3);

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covMatrix);
    eigenvalues = eigenSolver.eigenvalues();
    eigenvectors = eigenSolver.eigenvectors();

其中 ${\lambda }_0<{\lambda }_1<{\lambda }_2$。可以使用特征值来大致判断法线向量的分布，其中最小的特征值 ${\lambda }_0$，如果它低于特定阈值，则表示退化情况。随后，每个特征值 ${\lambda }_j$ 对应于一个特征向量 $v_i$，允许将测量协方差Q设置如下：
$$Q=sV\mathrm{diag}({\lambda }_0,{\lambda }_1,{\lambda }_2)V^{\top }(10)$$
其中s是给定的常数。在与紧接前一个关键帧匹配时，我们在退化情况下使用这种基于分布的测量协方差插入因子。然而，在闭环的情况下，如果检测到退化，存在错误匹配的高概率，我们避免插入循环因子以确保稳定性。
插入因子图协方差代码：

                Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(6, 6);
                Q*= 0.01;
                Q.block(3, 3, 3, 3) = eigenvectors * eigenvalues.cwiseInverse().asDiagonal() * eigenvectors.transpose() * 0.01;

                gaussian_noise = gtsam::noiseModel::Gaussian::Covariance(Q);
                gtsam::BetweenFactor<gtsam::Pose3> pose_factor(X(key), X(PointNodes[previous_node_id].node_no), rel_pose, gaussian_noise);

G. 姿态图SLAM框架

姿态图由初始姿态的先验因子、通过法线云注册获得的相对姿态因子、循环检测中的闭环因子，以及IMU预积分中的IMU因子和常数偏置因子组成。从上一次姿态图优化结果中获得的反映偏置的IMU测量值被集成，以连续估计当前帧在IMU的速率下的状态。如果当前帧和上一帧之间的位姿差异超过某个阈值，就会插入一个新的关键帧。使用iSAM2框架[17]构建和优化姿态图。

H. 实现细节

球面图像投影中使用的像素数被设置为通道数乘以1024。为了稳定地提取法线向量，对于32通道或更少的激光雷达，使用了3x3的窗口；而对于超过32通道的激光雷达，则使用了5x5的窗口来计算法线向量。法线云注册的距离阈值被设置为0.5米，下采样体素的大小根据场景被设置为0.4米或0.2米。对于关键帧，如果与前一个关键帧的姿态角度差异超过30度，或者距离差异大于阈值，就会添加一个新的关键帧。这个距离阈值根据建筑特征被设置为1.0米或0.5米。闭环的距离阈值被设置为10米。

四、结果