3. 激光SLAM回环检测基础

任务目标

实现一个基于点云直方图的全局描述子提取与匹配模块，用于激光SLAM中的回环检测。给定两帧点云，提取其直方图特征，通过特征相似度判断是否构成回环候选。

核心思路

将三维点云投影到球坐标系，按方位角、俯仰角划分网格，统计每个网格内点的分布（距离均值或点数），形成一维直方图向量作为全局描述子。匹配时计算两描述子的余弦相似度或卡方距离，超过阈值则判定为回环候选。

关键完整代码

import numpy as np
from scipy.spatial import KDTree

class PointCloudHistogramDescriptor:
    """基于球坐标网格统计的点云全局描述子"""
    def __init__(self, azim_bins=36, elev_bins=18, max_range=50.0):
        self.azim_bins = azim_bins      # 方位角分桶数 (360°/36=10°每桶)
        self.elev_bins = elev_bins      # 俯仰角分桶数
        self.max_range = max_range      # 有效距离上限(m)
        self.azim_step = 2 * np.pi / azim_bins
        self.elev_step = np.pi / elev_bins

    def compute(self, points):
        """
        输入: points (N,3) 点云坐标
        输出: desc (azim_bins*elev_bins,) 归一化直方图描述子
        """
        # 过滤距离过远的点
        dist = np.linalg.norm(points, axis=1)
        mask = (dist > 0.1) & (dist < self.max_range)
        pts = points[mask]
        if len(pts) == 0:
            return np.zeros(self.azim_bins * self.elev_bins)

        # 转球坐标
        x, y, z = pts[:, 0], pts[:, 1], pts[:, 2]
        r = np.linalg.norm(pts, axis=1)
        azim = np.arctan2(y, x) + np.pi          # [0, 2π)
        elev = np.arccos(np.clip(z / r, -1, 1))  # [0, π]

        # 分桶索引
        azim_idx = np.floor(azim / self.azim_step).astype(int)
        elev_idx = np.floor(elev / self.elev_step).astype(int)
        azim_idx = np.clip(azim_idx, 0, self.azim_bins - 1)
        elev_idx = np.clip(elev_idx, 0, self.elev_bins - 1)

        # 统计每桶内平均距离（比点数更鲁棒）
        flat_idx = azim_idx * self.elev_bins + elev_idx
        hist = np.zeros(self.azim_bins * self.elev_bins)
        for i in range(len(flat_idx)):
            hist[flat_idx[i]] += r[i]
        # 归一化
        norm = np.linalg.norm(hist)
        if norm > 0:
            hist /= norm
        return hist

    @staticmethod
    def match_score(desc1, desc2):
        """余弦相似度，范围[-1,1]，越大越相似"""
        return float(np.dot(desc1, desc2))

# ---------- 最小可复现验证 ----------
def generate_test_cloud(seed=0, yaw_offset=0.0):
    """生成带随机噪声的环形点云，模拟同一位置不同视角"""
    rng = np.random.default_rng(seed)
    n = 2000
    # 环形结构 + 随机噪声
    theta = rng.uniform(0, 2*np.pi, n)
    radius = 5.0 + rng.normal(0, 0.3, n)
    z = rng.normal(0, 0.5, n)
    x = radius * np.cos(theta)
    y = radius * np.sin(theta)
    pts = np.stack([x, y, z], axis=1)
    # 绕Z轴旋转模拟视角变化
    c, s = np.cos(yaw_offset), np.sin(yaw_offset)
    R = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
    return pts @ R.T

if __name__ == "__main__":
    # 1. 同一位置不同视角 → 应高相似度
    cloud_a = generate_test_cloud(seed=42, yaw_offset=0.0)
    cloud_b = generate_test_cloud(seed=42, yaw_offset=0.15)  # ~8.6°偏航

    # 2. 不同位置 → 应低相似度
    cloud_c = generate_test_cloud(seed=7, yaw_offset=0.0)

    desc_extractor = PointCloudHistogramDescriptor(azim_bins=36, elev_bins=18)

    desc_a = desc_extractor.compute(cloud_a)
    desc_b = desc_extractor.compute(cloud_b)
    desc_c = desc_extractor.compute(cloud_c)

    score_same = PointCloudHistogramDescriptor.match_score(desc_a, desc_b)
    score_diff = PointCloudHistogramDescriptor.match_score(desc_a, desc_c)

    print(f"同一位置(视角差8.6°) 相似度: {score_same:.4f}")
    print(f"不同位置              相似度: {score_diff:.4f}")
    print(f"判定阈值建议: 0.85")
    print(f"回环检测结果: 同一位置→{score_same > 0.85}, 不同位置→{score_diff > 0.85}")

运行与验证

依赖安装：

pip install numpy scipy

运行方式：

python histogram_descriptor.py

通过条件：程序输出中，score_same应明显高于score_diff（通常前者>0.9，后者<0.7），且回环检测结果两行均为True。若差异不明显，可增大azim_bins或减小噪声幅度。

关键说明

• 描述子维度为 azim_bins × elev_bins（本例36×18=648维），对点云密度不敏感，但对视角旋转敏感——绕Z轴旋转会导致直方图循环移位，实际工程中可结合傅里叶变换或滑动匹配处理。
• 本实现采用“桶内平均距离”作为统计量，相比纯点数统计对距离噪声更鲁棒。若场景为室内结构化环境，可改用桶内点数并配合归一化。
• 回环判定阈值需根据传感器噪声和场景自适应性调整，建议在真实数据上统计同类/异类相似度分布后选取。