4. 激光SLAM回环检测基础
任务目标
实现M2DP（Modified 2D Descriptor Place recognition）全局描述子的生成与匹配，并在标准数据集上评估其回环检测性能。M2DP将3D点云投影到多个2D平面，对每个平面生成密度签名，最终构成全局描述子，适用于激光SLAM中的位置识别与回环检测。
环境依赖
- Python 3.8+
- NumPy
- Open3D（用于点云读取与可视化，可选）
- Scikit-learn（用于KNN匹配）
核心实现
import numpy as np
from sklearn.neighbors import NearestNeighbors
class M2DP:
def __init__(self, num_planes=8, num_azimuth=16, num_rings=8):
"""
num_planes: 投影平面数量
num_azimuth: 方位角离散数
num_rings: 径向环数
"""
self.num_planes = num_planes
self.num_azimuth = num_azimuth
self.num_rings = num_rings
self.descriptor_dim = num_planes * num_azimuth * num_rings
def _generate_plane_directions(self):
"""生成均匀分布的平面法向量"""
# 使用斐波那契球面均匀采样
n = self.num_planes
indices = np.arange(n) + 0.5
phi = np.arccos(1 - 2 * indices / n)
theta = np.pi * (1 + 5 ** 0.5) * indices
normals = np.stack([
np.sin(phi) * np.cos(theta),
np.sin(phi) * np.sin(theta),
np.cos(phi)
], axis=1)
return normals
def _project_to_plane(self, points, normal):
"""将3D点投影到过原点的平面，返回2D坐标"""
# 投影矩阵 P = I - n*n^T
proj = np.eye(3) - np.outer(normal, normal)
projected = points @ proj.T
return projected[:, :2] # 取前两维作为平面坐标
def _compute_ring_signature(self, points_2d):
"""计算单个平面的环签名"""
# 计算极坐标
r = np.linalg.norm(points_2d, axis=1)
theta = np.arctan2(points_2d[:, 1], points_2d[:, 0])
# 归一化半径到[0,1]
max_r = np.max(r) if len(r) > 0 else 1.0
r_norm = r / (max_r + 1e-8)
# 离散化
ring_idx = np.clip((r_norm * self.num_rings).astype(int), 0, self.num_rings - 1)
azim_idx = np.clip(((theta + np.pi) / (2 * np.pi) * self.num_azimuth).astype(int),
0, self.num_azimuth - 1)
# 构建2D直方图
hist = np.zeros((self.num_azimuth, self.num_rings))
for a, r_i in zip(azim_idx, ring_idx):
hist[a, r_i] += 1
# 归一化
total = np.sum(hist)
if total > 0:
hist = hist / total
return hist.flatten()
def generate_descriptor(self, points):
"""
生成M2DP描述子
points: Nx3 numpy数组
返回: descriptor (num_planes*num_azimuth*num_rings,)
"""
if len(points) < 10:
return np.zeros(self.descriptor_dim)
# 中心化
centroid = np.mean(points, axis=0)
points_centered = points - centroid
# 生成平面方向
normals = self._generate_plane_directions()
descriptor_parts = []
for normal in normals:
# 投影到平面
pts_2d = self._project_to_plane(points_centered, normal)
# 计算环签名
signature = self._compute_ring_signature(pts_2d)
descriptor_parts.append(signature)
descriptor = np.concatenate(descriptor_parts)
# L2归一化
norm = np.linalg.norm(descriptor)
if norm > 0:
descriptor = descriptor / norm
return descriptor
@staticmethod
def match(desc1, desc2, threshold=0.5):
"""
匹配两个描述子
返回: (是否匹配, 余弦相似度)
"""
sim = np.dot(desc1, desc2) / (np.linalg.norm(desc1) * np.linalg.norm(desc2) + 1e-8)
return sim > threshold, sim
def evaluate_loop_closure(m2dp, db_descriptors, query_descriptors, db_labels, query_labels, k=1):
"""
评估回环检测性能
返回: 准确率@1
"""
if len(db_descriptors) == 0:
return 0.0
db_matrix = np.stack(db_descriptors)
query_matrix = np.stack(query_descriptors)
# KNN搜索
nn = NearestNeighbors(n_neighbors=min(k, len(db_matrix)), metric='cosine')
nn.fit(db_matrix)
distances, indices = nn.kneighbors(query_matrix)
correct = 0
for i, (dist_row, idx_row) in enumerate(zip(distances, indices)):
# 取最近邻
best_idx = idx_row[0]
if db_labels[best_idx] == query_labels[i]:
correct += 1
return correct / len(query_matrix)
# ============ 最小可复现示例 ============
if __name__ == "__main__":
# 生成模拟点云数据（真实使用时替换为激光雷达点云）
np.random.seed(42)
def generate_synthetic_cloud(num_points=500, noise=0.05):
"""生成模拟环境点云：墙面+地面+随机物体"""
# 地面
ground = np.random.uniform([-5, -5, -0.1], [5, 5, 0], (num_points // 2, 3))
# 墙面
wall = np.random.uniform([-5, -5, 0], [-4.5, 5, 3], (num_points // 4, 3))
# 随机物体
objects = np.random.uniform([-5, -5, 0], [5, 5, 3], (num_points // 4, 3))
cloud = np.vstack([ground, wall, objects])
cloud += np.random.normal(0, noise, cloud.shape)
return cloud
# 创建参考帧（数据库）
db_clouds = []
db_labels = []
for i in range(5):
cloud = generate_synthetic_cloud()
# 给每帧添加不同偏移模拟不同位置
cloud[:, 0] += i * 0.5
db_clouds.append(cloud)
db_labels.append(i)
# 创建查询帧（包含回环帧和全新帧）
query_clouds = []
query_labels = []
# 回环帧：与db[2]相同位置
loop_cloud = generate_synthetic_cloud()
loop_cloud[:, 0] += 2 * 0.5
query_clouds.append(loop_cloud)
query_labels.append(2)
# 全新帧：新位置
new_cloud = generate_synthetic_cloud()
new_cloud[:, 0] += 10.0
query_clouds.append(new_cloud)
query_labels.append(99) # 99表示无匹配
# 生成描述子
m2dp = M2DP(num_planes=8, num_azimuth=16, num_rings=8)
db_descs = [m2dp.generate_descriptor(c) for c in db_clouds]
query_descs = [m2dp.generate_descriptor(c) for c in query_clouds]
# 匹配测试
print("=== 描述子维度 ===")
print(f"描述子维度: {m2dp.descriptor_dim}")
print("\n=== 回环匹配测试 ===")
for i, q_desc in enumerate(query_descs):
best_sim = -1
best_idx = -1
for j, db_desc in enumerate(db_descs):
sim = np.dot(q_desc, db_desc)
if sim > best_sim:
best_sim = sim
best_idx = j
print(f"查询帧{i} (真实标签{query_labels[i]}): 最佳匹配db[{best_idx}] (标签{db_labels[best_idx]}), 相似度={best_sim:.4f}")
# 评估
print("\n=== 回环检测准确率 ===")
# 注意：query_labels中99表示无匹配，评估时只统计有匹配的帧
valid_indices = [i for i, l in enumerate(query_labels) if l != 99]
if valid_indices:
acc = evaluate_loop_closure(
m2dp, db_descs,
[query_descs[i] for i in valid_indices],
db_labels,
[query_labels[i] for i in valid_indices]
)
print(f"准确率@1: {acc:.2%}")
运行与验证
- 运行方式：将上述代码保存为
m2dp_demo.py，执行python m2dp_demo.py。 - 预期输出：
- 描述子维度为 8×16×8 = 1024。
- 查询帧0（真实标签2）应匹配到db[2]（标签2），相似度较高。
- 查询帧1（新位置）匹配到某个db帧，但相似度应明显低于回环帧。
- 准确率@1应为100%（仅1个有效回环查询）。
- 通过条件：回环帧正确匹配到对应数据库帧，且相似度显著高于非回环帧（通常差距>0.1）。
真实数据接入说明
使用真实激光雷达点云时，将点云转换为N×3的NumPy数组（去除NaN和无效点），直接调用 generate_descriptor() 即可。建议先对点云做体素滤波降采样（体素大小0.2m），以加速计算并提升描述子稳定性。数据库帧建议每0.5m～1m保存一帧，查询帧与数据库帧的匹配阈值需根据实际场景重新标定（通常余弦相似度0.6～0.8）。
常见问题
- 描述子对点云密度敏感：不同距离的点云密度差异大时，建议先做半径滤波或体素滤波统一密度。
- 平面数量影响：num_planes=8时描述子维度为1024，计算量适中；若实时性要求高可减至4（维度512），但判别力会下降。
- 匹配阈值选择：阈值过高会漏检，过低会误检。建议在验证集上绘制PR曲线确定最优阈值。