Files
fusion_LCD/PIPELINE_OVERVIEW.md
2026-05-08 09:03:46 +08:00

23 KiB
Raw Blame History

模型输入输出与数据预处理流程详解

论文:Cross Fusion of Point Cloud and Learned Image for Loop Closure Detection


1. 整体架构概览

┌──────────────────────────────────────────────────────────────────────┐
│                          Fusion (总模型)                              │
│                                                                      │
│  ┌──────────────┐   ┌──────────────┐   ┌─────────────────────────┐  │
│  │   ImgHead    │   │   BEVHead    │   │     FusionHead          │  │
│  │  (图像分支)   │   │  (点云分支)   │   │   (跨模态融合)          │  │
│  │              │   │              │   │                         │  │
│  │  ALNet 提取  │   │  RICNN 提取  │   │  Cross-Attention        │  │
│  │  图像特征     │   │  BEV特征     │   │  融合多模态特征          │  │
│  └──────┬───────┘   └──────┬───────┘   └───────────┬─────────────┘  │
│         │                  │                       │                │
│         └──────────────────┴───────────────────────┘                │
│                            │                                        │
│                    ┌───────▼────────┐                               │
│                    │  NetVLAD 聚合   │                               │
│                    │  全局描述子     │                               │
│                    └───────┬────────┘                               │
│                            │                                        │
│                    ┌───────▼────────┐                               │
│                    │  UOT 位姿估计   │                               │
│                    │ (训练时)        │                               │
│                    └────────────────┘                               │
└──────────────────────────────────────────────────────────────────────┘

三种运行模式(由 config.yaml 中的 flag 控制):

  • bev: 仅使用点云BEV分支
  • img: 仅使用图像分支
  • fusion: 点云+图像融合(完整模式)

2. 数据预处理流水线

2.1 KITTI 数据集结构

KITTI/
└── sequences/
    └── XX/
        ├── velodyne/          # 激光雷达点云 (.bin, 每文件一行4个float: x,y,z,intensity)
        ├── image_2/           # 相机图像 (.png, 原始尺寸约~1242×375)
        ├── calib.txt          # 标定矩阵 (P2投影矩阵, Tr相机到雷达)
        ├── poses.txt          # GPS真实轨迹 (每行12个float, 3×4位姿矩阵)
        └── loop_GT_4m.pickle  # 闭环真值标注 (由 loop_gt.py 生成)

2.2 KITTI360 到 KITTI 格式转换 (preparedataset.py)

KITTI360 使用不同目录结构(2013_05_28_drive_XXXX_sync),通过 preparedataset.pyk3602k() 函数转换为标准 KITTI 格式:

  • 序列 ID 映射: KITTI360 src (0,3,4,5,6,7,9,10) → KITTI tgt (50,53,54,55,56,57,59,60)
  • 姿态从 cam0_to_world.txt 读取并通过 cam0_to_velo 外参转换为 Velodyne 坐标系
  • 点云和图像通过符号链接组织

2.3 闭环真值生成 (loop_gt.py)

输入: 序列中所有帧的 4×4 位姿矩阵

处理流程:

  1. 使用 KDTree 在 3D 位置空间 (x,y,z) 上建立索引
  2. 对每一帧,查询距离 ≤ 4m 的其他帧作为正样本(闭环)
  3. 排除时间上太近的帧(前后各 50 帧)
  4. 根据方向角差判断是否反向:角度差 > 90° 标记为反向闭环
  5. 距离 > 15m 的帧作为负样本

输出: loop_GT_4m.pickle,每帧的格式:

{
    'idx': 帧索引,
    'positive_idxs': [正样本帧索引列表],  # 距离 < 4m 且不在时间窗口内
}

2.4 KittiDataset 训练数据生成 (dataset.py)

2.4.1 数据加载 (train模式)

对每个训练样本(一个闭环对),加载 query 帧和 positive 帧:

步骤 操作 输入维度 输出维度
1 读图像 (H,W,3) ~1242×375 (H,W,3) 原始BGR
2 Cropped & Scaled (H,W,3) (192, 576, 3) 即 HR=384×0.5, WR=1152×0.5
3 读点云 .bin 每点4个float (N,4) [x,y,z,intensity]
4 投影到图像 (N,4) → 通过 P2×Tr 变换 (N,6) [x,y,z,i, u_pixel, v_pixel]
5 体素化BEV (N,6) (h_bev, w_bev, 7) BEV特征图

2.4.2 数据增强(训练时)

对 positive 帧施加随机变换(rt_mat())模拟位姿不确定性:

rand = np.random.random(6) * 2 - 1  # [-1, 1]
Rt = rt_mat(rand[0]*3°/180π,   # ±3° roll
            rand[1]*3°/180π,   # ±3° pitch
            rand[2]*180°/180π,  # ±180° yaw
            rand[3]*3m,         # ±3m x
            rand[4]*3m,         # ±3m y
            rand[5]*0.3m)       # ±0.3m z

2.4.3 点云到BEV体素化 (pointcloud_encoder())

输入: (N, 4)(N, 6) 点云

配置参数 (config.yaml):

bev_range: -32,-32,-2.5,32,32,1.5    # [xmin,ymin,zmin,xmax,ymax,zmax]
bev_resolution: 0.2                    # 体素分辨率 (m)
voxel_max_points: 100                  # 每体素最大点数
voxel_num: 15000                       # 最大体素数
voxel_sample: 'top'                    # 按z高度排序采样

体素化过程:

  1. 范围滤波: 过滤 bev_range 外的点
  2. 排序: 按 z 高度降序排列(优先保留高点)
  3. 体素赋值: 使用 Numba JIT 加速的 _points_to_voxel_reverse_kernel()
    • Z轴分辨率 = bev_range[5] - bev_range[2] = 4m单个体素覆盖全高度
    • 有效格网尺寸: (64/0.2, 64/0.2) = (320, 320) → 输出 BEV 图尺寸

输出BEV特征图: (320, 320, 7)

通道0: max_z / resolution_z   # 每体素最大z高度
通道1: mean_intensity         # 平均反射强度
通道2: density                # log(点数) 密度
通道3: voxel_center_x         # 体素中心x
通道4: voxel_center_y         # 体素中心y
通道5: voxel_center_z         # 体素中心z  
通道6: voxel_intensity_center  # 体素中心反射强度

Relation (像素-BEV关联): 当点云包含像素投影信息(shape[1]==6)时:

relation.shape = (M, 每个体素内的投影点数, 2)
relation[:,:,0] = [u_pixel, v_pixel]    # 图像像素坐标
relation[:,:,-1] = [bev_row, bev_col]    # BEV格网坐标

2.4.4 标签分数 (label_score)

用于训练 score 损失。通过在 query 和 positive 之间计算体素级匹配关系:

输入: query 体素中心 (x,y) 坐标、positive 体素中心 (x,y) 坐标

流程:

  1. 通过 pose_to_frame 将 query 体素变换到 positive 坐标系
  2. 做 NN 匹配(欧氏距离)
  3. 距离 < 阈值max(2, 前30%分位数)的体素对标记为匹配score=1

输出: (h_bev, w_bev, 2) — 两个通道分别对应 query 和 positive 的匹配标签

2.5 图像预处理细节

输入尺寸: 原始 KITTI 图像 ~1242×375 处理流程:

  1. crop_image() 裁剪到 1152×384(居中裁剪,按需零填充)
  2. 降采样到 576×192scale=0.5

最终输入图像: (192, 576, 3) 或 batch 中 (B, 3, 192, 576)


3. 模型输入输出详解

3.1 batch_dict 完整结构

训练时,KittiTotalLoader 的 collate 函数构造 train() 函数的输入 data:

data 的原始字段来自Dataset

字段 维度 说明
sequence (B,) Tensor 或 int 序列编号
id_query (B,) query帧索引
id_positive (B,) positive帧索引
bev_query (B, 320, 320, 7) query BEV特征图
bev_positive (B, 320, 320, 7) positive BEV特征图
img_query (B, 192, 576, 3) query图像 (numpy→tensor)
img_positive (B, 192, 576, 3) positive图像
pose_query (B, 4, 4) query位姿矩阵
pose_positive (B, 4, 4) positive位姿矩阵
pose_to_frame (B, 4, 4) query到positive的相对位姿 = inv(pose_pos) @ pose_query
label_score (B, 320, 320, 2) 匹配标签分数
relation (B, max_len, 每个体素点数, 2) 像素-BEV关联变长由collate padding

train() 预处理后送入 net 前

bev = cat([bev_query, bev_positive], dim=0)    # (2B, 320, 320, 7)
bev = bev.permute(0, 3, 1, 2)                   # (2B, 7, 320, 320)
img = cat([img_query, img_positive], dim=0)     # (2B, 192, 576, 3)
img = img.permute(0, 3, 1, 2)                   # (2B, 3, 192, 576)

送入模型的 batch_dict:

字段 维度 说明
bev (2B, 7, 320, 320) BEV输入前B个query后B个positive
img (2B, 3, 192, 576) 图像输入
relation (2B, max_len, K, 2) 像素-BEV关联
id_query (B,) query索引
id_positive (B,) positive索引
sequence (B,) 序列号
pose_to_frame (B, 4, 4) 相对位姿
pose_query (B, 4, 4) query位姿
pose_positive (B, 4, 4) positive位姿
label_score (B, 320, 320, 2) 匹配标签
batch_size int 2B

3.2 ImgHead 图像分支

输入: img (2B, 3, 192, 576)
     归一化: x = img[:, 0:3] / 255.0  → (2B, 3, 192, 576)

ALNet 骨干网络(类似 ALIKE 架构):

输入 输出 操作
block1 (2B,3,192,576) (2B,16,192,576) ConvBlock(3→16, ReLU+BN)
pool2+block2 (2B,16,192,576) (2B,32,96,288) MaxPool2d(2)+ResBlock(16→32)
pool4+block3 (2B,32,96,288) (2B,64,24,72) MaxPool2d(4)+ResBlock(32→64)
pool4+block4 (2B,64,24,72) (2B,128,6,18) MaxPool2d(4)+ResBlock(64→128)
特征聚合 4层特征拼接 (2B,128,192,576) 1×1 conv + 上采样 + concat
输出头 (2B,128,192,576) score (2B,1,192,576) + feat (2B,128,192,576) Conv1x1(128→129)

ImgHead 输出:

字段 维度 说明
score_img (2B, 1, 192, 576) 逐像素关键点得分 (sigmoid激活)
fea_img (2B, 128, 192, 576) 密集描述子图
key_pixels (2B, 150, 2) Top-150 关键点像素坐标 [row, col]
fea_kpl (2B, 128, 150) Top-150 关键点处描述子 (从fea_img采样)

关键点选择: NMS (radius=2, iter=2) + Top-K (k=150 由 kpts_number_img 配置)

3.3 BEVHead 点云分支

输入: bev (2B, 7, 320, 320)
     x = bev[:, 0:3, :, :]     → (2B, 3, 320, 320)   可视BEV (RGB-like)
     points = bev[:, 3:7, :, :] → (2B, 4, 320, 320)   体素中心xyzi
     guider = (bev[:, 2:3] > 0) → (2B, 1, 320, 320)   有效区域mask

RICNN 骨干网络旋转不变CNN:

输入 输出 操作
block1 (2B,3,320,320) (2B,16,320,320) RIConvBlock(3→16)
pool2+block2 (2B,16,320,320) (2B,32,160,160) RIMaxPool(2)+RIResBlock(16→32)
pool4+block3 (2B,32,160,160) (2B,64,40,40) RIMaxPool(5)+RIResBlock(32→64)
pool4+block4 (2B,64,40,40) (2B,128,10,10) RIMaxPool(5)+RIResBlock(64→128)
特征聚合 4层拼接 (2B,128,320,320) 1×1 conv + 上采样 + concat
输出头 (2B,128,320,320) score (2B,1,320,320) + feat (2B,128,320,320) Conv1x1(128→129)

旋转不变性: RICNN 使用 RIConv2d / RIMaxpool2d / RIAvgpool2d根据像素距离中心点的欧氏距离而非固定kernel位置分权重组在推理时可用 disable_ri() 转为标准 Conv2d/MaxPool2d

关键点选择(默认 select='maxpool':

  1. NMS (radius=3) 选出局部最大值
  2. 每个batch元素取 Top-K (k=150 = kpts_number_bev) 得分最高像素
  3. 提取对应的体素中心坐标和描述子

BEVHead 输出:

字段 维度 说明
score_bev (2B, 320, 320) 逐像素关键点得分
fea_bev (2B, 128, 320, 320) 密集描述子图
key_points (2B, 150, 4) Top-150 关键点体素坐标 [x,y,z,i]
pixels_kpt (2B, 150, 2) 关键点BEV像素坐标 [row, col]
fea_kpt_original (2B, 128, 150) 关键点描述子
vlad_bev (2B, 256) BEV NetVLAD全局描述子

EncodePosition位置编码

BEV 关键点位置编码:

# 输入: kpts (B, 150, 4)
# 计算150×150的距离矩阵
# 距离直方图 (bins=16, range=[1,80]m)
# → MLP(16→64→64→128) → 残差加到描述子上

3.4 FusionHead 跨模态融合

3.4.1 双向特征生成与转换

(1) 图像→BEV 特征采样:

# relation: (2B, max_len, K, 2)
# pixel_img: 图像像素位置 → grid_sample(fea_img) → fea_pl_dual
fea_pl_dual.shape = (2B, 128, max_len, K)  # 从图像特征图采样的像素特征
# LocalPool: Conv1x1(100→10) + MaxPool(1,10) 聚合每体素内多个像素点
fea_pl_dual  LocalPool  (2B, 128, max_len, 1)  squeeze  (2B, 128, max_len)
# Converter (cvt_bev): Self-Attention + Conv1d残差
fea_pl_dual  cvt_bev  fea_pt_dual_gen (2B, 128, max_len)

(2) BEV→图像特征采样 (训练时,有 pose_to_frame 时才执行)**:

# pixel_bev: BEV格网坐标 → grid_sample(fea_bev) → fea_pt_dual
fea_pt_dual  cvt_img  fea_pl_dual_gen (2B, 128, max_len)

3.4.2 全景特征生成 (Generator)

# 输入: fea_pt_dual_gen (B, 128, N)
# Self-Attention → ConvTranspose1d(k3,s3) → AdaptiveMaxPool1d(150)
fea_pt_dual_gen  gen_pan  fea_kpt_original_gen (B, 128, 150)
# 注意这个模块从图像特征生成与BEV关键点数量(150)对齐的特征

3.4.3 双路径转换器

# 路径1: BEV关键点特征 → cvt_img → fea_kpt (B, 128, 150)  残留在图像空间的特征
fea_kpt_original  cvt_img  fea_kpl_gen (B, 128, 150)

# 路径2: 路径1输出 → cvt_bev → 再回到BEV空间
fea_kpl_gen  cvt_bev  fea_kpt_gen_gen (B, 128, 150)

3.4.4 FusionHead Attention 融合

# 拼接4种特征: [original, gen, gen_gen, kpl_gen]
fea_kpts = cat([fea_kpt_original, fea_kpt_original_gen,
                fea_kpt_gen_gen, fea_kpl_gen], dim=2)
# 维度: (B, 128, 150, 4)

# 对每对匹配点 (共3对: 原始-生成, 原始-回环生成, 原始-图像残差)
# Self-Attention across pairs → 取max → Cross-Attention with kpl_gen
# 最终输出: fea_kpt_fusion (B, 128, 150)

3.4.5 FusionHead 输出

字段 维度 说明
fea_kpt_original (2B, 128, 150) BEV原始关键点特征
fea_kpt_fusion (2B, 128, 150) 融合后关键点特征当前实现等同original
fea_kpl (2B, 128, 150) 图像关键点特征
fea_pt_dual (2B, 128, max_len) 从BEV特征图采样的匹配点特征
fea_pl_dual (2B, 128, max_len) 从图像特征图采样的匹配点特征
fea_pt_dual_gen (2B, 128, max_len) 图像特征生成的点云特征
fea_pl_dual_gen (2B, 128, max_len) 点云特征生成的图像特征
fea_kpt_original_gen (2B, 128, 150) 全景生成器输出
fea_kpt_gen_gen (2B, 128, 150) 双路径转换器输出
fea_kpl_gen (2B, 128, 150) Converter从BEV特征生成的图像空间特征

3.5 NetVLAD 全局描述子

NetVLAD (标准版,实际使用)

输入: fea_kpt_fusion.unsqueeze(3)  # (2B, 128, 150, 1)
      soft_assign = Conv2d(12816)(x)  ReLU  Softmax  # (2B, 16, 150, 1)
      残差 = x - centroids[16×128]                         # (2B, 16, 150, 128)
      VLAD = sum(soft_assign * 残差, dim=2)               # (2B, 16, 128)
      归一化  flatten  归一化                            # (2B, 2048)

注意: 当前代码中 NetVLAD 的 fea_size=128, cluster_num=16,输出维度应为 16 × 128 = 2048。但在 Fusion__init__ 中:

self.netvlad_fusion = NetVLAD(feature_size, cfg['cluster_num_fusion'])

cluster_num_fusion: 16,所以 VLAD 输出维度 = 16 × 128 = 2048

但实际上看训练代码中 VLAD 维度配置为 vlad_size: 256,这个参数在 NetVLAD 类中没有被使用(不使用 NetVLADLoupe 时)。当前实现中 VLAD 维度固定为 cluster_num × fea_size = 2048

最终 VLAD 融合:

vlads = sigmoid(w) * vlad_fusion + (1 - sigmoid(w)) * vlad_bev
# w 是可学习参数

3.6 UOT (Unbalanced Optimal Transport) 位姿估计

仅在训练时pose_to_frame 存在时)执行:

输入:
  fea_kpt (2B, 128, 150)  # 关键点特征
  key_points (2B, 150, 4) # 关键点坐标

流程:
  1. 前B个为query, 后B个为positive
  2. Sinkhorn Unbalanced OT:
     - Cost matrix C = 1 - cosine_sim(feat1, feat2)  # (B, 150, 150)
     - 迭代5次 (sinkhorn_iter=5)
     - epsilon = exp(learnable) + 0.03 (熵正则)
     - gamma = exp(learnable) (质量正则)
  3. 得到 Transport Plan T (B, 150, 150)
  4. project_kpts = T @ key_points2 / sum(T)  # 加权投影
  5. compute_rigid_transform (加权SVD):
     - 计算加权中心
     - SVD分解协方差矩阵
     - 输出 transformation (B, 3, 4)  # R|t 刚体变换

输出:
  transformation_original (B, 3, 4)  # 估计的相对位姿 [R|t]
  project_kpts_original (B, 150, 3)  # 投影后的关键点
  correspondences_feature (B, 150, 150)  # 匹配矩阵

注意: UOTHead 对原始特征和融合特征各执行一次(如果 name='original'name='fusion' 都有),分别输出 transformation_originaltransformation_fusion


4. 模型最终输出汇总

4.1 推理模式 (test/eval, 无 pose_to_frame 时)

字段 维度 说明
vlads (1, 2048) 全局描述子(融合 + BEV加权
vlad_bev (1, 256) BEV全局描述子
score_bev (1, 320, 320) BEV得分图
fea_bev (1, 128, 320, 320) BEV描述子图
key_points (1, 150, 4) BEV关键点坐标 [x,y,z,i]
pixels_kpt (1, 150, 2) BEV关键点像素坐标
fea_kpt_original (1, 128, 150) BEV关键点特征
key_pixels (1, 150, 2) 图像关键点像素坐标
fea_kpl (1, 128, 150) 图像关键点特征
fea_img (1, 128, 192, 576) 图像密集描述子图
score_img (1, 1, 192, 576) 图像得分图
fea_kpt_fusion (1, 128, 150) 融合特征(当前=BEV原始

4.2 训练模式 (有 pose_to_frame 时额外输出)

字段 维度 说明
transformation_original (B, 3, 4) UOT估计的相对位姿原始特征
transformation_fusion (B, 3, 4) UOT估计的相对位姿融合特征
project_kpts_original (B, 150, 3) 投影关键点(原始)
project_kpts_fusion (B, 150, 3) 投影关键点(融合)
correspondences_feature (B, 150, 150) 特征匹配矩阵
fea_pt_dual (2B, 128, max_len) BEV匹配点特征
fea_pl_dual (2B, 128, max_len) 图像匹配点特征
fea_pt_dual_gen (2B, 128, max_len) 生成的点云特征
fea_pl_dual_gen (2B, 128, max_len) 生成的图像特征
fea_kpt_original_gen (2B, 128, 150) 全景生成器输出
fea_kpt_gen_gen (2B, 128, 150) 双路径转换器输出
fea_kpl_gen (2B, 128, 150) BEV→图像特征

5. 损失函数 (loss.py)

总损失 = 各子损失加权求和:

损失 权重 说明
l_score 1.0 BEV score与label_score的MSE
l_pose 1.0 UOT估计位姿投影后的关键点与真值位姿投影的L1误差
l_match 0.05 Sinkhorn匹配投影关键点误差
l_triplet 1.0 全局描述子三元组损失 (margin=0.5)
l_gb 1.0 生成BEV特征与原始BEV特征的cosine相似度损失
l_gi 1.0 生成图像特征与原始图像特征的cosine相似度损失
l_gpa 1.0 全景生成特征与原始关键点特征的cosine相似度损失
l_kpl 1.0 关键点级别生成损失

三元组选择策略 (由 negetative_selsector 配置):

  • random: 随机选择违反margin的负样本
  • semihard: 半难负样本(距离在 margin 内)
  • hardest: 最难负样本

6. 推理与评估流程

6.1 闭环检测 (evaluate_lcd.py)

1. 遍历测试序列所有帧提取VLAD + 局部特征
2. 对每帧排除前后50帧在剩余帧中NN搜索最相似VLADTop-1
3. 用RANSAC验证局部特征匹配EuclideanTransform, min_samples=15
4. 统计AP、Recall@100、F1

6.2 位姿估计评估 (evaluate_pose.py)

1. 对所有闭环对(真值距离<4m
2. RANSAC: 局部特征NN匹配 + RANSAC估计2D刚体变换
3. UOT:   Sinkhorn OT + Weighted SVD估计3D刚体变换
4. 计算平移误差(m)和旋转误差(deg)

7. 维度速查表

关键维度常量

参数 config字段
BEV分辨率 0.2m bev_resolution
BEV范围 [-32, -32, -2.5, 32, 32, 1.5] bev_range
BEV图尺寸 (H,W) 320×320 64/0.2
BEV通道数 7
BEV关键点数 150 kpts_number_bev
图像分辨率 192×576 (384×1152)×0.5
图像关键点数 150 kpts_number_img
特征维度 128 ALNet dim
VLAD聚类数 16 cluster_num_fusion/bev/img
VLAD输出维度 2048 16×128
Sinkhorn迭代 5 sinkhorn_iter
batchsize 6 batchsize
训练序列 0,5,6,7,9 train
验证序列 8,50,54,55,56,59 validate
测试序列 8,50,54,55,56,59 test

数据流维度变化

原始点云:  (N, 4) [x,y,z,intensity]
                ↓ 体素化
BEV特征图: (320, 320, 7)
                ↓ permute
BEV输入:   (1, 7, 320, 320)
                ↓ RICNN
score_bev: (1, 1, 320, 320)
fea_bev:   (1, 128, 320, 320)
key_points:(1, 150, 4)
fea_kpt:   (1, 128, 150)
                ↓ NetVLAD
vlad_bev:  (1, 2048)


原始图像:  (H, W, 3) ~1242×375
                ↓ crop + resize
图像输入:  (192, 576, 3)
                ↓ permute
Img输入:   (1, 3, 192, 576)
                ↓ ALNet
score_img: (1, 1, 192, 576)
fea_img:   (1, 128, 192, 576)
key_pixels:(1, 150, 2)
fea_kpl:   (1, 128, 150)


fusion VLAD = sigmoid(w)*vlad_fusion + (1-sigmoid(w))*vlad_bev
vlads:     (1, 2048)

8. 模型参数量

# 从代码中: sum(p.numel() for p in model.parameters()) / 1e6
# 完整 Fusion 模式 (BEV + Img + FusionHead):
#   ALNet ×2 (BEV/Img共享一个但各自实例化) + RICNN + NetVLAD ×2 + UOT + Converters + Generator + FusionHead
#   约 10-15M 参数

文档基于代码版本: commit c3d268f "位姿可视化"