前言

• 前两期我们深入解析了 Scan Context 作为粗匹配、small_gicp 作为细匹配的回环检测管线——从描述符生成到两阶段检索再到精配准，完整覆盖了"发现回环 → 验证回环 → 输出 6-DOF 约束"的链路
• 往前内容：
  • 第一期：【3D SLAM源码解读系列】(一)Scan Context与Scan Context++：3D点云回环检测源码解析
  • 第二期：【3D SLAM源码解读系列】(二)Small_gicp——5 个积木搭出最优点云配准
• 然而，回顾第一期的内容，Scan Context 有一个隐含的设计前提：激光雷达拥有 360° 水平视场角（FOV）。当我们面对的是固态激光雷达（如 Livox 系列）或被机身/机械结构严重遮挡的视场时，Scan Context 的极坐标分箱和 max_z 编码就会暴露出显著的局限
• 刚好，SPARO 实验室在 2024 年 IEEE RA-L 上发表了 SOLiD（Spatially Organized and Lightweight Global Descriptor）——专门针对 FOV 受限场景下的激光雷达地点识别。它的核心思路是把 Scan Context 的 2D 极坐标分箱升级为 3D RAH（Range-Angle-Height）分箱，同时引入垂直方向加权来弥补有限视场下的信息缺失，最终输出一个仅 100 维的超轻量描述符
• 本文将基于 SOLiD 官方 C++ 源码，从预处理到描述符生成再到回环检测与偏航估计，完整覆盖公式推导与代码逐行对照，同时对比 Scan Context，讲清楚"为什么有限 FOV 下 SC 不行而 SOLiD 行"

需要注意的是：SOLiD 和 Scan Context 一样，定位是地点识别 + 偏航估计——它只回答"这个地方我来过吗"和"上次朝哪个方向"，不输出 x, y, z, roll, pitch。完整 6-DOF 回环约束还得靠 small_gicp 精配准来补上。换句话说，本期是第一期的"有限 FOV 升级版"——把 SC 的粗匹配能力扩展到固态雷达，后续管线不变。

---

---

1 SC的局限

1-1 SC 回顾

• 以防你忘记，Scan Context 的核心流程是：
  • 极坐标分箱：把 XY 平面按距离（ring）和方位角（sector）划分为 $N_r\times N_s$ 的 2D 网格
  • 最大高度编码：每个 bin 取 max_z，得到一个 $20\times 60$ 的描述符矩阵
  • Ring Key 粗筛：对每行取均值，得到旋转不变的 20 维紧凑指纹，用 KD 树做快速候选检索
  • 列平移精排：用 Sector Key 粗对齐偏航 → 列向余弦距离精算相似度 → 输出回环帧 ID + yaw 角差
• 这整条管线依赖一个关键假设：雷达水平 FOV = 360°。描述符矩阵的 60 列覆盖全部方位角，circshift 列平移模拟 yaw 旋转的前提是"转了之后所有列的数据依然有效"——360° 雷达转一圈，数据确实只是列的循环重排

1-2-1 FOV 受限场景与问题引入

• 然而，很多实际场景下雷达的 FOV 是严重受限的：

受限原因	典型场景	FOV 影响
固态激光雷达	Livox Mid-360（圆形 FOV，~70° 圆锥）	水平方向只有 ~70° 覆盖，60 列里大部分是空的
Livox Avia	非重复扫描	水平 ~70°，垂直 ~77°，但点云分布不均匀
机身遮挡	无人机、多传感器融合平台	雷达被机臂/其他传感器遮挡，某些方位角几乎没数据
地面机器人局部视场	雷达只朝前方	后方 180° 完全盲区

• 在这些场景下，Scan Context 的 2D 极坐标分箱暴露出三个致命问题：

• 问题一：大量空 bin 导致描述符退化。SC 的 $20\times 60$ 网格里，60 列中可能有 40 列以上完全没有点落入——这些列的 max_z 全为 0。两张不同地点的 Scan Context 可能因为"大部分列都是空的"而算出极高的余弦相似度——误匹配。

说人话：就像用 60 个字的指纹去认地方，但只写满了 10 个字——剩下 50 个字全是空格。两张不同的"空格指纹"看起来几乎一模一样，算法根本分不清。

1-2-2 circshift 失效与 max_z 不稳定

• 问题二：circshift 列平移对齐失效。SC 的偏航对齐机制是"把描述符的列整体循环平移"。如果只有前方 70° 的点云（约 12 列有数据），其他 48 列全是空列——循环平移后，原本有数据的 12 列可能有一大半被移到盲区（空列），而盲区的空列被移到有数据的区域。两张原本能对齐的描述符，平移后反而对不上了——distDirectSC 算出的是完全随机的距离，无论怎么平移都找不到正确对齐。

说人话：就像你只有正面 70° 的视野。SC 的列平移假设你可以"扭头看一圈"——但如果只能看见前方 70°，扭头之后你之前看到的东西就跑到视野外面去了，算法看到的是一堆空列，自然对不上。

• 问题三：max_z 编码在垂直信息稀疏时不稳定。360° 雷达扫一圈，每个 bin 里有几十上百个点，max_z 能稳定反映"这个方向最高的东西"。但有限 FOV 下，每个 bin 里的点数大幅减少，max_z 对单个噪声点极其敏感——一个飞点就能彻底改变 bin 的值。

说人话：人多了取最高身高有意义（正态分布的极值稳定），但只有一两个人时，取最高身高就不靠谱了——来个 2 米的篮球运动员就把你的统计彻底掀翻。

1-2-3 SC 维度代价与本质总结

• 除了上述三个空间编码层面的问题，SC 还有一个容易被忽视的工程代价——描述符维度太高：

• SC 的完整描述符是一个 $20\times 60=1200$ 维矩阵，不是一个向量。1200 维带来的直接后果是：

  • 必须两阶段检索：1200 维直接暴力比对太慢，所以 SC 不得不在 Stage 1 用 Ring Key（20 维行均值）建 KD 树粗筛，Stage 2 再用 1200 维完整描述符做精排——整个检索流程多了一层复杂度
  • 大部分维度是空的：有限 FOV 下 60 列中可能 40+ 列为空，1200 维里有效信息可能不到 200 维——维度假高，信息量并没有更多
  • 对比 SOLiD：SOLiD 只有 100 维（Range-SOLiD 40 维 + Angle-SOLiD 60 维），比 SC 的 Ring Key（20 维）多不了多少，可以直接暴力搜，同时区分度又够强

• 换句话说，SC 的高维度是"360° FOV 的遗产"——360° 下 1200 维大部分有效，维度高是优势（区分度强）。但有限 FOV 下，高维度变成了"注水肉"——维度还在，信息没了，反而拖累检索效率。

• 总结：SC 不适配有限 FOV 的根本原因——

  • SC 是 2D（XY 平面）极坐标描述符，包含 $N_r\times N_s$ 个空间格
  • 每个空间格只存一个标量（max_z），丢失了垂直分布信息
  • 360° FOV 下信息充分 → 信息压缩（取 max）损失可接受
  • 有限 FOV 下信息稀缺 → 再取 max 就是灾难——信息本就不够，压缩后连基本的地点区分都做不到

SC 的局限性本质：它把 3D 点云压成 2D 描述符，保留了足够的信息处理 360° FOV 下的 yaw 旋转，但面对 FOV 受限场景，2D 压缩丢失的信息恰恰是最关键的垂直结构分布——而这正是 SOLiD 要补上的。

---

2 SOLiD

2-1 介绍

• SOLiD（Spatially Organized and Lightweight Global Descriptor） 是 SPARO 实验室（Hogyun Kim 等）在 2024 年 IEEE RA-L 发表的激光雷达地点识别算法，专门针对 FOV 受限 场景
• 论文全称：Narrowing your FOV with SOLiD: Spatially Organized and Lightweight Global Descriptor for FOV-constrained LiDAR Place Recognition
• 作者团队中包括 Giseop Kim——没错，他就是 Scan Context 的提出者。SOLiD 可以理解为"SC 的作者自己下场，设计了一个解决 SC 局限的新算法"
• 代码开源在 https://github.com/sparolab/solid，提供 C++ 和 Python 两个版本
• SOLiD 的 README 中还列出了多个 SLAM 系统集成计划，但截至本文写作时（2026.08），这些仓库的公开状态如下：
  • SOLiD-A-LOAM、SOLiD-PyICP-SLAM：README 中标注为已集成，但仓库链接返回 404，尚未公开
  • Distributed-SOLiD-SLAM：README NEWS 中标注为 “code is released!!”，但链接实际重定向到了 SKiD-SLAM（另一个分布式 SLAM 项目），并非独立的 SOLiD 集成仓库
  • SOLiD-SLAM、SOLiD-FAST-LIO2、SOLiD-LIO-SAM、SOLiD-KISS-ICP 等：README TODO 列表中明确标注为计划中，尚未发布

说人话：SC 是"把点云拍扁成一张 2D 图片（俯视图），每个像素存最高的东西"——适合 360° 扫一圈的室外场景。SOLiD 是"把点云切成三明治（3D 体素），数每个格子里的点数，然后从两个方向（距离和角度）投射成两个条形码"——FOV 再窄，只要有垂直结构（墙壁、柱子、路沿），两个条形码就能拼出一个区分度足够的地点指纹。

2-2 算法流程

• SOLiD 的完整算法流程可以概括为以下三步：

• Step 1 — 3D 柱坐标分箱与计数：把一帧 3D 点云按 3D 柱坐标（范围-方位角-高度） 划分成 $N_r\times N_a\times N_h$ 个体素，统计每个体素内的点数——得到"这个方向、这个距离、这个高度上有多少点"

  • 相比 SC 的 2D 分箱 + max_z，SOLiD 多了一维（高度），存的是计数而非最大值
  • 这是应对有限 FOV 的关键——信息稀缺时不能再取 max（单个飞点就能污染整个 bin），必须用计数保留完整的 3D 空间分布

• Step 2 — 双投影 + 垂直加权：从 3D 体素网格中导出两个 2D 投影矩阵：

  • Range-Height 矩阵（$N_r\times N_h$）：沿 Angle 轴坍缩，问"每个距离环、每个高度层有多少点"
  • Angle-Height 矩阵（$N_a\times N_h$）：沿 Range 轴坍缩，问"每个方位扇区、每个高度层有多少点"
  • 计算垂直分布权重向量——统计每个高度层有多少点，归一化到 $[0,1]$ 后对两个矩阵的每一列做加权求和
  • 得到 Range-SOLiD（$N_r$ 维）和 Angle-SOLiD（$N_a$ 维），拼接为 $N_r+N_a=40+60=100$ 维向量——比 SC 的 $20\times 60=1200$ 维更紧凑

• Step 3 — 两步走检索：

  • 回环检测：用 Range-SOLiD（旋转不变，只管"距离远近的密度分布"）算余弦相似度——相似度最高的候选帧即为回环帧
  • 偏航估计：用 Angle-SOLiD（旋转敏感，保留了方位角顺序）做循环移位 + L1 距离最小化——最小距离对应的偏移量即 yaw 角差
  • 和 SC 一样输出"回环帧 ID + yaw 角差"，可直接作为 small_gicp 等精配准的初始值

2-3 算法背景和应用场景

• SOLiD 的设计动机来自真实的工程痛点：

场景	传感器	FOV	SC 的问题	SOLiD 的解法
固态 LiDAR SLAM	Livox Mid-360 / Avia	水平 ~70°，垂直 ~77°	60 列中 48 列为空列，circshift 失效	3D RAH 计数分箱，Angle-SOLiD 正常做循环移位
无人机避障	Livox Avia 安装在无人机正下方	被机身/桨叶严重遮挡	某些方位角的 max_z 完全由旋翼噪声贡献	统计分布而非极值，飞点被稀释
地面机器人	单线/多线雷达仅朝前方	前方 180° ~ 270°	后方列全空，描述符极度稀疏	垂直维度补偿水平信息的缺失
多传感器融合平台	雷达 FOV 被相机/IMU 支架遮挡	不规则 FOV	bin 覆盖不均匀，行均值 Ring Key 不稳定	垂直加权后，有垂直结构的区域获得更高权重

• SOLiD 的整个设计——3D 分箱、计数编码、垂直加权——都围绕一条工程洞察展开：当水平 FOV 受限时，垂直方向的结构信息（墙壁、柱子、路沿、建筑立面）是唯一可靠的地点识别线索。

说人话：360° 雷达就像站在山顶看一圈——每个方向都有信息。有限 FOV 雷达就像透过门缝看房间——只能看到一条窄缝。SC 试图从门缝里认出整条街的轮廓（isovist），不现实。SOLiD 的方法是：既然只能看到一条缝，那就把这条缝里的"垂直结构分布"（天花板多高、墙在哪儿、柱子在哪儿）记下来——信息量不输 360° 全景，只是表达方式不同。

• 因此，SOLiD 不限定室内还是室外，论文在 KITTI（室外城市道路）和 HeLiPR（室外+室内混合）上都验证过——它只关心场景中是否有垂直结构，这决定了 SOLiD 的表现上限：

场景类型	垂直结构	SOLiD 表现
城市峡谷、街道两侧有建筑	密集（墙壁、柱子、路沿、招牌）	很好——每个距离环都有清晰的"垂直指纹"
室内走廊、房间	极密集（四面墙+天花板）	很好——高度维度利用最充分，number_vector 区分度极高
开阔广场、高速公路、沙漠	稀疏（几乎只有地面）	差——除地面外几乎没有垂直信息，描述符退化

• 这和 SC 刚好互补——SC 在开阔 360° 室外表现最好（每个方向都有地平线轮廓），SOLiD 在有垂直结构的地方表现最好。说白了，不关心里里外外，只关心激光照到的方向上有没有墙、柱、立面。

需要明确的是，SOLiD 和 SC 的定位一样——地点识别 + 偏航估计，不输出完整的 6-DOF 位姿：
* 回环检测告诉你"这个地方我来过"，偏航估计告诉你"上次朝那个方向差了 $k\times 6^{\circ }$"
* x、y、z、roll、pitch 这五个自由度，SOLiD 不碰——它只负责"认路"，不负责"定位"
* 完整管线是：SOLiD（粗匹配）→ small_gicp（精配准），和本系列前两期一脉相承——第一期讲 SC 粗匹配，第二期讲 small_gicp 精配准，本期 SOLiD 就是第一期 SC 的"有限 FOV 升级版"

2-4 预处理：点云裁剪与降采样

• 在生成描述符之前，SOLiD 做了三步预处理——这些步骤直接对应源码中的三个函数：

• Step 1 — 去除过远点（remove_far_points）：距离超过 MAX_DISTANCE（默认 80m）的点直接丢弃。远距离点稀疏且信噪比低，对地点识别贡献有限

• Step 2 — 去除过近点（remove_closest_points）：距离小于 MIN_DISTANCE（默认 3m）的点丢弃。近处的点往往是地面或雷达自身的回波噪声，不具备区分度

• Step 3 — 体素降采样（down_sampling）：用 PCL 的 VoxelGrid 滤波器以 VOXEL_SIZE（默认 0.4m）对点云做体素降采样，减少点数、保证均匀密度

• 源码非常直白：

// solid.cpp — remove_far_points
void SOLiDModule::remove_far_points(pcl::PointCloud<PointType> & scan_raw, 
                                     pcl::PointCloud<PointType>::Ptr scan_out)
{
    for(int i = 0; i < scan_raw.points.size(); i++)
    {
        float dist = calc_dist(scan_raw.points[i].x, 
                               scan_raw.points[i].y, 
                               scan_raw.points[i].z);
        if(dist < MAX_DISTANCE)  // 只保留距离 < 80m 的点
        {
            scan_out->points.push_back(scan_raw.points[i]);
        }
    }
}

// solid.cpp — remove_closest_points
void SOLiDModule::remove_closest_points(pcl::PointCloud<PointType> & scan_raw, 
                                         pcl::PointCloud<PointType>::Ptr scan_out)
{
    for(int i = 0; i < scan_raw.size(); i++)
    {
        float dist = calc_dist(scan_raw.points[i].x, 
                               scan_raw.points[i].y, 
                               scan_raw.points[i].z);
        if(dist > MIN_DISTANCE)  // 只保留距离 > 3m 的点
        {
            scan_out->points.push_back(scan_raw.points[i]);
        }
    }
}

// solid.cpp — down_sampling
void SOLiDModule::down_sampling(pcl::PointCloud<PointType> & scan_raw, 
                                 pcl::PointCloud<PointType>::Ptr scan_down)
{
    pcl::PointCloud<PointType> Data_for_Voxel;
    pcl::VoxelGrid<PointType> downSizeFilterSolid;

    copyPointCloud(scan_raw, Data_for_Voxel);
    downSizeFilterSolid.setInputCloud(Data_for_Voxel.makeShared());
    downSizeFilterSolid.setLeafSize(VOXEL_SIZE, VOXEL_SIZE, VOXEL_SIZE);
    downSizeFilterSolid.filter(*scan_down);
}

• 三步走完，原始点云被精简为一个干净、均匀的稀疏点云，送入 makeSolid 做描述符提取。这一步在 test.cpp 中的完整调用链为：

// test.cpp — 完整预处理 + 描述符生成流程
// 1. Preprocessing
pcl::PointCloud<PointType>::Ptr rm_far_points(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr rm_cls_points(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr downsampled_pts(new pcl::PointCloud<PointType>);
solid.remove_far_points(*query, rm_far_points);
solid.remove_closest_points(*rm_far_points, rm_cls_points);
solid.down_sampling(*rm_cls_points, downsampled_pts);

// 2. Generate SOLiD
Eigen::VectorXd query_solid = solid.makeSolid(*downsampled_pts);

---

3 SOLiD 描述符生成

3-1 RAH 分箱：从 2D 极坐标升级到 3D 柱坐标

• 这是 SOLiD 最核心的创新——把 Scan Context 的 2D（Range × Angle）分箱升级为 3D（Range × Angle × Height） 分箱。我们先从数据结构 RAH 说起：

// solid_module.h — RAH 结构体
struct RAH 
{
    int idx_range = 0;   // 距离索引（0 ~ NUM_RANGE-1）
    int idx_angle = 0;   // 方位角索引（0 ~ NUM_ANGLE-1）
    int idx_height = 0;  // 高度索引（0 ~ NUM_HEIGHT-1）
};

• 三个维度各自的含义和参数：

维度	符号	分箱数	分辨率	计算方式
Range（距离）	$r=\sqrt{x^2+y^2}$	NUM_RANGE = 40	MAX_DISTANCE/40 = 2m	idx_range = floor(r / gap_range)
Angle（方位角）	$\theta =\arctan 2(y,x)$	NUM_ANGLE = 60	360°/60 = 6°	idx_angle = floor(θ / gap_angle)
Height（仰角）	$\varphi =\arctan 2(z,r)$	NUM_HEIGHT = 32	(FOV_u - FOV_d)/32	idx_height = floor((φ - FOV_d) / gap_height)

说人话：在 SC 的 Range（环）和 Angle（扇区）两个维度之外，SOLiD 增加了第三维——Height（仰角/高度层）。原来 SC 只能描述"30° 方向、20m 远处的最高东西"，SOLiD 能描述"30° 方向、20m 远处、高度在 5m~6m 之间的那层有多少个点"——多了整整一维的空间分辨率。

• 核心函数 pt2rah 负责把每个点的笛卡尔坐标 $(x,y,z)$ 转换为 $(idx\_range,idx\_angle,idx\_height)$：

// solid.cpp — pt2rah：笛卡尔坐标 → RAH 索引
RAH SOLiDModule::pt2rah(PointType & point, 
                         float gap_angle, float gap_range, float gap_height)
{
    RAH rah;
    float point_x = point.x;
    float point_y = point.y;
    float point_z = point.z;

    // 防止除零
    if(point_x == 0.0) point_x = 0.001;
    if(point_y == 0.0) point_y = 0.001;

    // 1. xy2theta：笛卡尔 → 方位角 θ ∈ [0°, 360°)
    float theta = xy2theta(point_x, point_y);
    // 2. 水平距离 r = sqrt(x² + y²)
    float dist_xy = sqrt(point_x*point_x + point_y*point_y);
    // 3. 仰角 φ = atan2(z, r)，转角度制
    float phi = rad2deg(atan2(point_z, dist_xy));

    // 4. 三通道分箱索引
    rah.idx_range  = std::min(static_cast<int>(dist_xy / gap_range), NUM_RANGE - 1);
    rah.idx_angle  = std::min(static_cast<int>(theta / gap_angle),   NUM_ANGLE - 1);
    rah.idx_height = std::min(static_cast<int>((phi - FOV_d)/gap_height), NUM_HEIGHT - 1);

    return rah;
}

• 每个点的三维 bin 索引为：

$$id{x}_{range}=\lfloor \frac{r}{R_{max}}\cdot N_r\rfloor$$

$$id{x}_{angle}=\lfloor \frac{\theta }{360^{\circ }}\cdot N_a\rfloor$$

$$id{x}_{height}=\lfloor \frac{\varphi -FO{V}_d}{FO{V}_u-FO{V}_d}\cdot N_h\rfloor$$

• 其中 $r=\sqrt{x^2+y^2}$ 为水平距离，$\theta \in [0^{\circ },360^{\circ })$ 为方位角（xy2theta 手动四象限 atan，和 SC 完全一致），$\varphi =\arctan 2(z,r)$ 为仰角

• FOV_u - FOV_d 就是垂直视场总范围，除以 NUM_HEIGHT 得到每层的高度分辨率。FOV_d 和 FOV_u 不同雷达不同值——换传感器只改这两个参数即可，分箱逻辑不变

  • FOV_d：雷达垂直视场的下界（down），即最低能看到的仰角。Velodyne HDL-64E 默认 -24.8°，Livox Avia 默认 -38.6°
  • FOV_u：雷达垂直视场的上界（up），即最高能看到的仰角。Velodyne HDL-64E 默认 +2°，Livox Avia 默认 +38.6°

说人话：离雷达越远，idx_range 越大（80m 切成 40 环，每环 2m）；方位角越大，idx_angle 越大（360° 切成 60 列，每列 6°）；仰角越高，idx_height 越大（FOV 范围 $FO{V}_u-FO{V}_d$ 切成 32 层）。FOV_d 和 FOV_u 是可配参数——换雷达只需改这两个值，分箱自动适配

• xy2theta 和 SC 的同名函数一模一样——手动四象限映射，不用 atan2：

// solid.cpp — xy2theta：手动四象限 atan → [0°, 360°)
float xy2theta(float &x, float &y)
{
    if (x >= 0 && y >= 0)       // 第一象限：(0°, 90°)
        return (180/M_PI) * atan(y/x);
    if (x < 0 && y > 0)         // 第二象限：(90°, 180°)
        return 180 - ((180/M_PI) ) * atan(y/(-x));
    if (x < 0 && y < 0)         // 第三象限：(180°, 270°)
        return 180 + ((180/M_PI) ) * atan(y/x);
    if (x >= 0 && y < 0)        // 第四象限：(270°, 360°)
        return 360 - ((180/M_PI) * atan((-y)/x));
}

说人话：pt2rah 做的就是给点云中的每个点发一个"三维门牌号"——第几圈（range）、第几扇（angle）、第几层（height）。有了门牌号，下一步就是统计每层住多少人。

3-2 双投影矩阵：Range-Height 与 Angle-Height

• Step 2 — 双投影计数矩阵。这一步构建了两个矩阵：

$$R(r,h)=\sum _{p_i\in \text{bin}(r,h)}1,A(a,h)=\sum _{p_i\in \text{bin}(a,h)}1$$

• range_matrix（$40\times 32$）：第 $(r,h)$ 格 = “距离为第 $r$ 环、高度为第 $h$ 层的点有多少个”
• angle_matrix（$60\times 32$）：第 $(a,h)$ 格 = “方位角为第 $a$ 扇区、高度为第 $h$ 层的点有多少个”
• 两个矩阵共享高度维度——它们是从两个不同视角（距离 vs 方位角）看同一帧点云的"垂直剖面"

注意：SOLiD 存的是计数（密度），而不是 SC 的 max_z。这个选择在 FOV 受限时至关重要——计数保留了完整的空间分布，不会因为一个飞点而改变

• 为了直观理解 RAH 分箱的 3D 结构，我们模拟了一个有限 FOV 场景（前方 120° 扇形区域，两堵墙壁 + 地面），展示原始点云和两个投影矩阵：
  

• 三幅图从左到右对应 SOLiD 的分箱逻辑：

  • 左图（3D 点云）：模拟前方 120° 有限 FOV 场景，两堵墙壁（橙色 Wall A 在 ~13m 处，青色 Wall B 在 ~32m 处）和地面。红色虚线为 FOV 边界，后方 240° 完全没有点——这就是 SC 会出问题的典型场景
  • 中图（Range-Height 矩阵 40×32）：沿 Angle 轴坍缩——每行 = 一个距离环上各高度层的点数分布。可以清晰看到地面（底部亮带）、Wall A（~13m 处高层亮斑）、Wall B（~32m 处中层亮斑）——不同距离环的垂直结构"指纹"截然不同，且旋转不变
  • 右图（Angle-Height 矩阵 60×32）：沿 Range 轴坍缩——每行 = 一个方位扇区上各高度层的点数分布。前方 120° 以外全是空列（灰色盲区）——Range-SOLiD 旋转不变（中图），Angle-SOLiD 保留角度顺序（右图，列可循环平移做偏航对齐）

说人话：左图就是你透过 120° 门缝看到的场景。中图是"不管方向，只管距离——每个距离环上的垂直密度分布"，转圈不变。右图是"不管距离，只管方向——每个方向上的垂直密度分布"，转圈会平移。两幅热力图都是原始计数，颜色越亮 = 这个 bin 里点越多。后续 3-3 到 3-4 的垂直加权，就是让这两个矩阵坍缩成 100 维向量的过程。

• 对应的 C++ 代码：

// solid.cpp — makeSolid 中的矩阵初始化 + 累加计数
// ===== 1. 初始化：两个计数矩阵 + 输出向量 =====
Eigen::MatrixXd range_matrix(NUM_RANGE, NUM_HEIGHT);   // 40 × 32
Eigen::MatrixXd angle_matrix(NUM_ANGLE, NUM_HEIGHT);   // 60 × 32
Eigen::VectorXd solid(NUM_RANGE + NUM_ANGLE);           // 100 维输出
range_matrix.setZero();
angle_matrix.setZero();
solid.setZero();

float gap_angle  = 360.0 / NUM_ANGLE;                   // 6°/格
float gap_range  = static_cast<float>(MAX_DISTANCE) / NUM_RANGE;  // 2m/格
float gap_height = (FOV_u - FOV_d) / NUM_HEIGHT;        // (2-(-24.8))/32 ≈ 0.8375°/格

// ===== 2. 遍历点云：累加计数 =====
for(int i = 0; i < scan_down.points.size(); i++)
{
    RAH rah = pt2rah(scan_down.points[i], gap_angle, gap_range, gap_height);
    range_matrix(rah.idx_range, rah.idx_height) += 1;  // 距离-高度 计数矩阵
    angle_matrix(rah.idx_angle, rah.idx_height) += 1;  // 方位角-高度 计数矩阵
}

3-3 垂直分布权重向量：number_vector

• Step 3 — 垂直分布权重向量（number_vector）。这是 SOLiD 最精妙的设计：

$$w_h=\frac{{\sum }_{r=0}^{N_r-1}R(r,h)-w_{\min }}{w_{\max }-w_{\min }},h=0,1,...,N_h-1$$

* $w_h$ 的含义：**第 $h$ 个高度层内所有距离环的点数总和，归一化到 $[0, 1]$**
* 为什么用 `range_matrix` 的列和而不是 `angle_matrix` 的列和？两者的列和本质相同（总点数相同），选 `range_matrix` 没有特别含义，任意一个都行
* 关键洞察：**高度层点的多少直接反映垂直结构的存在与否**——地面层（低仰角）点最多，天花板层（高仰角）在室内点也多，但中间层（墙壁立面）的分布才是不同地点真正的"指纹"差异

说人话：number_vector 告诉你"雷达扫描到的场景在垂直方向上是怎么分布的"——哪些高度层点最多（墙壁中间的立面），哪些高度层点最少（天花板附近过渡区）。这个分布模式在不同地点截然不同——比如开阔广场的低层点密集（地面）、高层几乎无点；窄巷子的中层点密集（两侧墙壁）、地面可能反而少。这个 32 维向量就是 SOLiD 的"垂直指纹核心"。

• 对应的 C++ 代码：

// solid.cpp — makeSolid 中的垂直分布权重向量
// ===== 3. 计算垂直分布权重向量 =====
Eigen::VectorXd number_vector(NUM_HEIGHT);  // 32 维
number_vector.setZero();
for(int col_idx = 0; col_idx < range_matrix.cols(); col_idx++)
{
    number_vector(col_idx) = range_matrix.col(col_idx).sum();
    // 含义：第 col_idx 个高度层一共包含多少个点
}
// 归一化到 [0, 1]
double min_val = number_vector.minCoeff();
double max_val = number_vector.maxCoeff();
number_vector = (number_vector.array() - min_val) / (max_val - min_val);

3-4 加权投影与描述符输出

• Step 4 — 加权投影。Range-SOLiD 和 Angle-SOLiD 的计算：

$$\text{r\_solid}(r)=\sum _{h=0}^{N_h-1}R(r,h)\cdot w_h,r=0,1,...,N_r-1$$

$$\text{a\_solid}(a)=\sum _{h=0}^{N_h-1}A(a,h)\cdot w_h,a=0,1,...,N_a-1$$

* 矩阵乘法的结果：
	* `range_solid`（$40 \times 1$）：每个距离环上，所有高度层的点数用 $w_h$ 加权求和——**垂直结构丰富的距离环获得更高值**
	* `angle_solid`（$60 \times 1$）：每个方位扇区上，所有高度层的点数用 $w_h$ 加权求和——**垂直结构丰富的方向获得更高值**
* 注意：这里的加权是直接用点数的——`number_vector` 是归一化后的"每个高度层有多少点"。所以最终是加权和，点密集的高度层对最终描述符的贡献更大

• Step 5 — 拼接为 100 维向量：
  $$\text{SOLiD}=[{\text{r\_solid}}_{40}\Vert {\text{a\_solid}}_{60}]\in {\mathbb{R}}^{100}$$

• 100 维，对比 SC 的 $20\times 60=1200$ 维——信息压缩比 12 倍。更紧凑意味着更快的比对、更少的内存、更适合嵌入式平台。

• 关键设计洞察：为什么是两个投影而不是一个？

  • range_solid 的每一维 = 一个距离环上的"加权点密度"——不关心角度，只管"距离几米到几米之间点密不密集"。机器人原地旋转时，距离分布完全不变 → 旋转不变
  • angle_solid 的每一维 = 一个方位扇区上的"加权点密度"——保留了空间排列，各维度遵循方位角顺序排列。机器人转了 $n\times 6°$，angle_solid 就循环平移 $n$ 格 → 旋转敏感，可估计 yaw
  • 这和 SC 的 Ring Key + Sector Key 是同一个思路——但 SOLiD 不是"后来算出来"的辅助指纹，而是"一开始就设计好"的两个正交投影

SOLiD 的本质：把 3D 空间信息通过双视角投影 + 垂直加权压缩为一个 100 维向量——Range-SOLiD 负责回答"是不是来过这儿"（旋转不变的相似度比对），Angle-SOLiD 负责回答"上次朝哪个方向看的"（循环平移匹配）。FOV 再窄，只要有垂直结构，两个投影就能捕获足够的地点区分信息。

• 对应的 C++ 代码——加权投影 + 拼接输出：

// solid.cpp — makeSolid 中的加权投影与拼接
// ===== 4. 加权投影：矩阵 × 权重向量 = 紧凑描述符 =====
Eigen::VectorXd range_solid = range_matrix * number_vector;  // 40 × 32  ·  32 × 1  →  40 × 1
Eigen::VectorXd angle_solid = angle_matrix * number_vector;  // 60 × 32  ·  32 × 1  →  60 × 1

// ===== 5. 拼接：Range-SOLiD(40维) + Angle-SOLiD(60维) = 100维 =====
solid.head(NUM_RANGE) = range_solid;   // 前 40 维
solid.tail(NUM_ANGLE) = angle_solid;   // 后 60 维
return solid;

3-5 Python 实现对照

• 官方同时提供了 Python 版本，逻辑完全一致但更易读。以下是核心对应关系：

• pt2rah（Python 版）——和 C++ 版一一对应：

# solid.py — pt2rah（Python 版）
def pt2rah(point, gap_ring, gap_sector, gap_height, 
           num_ring, num_sector, num_height, fov_d):
    x = point[0]; y = point[1]; z = point[2]
    
    if(x == 0.0): x = 0.001  
    if(y == 0.0): y = 0.001 

    theta   = xy2theta(x, y) 
    faraway = np.sqrt(x*x + y*y) 
    # 注意：Python 版直接用 rad2deg(atan2(z, r)) - fov_d，把仰角归一到以 fov_d 为基准
    phi     = np.rad2deg(np.arctan2(z, np.sqrt(x**2 + y**2))) - fov_d

    idx_ring   = np.divmod(faraway, gap_ring)[0]      
    idx_sector = np.divmod(theta, gap_sector)[0]   
    idx_height = np.divmod(phi, gap_height)[0]
    
    if(idx_ring >= num_ring):     idx_ring = num_ring - 1
    if(idx_height >= num_height): idx_height = num_height - 1

    return int(idx_ring), int(idx_sector), int(idx_height)

• ptcloud2solid（Python 版）——makeSolid 的等价实现：

# solid.py — ptcloud2solid
def ptcloud2solid(ptcloud, fov_u, fov_d, num_sector, num_ring, num_height, max_length):
    num_points = ptcloud.shape[0]               
    
    gap_ring   = max_length / num_ring            # 2m/格
    gap_sector = 360 / num_sector                 # 6°/格
    gap_height = (fov_u - fov_d) / num_height     # 约 0.84°/格

    rh_counter = np.zeros([num_ring, num_height])    # Range-Height 计数矩阵
    sh_counter = np.zeros([num_sector, num_height])  # Angle-Height 计数矩阵
    
    for pt_idx in range(num_points): 
        point = ptcloud[pt_idx, :]
        idx_ring, idx_sector, idx_height = pt2rah(
            point, gap_ring, gap_sector, gap_height, 
            num_ring, num_sector, num_height, fov_d) 
        try:
            rh_counter[idx_ring, idx_height]   += 1     
            sh_counter[idx_sector, idx_height] += 1  
        except:
            pass  # 越界的点直接丢弃
            
    ring_matrix   = rh_counter    
    sector_matrix = sh_counter
    
    # 垂直分布权重向量
    number_vector = np.sum(ring_matrix, axis=0)
    min_val = number_vector.min()
    max_val = number_vector.max()
    number_vector = (number_vector - min_val) / (max_val - min_val)
        
    # 加权投影
    r_solid = ring_matrix.dot(number_vector)    # Range-SOLiD (40,)
    a_solid = sector_matrix.dot(number_vector)  # Angle-SOLiD (60,)
            
    return r_solid, a_solid

• 回环检测与偏航估计（Python 版）——test.py 中的主流程：

# test.py — 回环检测 + 偏航估计
# Range-SOLiD 余弦相似度（和 C++ 版 loop_detection 等价）
similarity1 = np.dot(query_rsolid, candidates1_rsolid) / \
              (np.linalg.norm(query_rsolid) * np.linalg.norm(candidates1_rsolid))

# Angle-SOLiD 循环移位 + L1 距离（和 C++ 版 pose_estimation 等价）
if similarity1 > similarity2:
    initial_cosdist = []
    for shift_index in range(len(candidates1_asolid)):
        # np.roll 实现循环移位，L1 距离 = sum|a - b|
        initial_cosine_similarity = np.sum(
            np.abs(query_asolid - np.roll(candidates1_asolid, shift_index)))
        initial_cosdist.append(initial_cosine_similarity)
    initial_angle = np.argmin(initial_cosdist) * 6  # 每格 6°

• Python 版和 C++ 版的唯一区别：Python 版把 range_solid 和 angle_solid 作为两个独立返回值（r_solid, a_solid），而 C++ 版拼接成一个 100 维向量。逻辑完全一致。

3-6 参数配置总览

• 回顾 SOLiD 的全部可配参数——这些常量在 solid_module.h 中定义，贯穿所有代码：

// solid_module.h — 全部参数
// ===== FOV 参数（最关键——针对不同雷达必须调整）=====
const float FOV_u = 2;        // FOV 上界（仰角上限，单位：度）
const float FOV_d = -24.8;    // FOV 下界（仰角下限，单位：度）
                              // 默认值适配 Velodyne HDL-64E

// ===== 分箱参数 =====
const int NUM_ANGLE  = 60;    // 方位角分箱数（360°/60 = 6°/格）
const int NUM_RANGE  = 40;    // 距离分箱数（80m/40 = 2m/格）
const int NUM_HEIGHT = 32;    // 高度分箱数（(FOV_u-FOV_d)/32 ≈ 0.84°/格）

// ===== 距离裁剪参数 =====
const int MIN_DISTANCE = 3;   // 最近有效距离（米），排除近处噪声/地面
const int MAX_DISTANCE = 80;  // 最远有效距离（米），排除稀疏远点

// ===== 降采样参数 =====
const float VOXEL_SIZE = 0.4; // 体素降采样分辨率（米）

• 不同雷达的 FOV 参数对照：

雷达型号	类型	水平 FOV	垂直 FOV	FOV_d	FOV_u	Channels
Velodyne HDL-64E	机械旋转	360°	+2° ~ -24.8°	-24.8	2	64
Velodyne VLP-16	机械旋转	360°	+15° ~ -15°	-15	15	16
Ouster OS1-64	机械旋转	360°	+22.5° ~ -22.5°	-22.5	22.5	64
Livox Avia	固态非重复	~70°	+38.6° ~ -38.6°	-38.6	38.6	—
Livox Mid-360	固态圆形	~70°（圆锥）	取决于指向	取决于安装	取决于安装	—
Aeva 4D LiDAR	FMCW	取决于配置	+9.6° ~ -9.6°	-9.6	9.6	—

• 关键点：FOV_u 和 FOV_d 不是论文定死的，而是根据实际雷达型号可调的。换成 Livox Avia，就设 FOV_d = -38.6、FOV_u = 38.6——描述符的分箱自动适配新的 FOV。这是 SOLiD 应对"有限 FOV"的核心机制——不是硬编码参数，而是把 FOV 当成输入，分箱逻辑随 FOV 自动缩放。

---

4 SOLiD 回环检测与偏航估计

4-1 回环检测：Range-SOLiD 的余弦相似度

• 有了 SOLiD 描述符后，回环检测变得极其简单——只用 range_solid（前 40 维），算余弦相似度：

// solid.cpp — loop_detection：余弦相似度
double SOLiDModule::loop_detection(const Eigen::VectorXd &query, 
                                    const Eigen::VectorXd &candidate)
{
    // 取出前 40 维：Range-SOLiD
    Eigen::VectorXd r_query     = query.segment(0, NUM_RANGE);
    Eigen::VectorXd r_candidate = candidate.segment(0, NUM_RANGE);

    // 余弦相似度 = (a·b) / (|a|·|b|)，范围 [-1, 1]，越大越相似
    double cosine_similarity = (r_query.dot(r_candidate)) / 
                               (r_query.norm() * r_candidate.norm());
    return cosine_similarity;
}

• 余弦相似度公式：
  $$\text{sim}(q,c)=\frac{r_q\cdot r_c}{\Vert r_q\Vert \cdot \Vert r_c\Vert }=\frac{{\sum }_{i=1}^{N_r}{r}_q[i]\cdot r_c[i]}{\sqrt{\sum r_q[i{]}^2}\cdot \sqrt{\sum r_c[i{]}^2}}$$

• 结果在 $[-1,1]$ 之间，越接近 1 越相似。在 test.cpp 中通过简单比较相似度大小来判定回环：

// test.cpp — 回环判决
double similarity1 = solid.loop_detection(query_solid, candidates1_solid);
double similarity2 = solid.loop_detection(query_solid, cadidates2_solid);

if (similarity1 > similarity2) {
    std::cout << "Candidates 1 is decided a revisited place!! " << std::endl;
    initial_angle = solid.pose_estimation(query_solid, candidates1_solid);
}

• 注意：官方 demo 版本的 loop_detection 是极简实现——直接比两个候选帧和当前帧的相似度，取更高的那个。在实际 SLAM 系统中，你需要自己加一个余弦相似度阈值（类似 SC 的 SC_DIST_THRES = 0.13），低于阈值的才判定为回环。论文中默认阈值为 0.95（范围 $[-1,1]$，0.95 是一个非常高的阈值，强调 precision 优先）

• 为什么 Range-SOLiD 是旋转不变的？

  • range_solid 的每个元素对应一个距离环上的"加权点密度"，不涉及方位角
  • 机器人在原地旋转——激光雷达的角度在变，但每个距离环内的点数分布不变（墙壁依然是同样的距离环，只是方向变了）
  • 本质上：Range-SOLiD 是 3D 体素网格沿 Angle 轴的"坍缩"——就像把一卷胶卷沿圆周方向压扁，只看得见径向的密度分布，看不清角度排列

• 为了直观验证，我们取同一帧 KITTI 点云，原地绕 Z 轴旋转 36° 后分别计算 Range-SOLiD，再取另一帧不同地点的 Range-SOLiD 作为对照：
  

• 图中蓝色为原始帧、红色为旋转 36° 后的同一地点、绿色为不同地点（第 60 帧）——同一地点旋转前后 Range-SOLiD 完全重合（余弦相似度 = 1.0），不同地点则差异明显（cos = 0.71）。只要距离环上的垂直结构分布不变，Range-SOLiD 就不变——旋转不影响它。

• SC 对比：SC 的 Stage 1 用 20 维 Ring Key + KD 树做粗筛，Stage 2 用 1200 维完整描述符 + 列向余弦距离做精排。SOLiD 把这个两阶段流程简化了——100 维向量已经足够紧凑，直接用 Range-SOLiD（40 维）算余弦相似度就能区分绝大多数地点，不需要 KD 树。如果帧数极多，当然也可以对 Range-SOLiD 建 KD 树加速——SOLiD 的架构不排斥这一点。

说人话：Range-SOLiD 就是你闭着眼睛转圈——不管朝哪个方向，你脚下的地面、头顶的天花板、周围的墙壁——它们离你多远是不会因为你转圈而改变的。Range-SOLiD 忘掉了角度信息，只管"距离远近的密度分布"——所以怎么转圈数值都不变，适合做"我是不是大概在这个位置"的快速判断。

4-2 偏航估计：Angle-SOLiD 的循环移位匹配

• 回环检测只告诉你"这儿来过"，但还需要知道"上次和这次朝哪个方向差了多少度"——这是偏航估计，由 pose_estimation 完成：

// solid.cpp — pose_estimation：循环移位 + L1 距离，输出 yaw 角差
double SOLiDModule::pose_estimation(const Eigen::VectorXd &query, 
                                     const Eigen::VectorXd &candidate)
{
    // 取出后 60 维：Angle-SOLiD
    Eigen::VectorXd a_query     = query.segment(NUM_RANGE, NUM_ANGLE);
    Eigen::VectorXd a_candidate = candidate.segment(NUM_RANGE, NUM_ANGLE);

    double minL1normDist = std::numeric_limits<double>::max();
    int minIndex = 0;
    int numAngle = a_query.size();  // = 60
    
    // 遍历所有 60 种循环移位
    for(int shiftIndex = 0; shiftIndex < numAngle; ++shiftIndex)
    {
        // 将 query 循环右移 shiftIndex 格
        Eigen::VectorXd shiftedQuery = Eigen::VectorXd::Zero(numAngle);
        for (int i = 0; i < numAngle; ++i)
        {
            shiftedQuery((i + shiftIndex) % numAngle) = a_query(i);
        }
        // 计算 L1 距离（曼哈顿距离）：sum|a - b|
        double L1NormDist = (a_candidate - shiftedQuery).cwiseAbs().sum();
        if(L1NormDist < minL1normDist)
        {
            minL1normDist = L1NormDist;
            minIndex = shiftIndex;
        }
    }
    // 列偏移 → 角度差（每列 = 6°）
    double angleDifference = (minIndex + 1) * (360.0 / numAngle);
    return angleDifference;
}

• 数学表达——给定两个 Angle-SOLiD 向量 $a_q$ 和 $a_c$，找最优循环偏移 $k^{\ast }$：
  $$k^{\ast }=\arg \underset{k\in [0,N_a-1]}{\min }\sum _{i=0}^{N_a-1}\left|a_c[i]-a_q[(i+k)\mathrm{mod}{N}_a]\right|$$
  $$\Delta {\theta }_{\text{yaw}}=(k^{\ast }+1)\times 6^{\circ }$$

• 为什么用 L1 距离而不是余弦距离？

  • SC 用余弦距离是因为描述符合存的是 max_z——高度值本身有意义，用余弦能归一化幅度差异
  • SOLiD 描述符存的是加权计数（密度），不同地点的点数绝对值天然不同——L1 距离直接反映"密度分布差了多少"，不需要余弦归一化。而且 L1 距离计算更简单（绝对值求和，不需要点积和范数），在 60 维向量上遍历 60 次偏移基本不耗时

• 为什么是 minIndex + 1 乘 6°？

  • 代码中循环从 shiftIndex = 0 开始，0 对应偏移 1 格（第 0 列移到第 1 列）→ 实际偏移量是 shiftIndex + 1，对应的角度 = $(shiftIndex+1)\times 6°$
  • 注意：这里和 SC 的列偏移对齐逻辑完全一致——都是"列平移 N 格 ≈ yaw 旋转 $N\times 6°$"

• 同样用刚才那对旋转 36° 的点云验证 Angle-SOLiD 的偏航估计：
  

• 上图：Query（蓝色）和旋转 36° 后的 Candidate（红色）的 Angle-SOLiD——两条曲线波形几乎一样，但整体平移了 6 列（36° / 6°/列 = 6），箭头标注了偏移方向。下图：遍历 60 种循环偏移，计算每个偏移下两个 Angle-SOLiD 的 L1 距离——在 shift = 54（等价于反向平移 6 列 = -36°，绝对偏航 = 36°）处 L1 距离最小，恰好等于旋转造成的列平移量。60 次移位 + 60 次 L1 绝对值求和，毫秒级完成。

• SC 对比：SC 用 Sector Key（60 维）做粗对齐 + 完整描述符做精对齐（7 次 circshift + 列向余弦）。SOLiD 的 Angle-SOLiD（60 维）直接一把梭——遍历 60 个偏移、每个算一次 L1，总共 $60\times 60=3600$ 次绝对值加法——毫秒级完成，不需要粗对齐再精对齐的两阶段。

说人话：Angle-SOLiD 就是 SOLiD 里的"指南针"——它记得每个方向上的结构密度。你把当前帧的指南针"转"60 个角度（每次 6°），看哪个角度和最像的历史帧指南针对得最齐——对得最齐的那个角度，就是你们两次之间的偏航角差。

4-3 完整流程串联

• 至此，SOLiD 的完整管线可以总结为一条清晰的链路：
  • 一帧点云进入 → remove_closest_points / remove_far_points / down_sampling 三步预处理
  • 预处理后的点云 → pt2rah 为每个点计算 RAH 索引 → makeSolid 构建双投影矩阵 + 垂直加权 → 输出 100 维 SOLiD 描述符
  • 需要回环检测时 → loop_detection 取前 40 维 Range-SOLiD 算余弦相似度 → 相似度最高的即为回环候选
  • 确认回环后 → pose_estimation 取后 60 维 Angle-SOLiD 做循环移位 L1 匹配 → 输出 yaw 角差
  • 回环帧 ID + yaw 角差 → 直接作为 small_gicp 等精配准算法的初始值，完成"发现回环 → 验证回环 → 输出 6-DOF 约束"的完整闭环
• 和 Scan Context 的两阶段检索（Ring Key 粗筛 + 完整 SC 精排）相比，SOLiD 的流程更简洁——100 维向量一身兼两职，不需要额外的紧凑指纹。当然，如果历史帧数量极大（数万帧以上），对 Range-SOLiD 建 KD 树加速也是完全可行的

---

5 SOLiD vs Scan Context 对比

• 读到这里你已经发现了——SOLiD 和 SC 的很多设计思路是"同构"的，只是在维度上做了升级。下表做一次完整的一对一对比：

对比维度	Scan Context	SOLiD
空间分箱	2D 极坐标（Range × Angle）	3D 柱坐标（Range × Angle × Height）
编码方式	max_z（高度最大值）	计数（点数）
描述符维度	$20\times 60=1200$ 维矩阵	$40+60=100$ 维向量
旋转不变部分	Ring Key（20 维，事后对行取均值）	Range-SOLiD（40 维，Angle 轴坍缩，一次生成）
旋转敏感部分	Sector Key（60 维，事后对列取均值）	Angle-SOLiD（60 维，Range 轴坍缩，一次生成）
垂直信息利用	仅 max_z（一个标量）	完整的高度分层（32 层计数 + 加权）
粗筛方式	KD 树近邻搜索（Ring Key）	可直接余弦相似度（100 维够紧凑）
精排方式	Sector Key 粗对齐 + 列向余弦精对齐（~7 次）	Angle-SOLiD 循环移位 L1（60 次）
检索复杂度	$O(\log N)$ KD 树 + $O(K\cdot N_s)$ 精排	$O(N\cdot N_r+N_s)$ 直接比对（可加 KD 树）
适配 FOV 受限	差——空列 > 50% 时描述符退化	好——垂直信息弥补水平 FOV 缺失
内存占用	每帧 ~1200 floats + KD 树	每帧 100 floats
依赖	Eigen + nanoflann	Eigen + PCL
代码量	~500 行 C++	~150 行 C++

• 核心差异总结：

  • 信息维度：2D → 3D。SC 把 3D 点云压成 2D 描述符（丢失高度分布），SOLiD 用 3D 分箱保留了高度维度的分布信息——这是应对有限 FOV 的关键。FOV 越窄，水平信息越少，高度信息就越珍贵。SOLiD 的 32 层高度分箱 + 计数编码，让每一点垂直结构都被充分利用。

  • 编码方式：max_z → 计数。SC 的 max_z 在 360° 场景下稳定（柱状统计的大样本极值稳定），但在有限 FOV 下每个 bin 样本太少——max_z 对单个噪声/飞点极度敏感。SOLiD 改用计数——就像用投票代替独裁，一个飞点投一票，淹没在其他几十票里，不会颠覆整个 bin 的值。

  • 描述符结构：一个大矩阵 → 两个小向量。SC 输出 $20\times 60$ 矩阵，"旋转不变"和"旋转敏感"信息混在一起，需要事后提取 Ring Key 和 Sector Key。SOLiD 从一开始就设计成两个正交投影——Range-SOLiD（旋转不变）和 Angle-SOLiD（旋转敏感），一个描述符同时装了两个任务，不需要额外计算。

  • 工程简洁度。SOLiD 的代码只有 SC 的约 1/3，100 维向量可以直接做余弦比对，不需要 KD 树和两阶段搜索（当然帧数多时可以加）。不过这是 demo 版本的简单实现——在实际 SLAM 系统中，仍然建议对 Range-SOLiD 建 KD 树做粗筛。

---

6 附录：Python 可视化代码

6-1 RAH 分箱可视化（对应 3-2 节）

• 以下代码模拟一个有限 FOV 场景（前方 120° 扇形区域，两堵墙壁 + 地面），展示原始 3D 点云、Range-Height 计数矩阵和 Angle-Height 计数矩阵：
  

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.colors import PowerNorm

np.random.seed(42)

# ===== 模拟有限 FOV LiDAR 点云（前方 120°，含两堵墙 + 地面）=====
points = []

# 地面点（-1.5m ~ 0m，扇形分布，仅前方 120°）
for _ in range(8000):
    a = np.random.uniform(np.radians(-60), np.radians(60))
    r = np.random.exponential(20) + 3
    r = np.clip(r, 0, 80)
    x, y = r * np.cos(a), r * np.sin(a)
    z = np.random.uniform(-1.5, 0)
    points.append([x, y, z])

# 近处墙壁（10m~15m，θ 约 -30°~20°，高 6m）
for _ in range(4000):
    r = np.random.normal(13, 2)
    a = np.random.uniform(np.radians(-30), np.radians(20))
    r = np.clip(r, 0, 80)
    x, y = r * np.cos(a), r * np.sin(a)
    z = np.random.uniform(0.5, 6)
    points.append([x, y, z])

# 远处墙壁（25m~40m，θ 约 -10°~50°，高 3m）
for _ in range(3000):
    r = np.random.normal(32, 3)
    a = np.random.uniform(np.radians(-10), np.radians(50))
    r = np.clip(r, 0, 80)
    x, y = r * np.cos(a), r * np.sin(a)
    z = np.random.uniform(0.5, 3)
    points.append([x, y, z])

# 散乱噪声
for _ in range(2000):
    a = np.random.uniform(np.radians(-60), np.radians(60))
    r = np.random.uniform(5, 70)
    x, y = r * np.cos(a), r * np.sin(a)
    z = np.random.uniform(-0.5, 4)
    points.append([x, y, z])

points = np.array(points)
x, y, z = points[:, 0], points[:, 1], points[:, 2]

# ===== RAH 分箱参数 =====
MAX_DISTANCE = 80.0; NUM_RANGE = 40; NUM_ANGLE = 60; NUM_HEIGHT = 32
FOV_u = 2.0; FOV_d = -24.8  # Velodyne HDL-64E 默认值

gap_range  = MAX_DISTANCE / NUM_RANGE       # 2m/格
gap_angle  = 360.0 / NUM_ANGLE              # 6°/格
gap_height = (FOV_u - FOV_d) / NUM_HEIGHT   # ≈ 0.8375°/格

# 笛卡尔 → 极坐标
r_xy = np.sqrt(x**2 + y**2)
theta = np.degrees(np.arctan2(y, x))
theta = np.where(theta < 0, theta + 360, theta)  # → [0°, 360°)
phi = np.degrees(np.arctan2(z, r_xy))

# ===== 计算 RAH 索引 =====
idx_range  = np.floor(r_xy / gap_range).astype(int)
idx_angle  = np.floor(theta / gap_angle).astype(int)
idx_height = np.floor((phi - FOV_d) / gap_height).astype(int)

# 边界裁剪
idx_range  = np.clip(idx_range,  0, NUM_RANGE  - 1)
idx_angle  = np.clip(idx_angle,  0, NUM_ANGLE  - 1)
idx_height = np.clip(idx_height, 0, NUM_HEIGHT - 1)

# 过滤超出 FOV 范围的点（模拟有限 FOV：仅保留前方 120° 的点）
mask_fov = (theta >= 300) | (theta <= 60)  # 前方 120°（±60°）
mask_dist = r_xy <= MAX_DISTANCE
mask = mask_fov & mask_dist

idx_range_f  = idx_range[mask]
idx_angle_f  = idx_angle[mask]
idx_height_f = idx_height[mask]
x_f, y_f, z_f = x[mask], y[mask], z[mask]

# ===== 构建两个计数矩阵 =====
range_matrix  = np.zeros((NUM_RANGE,  NUM_HEIGHT))  # 40 × 32
angle_matrix  = np.zeros((NUM_ANGLE,  NUM_HEIGHT))  # 60 × 32
for i in range(len(x_f)):
    range_matrix[idx_range_f[i],  idx_height_f[i]] += 1
    angle_matrix[idx_angle_f[i],  idx_height_f[i]] += 1

# ===== 画图 =====
fig = plt.figure(figsize=(20, 6))

# --- 左图：2D 俯视图（X-Y 平面，按 Z 高度着色）---
ax1 = fig.add_subplot(1, 3, 1)
sc1 = ax1.scatter(x_f, y_f, c=z_f, cmap='viridis', s=0.5, alpha=0.5, vmin=-2, vmax=8)
# 标注雷达位置
ax1.scatter([0], [0], c='red', s=100, marker='*', zorder=10, edgecolors='white', linewidths=0.5)
ax1.text(0, 2, 'LiDAR', color='red', fontsize=10, ha='center', fontweight='bold')
# FOV 边界线
for deg_bound, label in [(-60, '-60°'), (60, '60°')]:
    rad = np.radians(deg_bound)
    ax1.plot([0, 85*np.cos(rad)], [0, 85*np.sin(rad)],
             'r--', linewidth=0.8, alpha=0.4)
    ax1.text(82*np.cos(rad), 82*np.sin(rad), label, fontsize=7, color='red', alpha=0.5)
# 距离环
for r in [20, 40, 60, 80]:
    circle = plt.Circle((0, 0), r, fill=False, color='gray', linewidth=0.3, alpha=0.3)
    ax1.add_patch(circle)
    ax1.text(r, 1, f'{r}m', fontsize=6, color='gray', alpha=0.5)
# 标注两堵墙
ax1.text(12, -2, 'Wall A\n(~13m)', fontsize=9, color='orange', fontweight='bold')
ax1.text(26, 22, 'Wall B\n(~32m)', fontsize=9, color='cyan', fontweight='bold')
ax1.set_xlabel('X (m)'); ax1.set_ylabel('Y (m)')
ax1.set_xlim(-85, 85); ax1.set_ylim(-85, 85)
ax1.set_aspect('equal')
ax1.set_title('Top-down View (X-Y plane)\nColored by Z-height, ~120° FOV', fontsize=12)
ax1.grid(True, alpha=0.2)
plt.colorbar(sc1, ax=ax1, label='Z (m)', shrink=0.7)

# --- 中图：Range-Height 矩阵（40×32），PowerNorm 提升暗部对比度 ---
ax2 = fig.add_subplot(1, 3, 2)
im2 = ax2.imshow(range_matrix.T, cmap='inferno', aspect='auto',
                 origin='lower', interpolation='nearest',
                 norm=PowerNorm(gamma=0.4, vmin=0, vmax=range_matrix.max()),
                 extent=[0, NUM_RANGE, 0, NUM_HEIGHT-1])
ax2.set_xlabel(f'Range index (0~{NUM_RANGE-1})')
ax2.set_ylabel(f'Height index (0~{NUM_HEIGHT-1})')
ax2.set_title(f'Range-Height Matrix ({NUM_RANGE}x{NUM_HEIGHT})\nPowerNorm gamma=0.4, count per bin',
              fontsize=12)
# 标注墙壁对应区域
ax2.annotate('Wall A\n(~13m, z=0.5~6m)', xy=(6.5, 28), fontsize=8,
             color='cyan', ha='center',
             bbox=dict(boxstyle='round', facecolor='black', alpha=0.7))
ax2.annotate('Wall B\n(~32m, z=0.5~3m)', xy=(16, 20), fontsize=8,
             color='cyan', ha='center',
             bbox=dict(boxstyle='round', facecolor='black', alpha=0.7))
ax2.annotate('Ground\n(z≈-1.5~0m)', xy=(20, 5), fontsize=8,
             color='lime', ha='center',
             bbox=dict(boxstyle='round', facecolor='black', alpha=0.7))
# 方向箭头
ax2.annotate('', xy=(35, 1), xytext=(5, 1),
             arrowprops=dict(arrowstyle='->', color='white', lw=1.5))
ax2.text(20, 0.3, 'near  <----- range ----->  far', fontsize=8,
         color='white', ha='center', fontweight='bold')
plt.colorbar(im2, ax=ax2, label='Point count')

# --- 右图：Angle-Height 矩阵（60×32），PowerNorm 提升暗部对比度 ---
ax3 = fig.add_subplot(1, 3, 3)
im3 = ax3.imshow(angle_matrix.T, cmap='inferno', aspect='auto',
                 origin='lower', interpolation='nearest',
                 norm=PowerNorm(gamma=0.4, vmin=0, vmax=angle_matrix.max()),
                 extent=[0, NUM_ANGLE, 0, NUM_HEIGHT-1])
ax3.set_xlabel(f'Angle index (0~{NUM_ANGLE-1}, 0deg~360deg)')
ax3.set_ylabel(f'Height index (0~{NUM_HEIGHT-1})')
ax3.set_title(f'Angle-Height Matrix ({NUM_ANGLE}x{NUM_HEIGHT})\nPowerNorm gamma=0.4, count per bin',
              fontsize=12)
# 标注前方 FOV 区域（绿色透明）
ax3.axvspan(0, 10, alpha=0.15, color='green')
ax3.axvspan(50, 60, alpha=0.15, color='green')
ax3.text(5, 31, 'Forward\nFOV', fontsize=7, color='green', ha='center', fontweight='bold')
ax3.text(55, 31, 'Forward\nFOV', fontsize=7, color='green', ha='center', fontweight='bold')
ax3.annotate('Wall A (ang≈-30°~20°)', xy=(5, 28), fontsize=8,
             color='cyan', ha='center',
             bbox=dict(boxstyle='round', facecolor='black', alpha=0.7))
ax3.annotate('Wall B (ang≈-10°~50°)', xy=(8, 20), fontsize=8,
             color='cyan', ha='center',
             bbox=dict(boxstyle='round', facecolor='black', alpha=0.7))
ax3.annotate('Blind zone\n(no points)', xy=(30, 15), fontsize=9,
             color='white', ha='center',
             bbox=dict(boxstyle='round', facecolor='gray', alpha=0.6))
plt.colorbar(im3, ax=ax3, label='Point count')

plt.tight_layout()
plt.savefig('solid_rah_binning.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"输入点数: {len(x_f)}")
print(f"Range-Height 矩阵: {range_matrix.shape} (非零bin: {np.count_nonzero(range_matrix)})")
print(f"Angle-Height 矩阵: {angle_matrix.shape} (非零bin: {np.count_nonzero(angle_matrix)})")
print(f"Range-Height 矩阵非零bin中: ground层(0~3)占 {np.count_nonzero(range_matrix[:, :4])}, "
      f"wall层(10~28)占 {np.count_nonzero(range_matrix[:, 10:28])}")

---

6-2 旋转不变性与偏航估计验证（对应 4-1/4-2 节）

• 以下代码用同一帧 KITTI 点云旋转 36° 来验证 SOLiD 的两个核心特性——Range-SOLiD 的旋转不变性与 Angle-SOLiD 的偏航可估计性。不依赖 open3d，可独立运行。

"""
Plot 4-1: Range-SOLiD rotation invariance
Plot 4-2: Angle-SOLiD yaw estimation via cyclic shift
Self-contained, no open3d dependency.
"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt

# ============================================================
# SOLiD implementation (self-contained, no open3d)
# ============================================================

def read_kitti_bin(bin_path):
    """Read KITTI format bin file"""
    dtype = [('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.float32)]
    scan = np.fromfile(bin_path, dtype=dtype)
    return np.stack((scan['x'], scan['y'], scan['z']), axis=-1)

def remove_close(points, thres=3.0):
    dists = np.sum(np.square(points[:, :3]), axis=1)
    return points[dists > thres * thres]

def remove_far(points, thres=80.0):
    dists = np.sum(np.square(points[:, :3]), axis=1)
    return points[dists < thres * thres]

def voxel_downsample(points, voxel_size=0.4):
    """Simple voxel grid downsampling using numpy only"""
    if len(points) == 0:
        return points
    voxel_indices = np.floor(points[:, :3] / voxel_size).astype(np.int64)
    _, unique_idx = np.unique(voxel_indices, axis=0, return_index=True)
    return points[np.sort(unique_idx)]

def xy2theta(x, y):
    if x >= 0 and y >= 0:
        return np.degrees(np.arctan(y / x))
    if x < 0 and y >= 0:
        return 180.0 - np.degrees(np.arctan(y / (-x)))
    if x < 0 and y < 0:
        return 180.0 + np.degrees(np.arctan(y / x))
    # x >= 0 and y < 0
    return 360.0 - np.degrees(np.arctan((-y) / x))

def pt2rah(point, gap_ring, gap_sector, gap_height, num_ring, num_sector, num_height, fov_d):
    x, y, z = point[0], point[1], point[2]
    if x == 0.0: x = 0.001
    if y == 0.0: y = 0.001

    theta = xy2theta(x, y)
    faraway = np.sqrt(x * x + y * y)
    phi = np.degrees(np.arctan2(z, np.sqrt(x**2 + y**2))) - fov_d

    idx_ring = int(np.divmod(faraway, gap_ring)[0])
    idx_sector = int(np.divmod(theta, gap_sector)[0])
    idx_height = int(np.divmod(phi, gap_height)[0])

    if idx_ring >= num_ring: idx_ring = num_ring - 1
    if idx_height >= num_height: idx_height = num_height - 1

    return idx_ring, idx_sector, idx_height

def ptcloud2solid(ptcloud, fov_u, fov_d, num_sector, num_ring, num_height, max_length):
    gap_ring = max_length / num_ring
    gap_sector = 360.0 / num_sector
    gap_height = (fov_u - fov_d) / num_height

    rh_counter = np.zeros((num_ring, num_height))
    sh_counter = np.zeros((num_sector, num_height))
    for pt_idx in range(ptcloud.shape[0]):
        point = ptcloud[pt_idx, :]
        try:
            idx_ring, idx_sector, idx_height = pt2rah(
                point, gap_ring, gap_sector, gap_height, num_ring, num_sector, num_height, fov_d)
            rh_counter[idx_ring, idx_height] += 1
            sh_counter[idx_sector, idx_height] += 1
        except:
            pass

    number_vector = np.sum(rh_counter, axis=0)
    min_val, max_val = number_vector.min(), number_vector.max()
    if max_val > min_val:
        number_vector = (number_vector - min_val) / (max_val - min_val)
    else:
        number_vector = np.ones_like(number_vector)

    r_solid = rh_counter.dot(number_vector)
    a_solid = sh_counter.dot(number_vector)
    return r_solid, a_solid

def get_descriptor(scan, fov_u, fov_d, num_height, max_length, num_ring=40, num_sector=60):
    return ptcloud2solid(scan, fov_u, fov_d, num_sector, num_ring, num_height, max_length)

# ============================================================
# Params (Velodyne HDL-64E)
# ============================================================
FOV_U = 24.8
FOV_D = -2.0
NUM_HEIGHT = 64
MAX_DIST = 80
MIN_DIST = 3
ROTATION_DEG = 36  # = 6 bins x 6 deg/bin

# ============================================================
# 1. Load & preprocess
# ============================================================
bin_path = "/tmp/solid/python/bin/kitti_00_60/000000.bin"
scan = read_kitti_bin(bin_path)
scan = remove_close(scan, MIN_DIST)
scan = remove_far(scan, MAX_DIST)
scan = voxel_downsample(scan, voxel_size=0.4)
print(f"Points after preprocessing: {scan.shape[0]}")

# ============================================================
# 2. Compute SOLiD for original (query)
# ============================================================
r_query, a_query = get_descriptor(scan, FOV_U, FOV_D, NUM_HEIGHT, MAX_DIST)

# ============================================================
# 3. Rotate point cloud around Z by ROTATION_DEG
# ============================================================
theta_rad = np.deg2rad(ROTATION_DEG)
cos_t, sin_t = np.cos(theta_rad), np.sin(theta_rad)
rot = np.array([[cos_t, -sin_t, 0],
                [sin_t,  cos_t, 0],
                [0,       0,    1]])
rotated_scan = scan @ rot.T

# ============================================================
# 4. Compute SOLiD for rotated (candidate)
# ============================================================
r_candidate, a_candidate = get_descriptor(rotated_scan, FOV_U, FOV_D, NUM_HEIGHT, MAX_DIST)

# Verify
cos_sim = np.dot(r_query, r_candidate) / (np.linalg.norm(r_query) * np.linalg.norm(r_candidate))
print(f"Range-SOLiD cosine similarity (should approx 1.0): {cos_sim:.6f}")

l1_dists = [np.sum(np.abs(a_query - np.roll(a_candidate, s))) for s in range(60)]
best_shift = np.argmin(l1_dists)
if best_shift <= 30:
    est_angle = best_shift * 6
else:
    est_angle = best_shift * 6 - 360
print(f"Best shift: {best_shift} bins, estimated yaw: {abs(est_angle)} deg (true yaw: {ROTATION_DEG} deg)")

# Different place for contrast
scan2 = read_kitti_bin("/tmp/solid/python/bin/kitti_00_60/000060.bin")
scan2 = remove_close(scan2, MIN_DIST)
scan2 = remove_far(scan2, MAX_DIST)
scan2 = voxel_downsample(scan2, voxel_size=0.4)
r_diff, a_diff = get_descriptor(scan2, FOV_U, FOV_D, NUM_HEIGHT, MAX_DIST)
cos_sim_diff = np.dot(r_query, r_diff) / (np.linalg.norm(r_query) * np.linalg.norm(r_diff))
print(f"Range-SOLiD cosine sim with different place: {cos_sim_diff:.4f}")

# ============================================================
# Colors
# ============================================================
C_QUERY  = '#2166ac'
C_CANDID = '#b2182b'
C_DIFF   = '#4dac26'
C_BG     = '#fafafa'
C_GRID   = '#e0e0e0'

# ============================================================
# Figure 4-1: Range-SOLiD rotation invariance
# ============================================================
fig1, ax1 = plt.subplots(figsize=(13, 4.5))
fig1.patch.set_facecolor(C_BG)
ax1.set_facecolor(C_BG)

x = np.arange(40)
width = 0.25
ax1.bar(x - width, r_query,    width, color=C_QUERY,  alpha=0.85, label='Query (original)')
ax1.bar(x,        r_candidate, width, color=C_CANDID, alpha=0.85, label=f'Candidate (rotated {ROTATION_DEG} deg) -- same place')
ax1.bar(x + width, r_diff,     width, color=C_DIFF,   alpha=0.60, label='Different place (frame 60)')

ax1.set_xlabel('Range index (0~39)', fontsize=11)
ax1.set_ylabel('Range-SOLiD value', fontsize=11)
ax1.set_title(f'Range-SOLiD Rotation Invariance | Same place cos={cos_sim:.4f}  vs  Different place cos={cos_sim_diff:.4f}',
              fontsize=12, fontweight='bold')
ax1.legend(fontsize=9, loc='upper right')
ax1.set_xlim(-1, 40)
ax1.grid(axis='y', color=C_GRID, linewidth=0.5)

plt.tight_layout()
out1 = 'solid_range_invariance.png'
fig1.savefig(out1, dpi=150, bbox_inches='tight', facecolor=fig1.get_facecolor())
print(f"Saved: {out1}")

# ============================================================
# Figure 4-2: Angle-SOLiD yaw estimation
# ============================================================
fig2, (ax_top, ax_bot) = plt.subplots(2, 1, figsize=(13, 7))
fig2.patch.set_facecolor(C_BG)

# Top: Angle-SOLiD overlay (with shift annotation)
ax_top.set_facecolor(C_BG)
x_a = np.arange(60)
ax_top.plot(x_a, a_query,     '-o', color=C_QUERY,  markersize=3, linewidth=1.5, label='Query (original)')
ax_top.plot(x_a, a_candidate, '-s', color=C_CANDID, markersize=3, linewidth=1.5, label=f'Candidate (rotated {ROTATION_DEG} deg)')

# Arrow to show shift
mid_h = (a_query.max() + a_query.min()) / 2
ax_top.annotate('', xy=(best_shift, mid_h), xytext=(0, mid_h),
                arrowprops=dict(arrowstyle='->', color=C_CANDID, lw=2.5, connectionstyle='arc3,rad=.3'))
ax_top.text(best_shift/2, mid_h * 1.15, f'Shift = {best_shift} bins\n=> Yaw = |{abs(est_angle)}| deg',
            ha='center', fontsize=10, color=C_CANDID, fontweight='bold')

for s in range(0, 60, 6):
    ax_top.axvline(x=s, color=C_GRID, linewidth=0.3, linestyle='--')

ax_top.set_xlabel('Angle index (0~59)', fontsize=11)
ax_top.set_ylabel('Angle-SOLiD value', fontsize=11)
ax_top.set_title(f'Angle-SOLiD: Rotation-Sensitive  |  Yaw = {ROTATION_DEG} deg => {ROTATION_DEG//6}-bin cyclic shift',
                 fontsize=12, fontweight='bold')
ax_top.legend(fontsize=9, loc='upper right')
ax_top.set_xlim(-0.5, 59.5)
ax_top.grid(axis='y', color=C_GRID, linewidth=0.5)

# Bottom: L1 distance vs shift
ax_bot.set_facecolor(C_BG)
colors_dist = [C_CANDID if s == best_shift else '#aaaaaa' for s in range(60)]
ax_bot.bar(range(60), l1_dists, color=colors_dist, alpha=0.85, width=0.7)
ax_bot.axvline(x=best_shift, color=C_CANDID, linewidth=2, linestyle='--',
               label=f'Min L1 at shift={best_shift}  =>  Yaw = |{abs(est_angle)}| deg')

ax_bot.set_xlabel('Shift index (0~59), each step = 6 deg', fontsize=11)
ax_bot.set_ylabel('L1 distance', fontsize=11)
ax_bot.set_title(f'Yaw Estimation: L1 Distance vs Cyclic Shift  |  True yaw = {ROTATION_DEG} deg  |  min L1 at shift {best_shift} => |{abs(est_angle)}| deg',
                 fontsize=12, fontweight='bold')
ax_bot.legend(fontsize=9, loc='upper right')
ax_bot.set_xlim(-0.5, 59.5)
ax_bot.grid(axis='y', color=C_GRID, linewidth=0.5)

plt.tight_layout()
out2 = 'solid_yaw_estimation.png'
fig2.savefig(out2, dpi=150, bbox_inches='tight', facecolor=fig2.get_facecolor())
print(f"Saved: {out2}")

---

总结

• 本文从 Scan Context 在有限 FOV 下的局限出发，系统性解读了 SOLiD 的数学原理与完整 C++/Python 源码实现，核心要点回顾：
  • 问题驱动（第 1 章）：SC 的 2D 极坐标 + max_z 编码在 360° FOV 下表现优异，但面对固态雷达或被遮挡的有限 FOV 时——大量空列、circshift 失效、max_z 对噪声敏感——三个问题导致描述符退化
  • 3D RAH 分箱（第 3-1 节）：升级为 Range × Angle × Height 三维体素分箱，每个体素统计点数——保留了完整的 3D 空间分布，而不是压成 2D
  • 双投影 + 垂直加权（第 3-2 至 3-4 节）：Range-Height 和 Angle-Height 两个计数矩阵，用垂直分布权重向量做加权投影——得到 40 维 Range-SOLiD 和 60 维 Angle-SOLiD，合计 100 维
  • 回环检测 + 偏航估计（第 4-1、4-2 节）：Range-SOLiD 做余弦相似度（旋转不变的回环判断），Angle-SOLiD 做循环移位 L1（yaw 角差估计）——和 SC 一样的"两步走"思路，但组件更紧凑
  • FOV 自适应（第 3-6 节）：FOV_u 和 FOV_d 根据实际雷达型号可配，分箱逻辑随 FOV 自动缩放——同一套代码适配 Velodyne、Ouster、Livox 等不同传感器
  • SC vs SOLiD（第 5 章）：127 维 vs 1200 维，3D 分箱 vs 2D 分箱，计数 vs 最大高度——SOLiD 不是 SC 的改进版，而是针对有限 FOV 的全新设计
• SOLiD 的作者里包括 Scan Context 的提出者 Giseop Kim——这意味着 SOLiD 不是对 SC 的否定，而是同一个团队对不同应用场景（360° FOV vs 有限 FOV）的正交解决方案。如果你用机械式 360° 雷达，SC 完全够用；如果你用固态雷达或 FOV 严重受限，SOLiD 是目前最优的选择。
• 如有错误，欢迎指出！
• 感谢观看！