2. 激光SLAM回环检测基础

任务目标

在已有Scan Context回环检测基础上，将点云反射强度编码进描述子，使回环检测在几何相似但强度分布不同的场景（如长走廊两端、对称结构）中仍能正确匹配。本案例实现强度扫描上下文(Intensity Scan Context, ISC)的核心编码与匹配流程，并给出与经典Scan Context的对比验证方法。

实现方案

ISC在Scan Context的俯仰角-径向距离二维网格划分基础上，为每个bin额外统计强度特征。具体做法：将点云按方位角(θ)和径向距离(r)划分网格，对落入同一bin的点云，计算其反射强度的统计量（均值），形成与几何描述子同尺寸的强度矩阵。匹配时，几何距离与强度距离加权求和，通过列偏移搜索消除视点朝向差异。

核心代码

import numpy as np
from scipy.spatial import KDTree

class IntensityScanContext:
    def __init__(self, num_sectors=60, num_rings=20, max_range=80.0):
        self.num_sectors = num_sectors      # 方位角分区数
        self.num_rings = num_rings          # 径向分区数
        self.max_range = max_range
        self.ring_gap = max_range / num_rings

    def _xy2scan(self, points_xy, intensities):
        """将点云映射到扇形网格，返回几何计数矩阵和强度均值矩阵"""
        # points_xy: (N,2) 雷达坐标系下的xy坐标
        # intensities: (N,) 对应反射强度
        dist = np.linalg.norm(points_xy, axis=1)
        ang = np.arctan2(points_xy[:, 1], points_xy[:, 0])  # [-pi, pi]
        ang = np.mod(ang, 2 * np.pi)  # [0, 2pi)

        ring_idx = np.floor(dist / self.ring_gap).astype(int)
        sector_idx = np.floor(ang / (2 * np.pi / self.num_sectors)).astype(int)

        # 过滤超出范围的点
        valid = (ring_idx < self.num_rings) & (sector_idx < self.num_sectors)
        ring_idx, sector_idx, intensities = ring_idx[valid], sector_idx[valid], intensities[valid]

        # 几何计数矩阵（用于后续可能的密度特征）
        geom_mat = np.zeros((self.num_rings, self.num_sectors), dtype=np.float32)
        np.add.at(geom_mat, (ring_idx, sector_idx), 1)

        # 强度累加与计数
        inten_sum = np.zeros((self.num_rings, self.num_sectors), dtype=np.float32)
        inten_cnt = np.zeros((self.num_rings, self.num_sectors), dtype=np.float32)
        np.add.at(inten_sum, (ring_idx, sector_idx), intensities)
        np.add.at(inten_cnt, (ring_idx, sector_idx), 1)

        # 强度均值，空bin置0
        inten_mean = np.divide(inten_sum, inten_cnt, out=np.zeros_like(inten_sum), where=inten_cnt > 0)
        return geom_mat, inten_mean

    def make_isc(self, points_xy, intensities):
        """生成ISC描述子：几何矩阵与强度矩阵堆叠"""
        geom, inten = self._xy2scan(points_xy, intensities)
        # 归一化强度到[0,1]便于加权
        if inten.max() > 0:
            inten = inten / inten.max()
        return np.stack([geom, inten], axis=0)  # (2, num_rings, num_sectors)

    @staticmethod
    def _column_shift_distance(desc1, desc2):
        """计算两描述子在不同列偏移下的距离，返回最小距离及对应偏移"""
        # desc: (2, R, S)
        geom1, inten1 = desc1[0], desc1[1]
        geom2, inten2 = desc2[0], desc2[1]
        S = geom1.shape[1]
        min_dist = float('inf')
        best_shift = 0
        for shift in range(S):
            geom2_shift = np.roll(geom2, shift, axis=1)
            inten2_shift = np.roll(inten2, shift, axis=1)
            # 几何距离：bin占用差异（L1归一化）
            geom_diff = np.sum(np.abs(geom1 > 0) ^ (geom2_shift > 0)) / (geom1.shape[0] * S)
            # 强度距离：仅对双方都非空的bin计算L1
            mask = (geom1 > 0) & (geom2_shift > 0)
            if mask.sum() > 0:
                inten_diff = np.sum(np.abs(inten1[mask] - inten2_shift[mask])) / mask.sum()
            else:
                inten_diff = 1.0
            total = geom_diff + 0.5 * inten_diff  # 强度权重0.5
            if total < min_dist:
                min_dist = total
                best_shift = shift
        return min_dist, best_shift

    def match(self, desc_query, desc_ref_list):
        """在参考描述子列表中查找最近邻，返回(最小距离, 索引, 列偏移)"""
        best_dist = float('inf')
        best_idx = -1
        best_shift = 0
        for i, desc_ref in enumerate(desc_ref_list):
            dist, shift = self._column_shift_distance(desc_query, desc_ref)
            if dist < best_dist:
                best_dist = dist
                best_idx = i
                best_shift = shift
        return best_dist, best_idx, best_shift

# ---------- 最小验证闭环 ----------
def generate_test_data():
    """生成两组模拟点云：同一位置不同视角 + 不同位置"""
    rng = np.random.default_rng(42)
    # 场景A：L形墙角结构
    def make_corner_scene(yaw_offset=0.0):
        pts = []
        inten = []
        # 墙面1：x=5, y从-10到10
        for y in np.linspace(-10, 10, 50):
            pts.append([5.0 * np.cos(yaw_offset) - y * np.sin(yaw_offset),
                        5.0 * np.sin(yaw_offset) + y * np.cos(yaw_offset)])
            inten.append(0.8)
        # 墙面2：y=5, x从-10到10
        for x in np.linspace(-10, 10, 50):
            pts.append([x * np.cos(yaw_offset) - 5.0 * np.sin(yaw_offset),
                        x * np.sin(yaw_offset) + 5.0 * np.cos(yaw_offset)])
            inten.append(0.3)  # 不同墙面不同强度
        return np.array(pts), np.array(inten)

    # 场景B：完全不同的空旷区域
    pts_b = rng.uniform(-20, 20, (100, 2))
    inten_b = rng.uniform(0.1, 1.0, 100)

    # 场景A的两次观测：第二次带90度偏航
    pts_a1, inten_a1 = make_corner_scene(0.0)
    pts_a2, inten_a2 = make_corner_scene(np.pi / 2)
    return (pts_a1, inten_a1), (pts_a2, inten_a2), (pts_b, inten_b)

if __name__ == "__main__":
    isc = IntensityScanContext()
    (pts1, inten1), (pts2, inten2), (pts_b, inten_b) = generate_test_data()

    desc1 = isc.make_isc(pts1, inten1)
    desc2 = isc.make_isc(pts2, inten2)
    desc_b = isc.make_isc(pts_b, inten_b)

    # 查询desc1，在[desc2, desc_b]中找最近邻
    dist, idx, shift = isc.match(desc1, [desc2, desc_b])
    print(f"最近邻索引: {idx} (期望0)")
    print(f"匹配距离: {dist:.4f}")
    print(f"列偏移: {shift} (期望接近15, 即90度对应60扇区/4)")

    # 对比：仅用几何信息（去掉强度通道）
    desc1_geo = desc1[0:1]
    desc2_geo = desc2[0:1]
    descb_geo = desc_b[0:1]
    dist_geo, idx_geo, _ = isc.match(desc1_geo, [desc2_geo, descb_geo])
    print(f"[纯几何]最近邻索引: {idx_geo} (期望0)")
    print(f"[纯几何]匹配距离: {dist_geo:.4f}")

运行与验证

依赖安装：

pip install numpy scipy

执行方式：将上述代码保存为isc_demo.py，直接运行：

python isc_demo.py

通过条件：

• 程序输出中“最近邻索引”为0，即正确识别出desc1与desc2（同一场景不同视角）更相似，而非与desc_b（不同场景）匹配；
• “列偏移”接近15（对应90°偏航，因为60扇区/360°×90°=15）；
• 对比纯几何模式，ISC在强度区分明显的场景中应给出更小的匹配距离（或至少不劣于纯几何），说明强度信息被有效利用。

关键说明

• 强度权重系数0.5为经验值，实际应用中需根据传感器强度分布调节；
• 本实现使用强度均值作为bin特征，也可替换为强度最大值或直方图分布；
• 几何距离采用占用差异的L1范数，强度距离仅在双方非空bin上计算，避免空bin引入噪声；
• 列偏移搜索覆盖360°全方向，保证对车辆航向变化的不变性。