【Voxel-SLAM】 体素地图与Bundle Adjustment算法深度分析（四）

体素地图与Bundle Adjustment算法深度分析 / Voxel Map & Bundle Adjustment Algorithm Deep Dive

1. 概述 / Overview

Voxel-SLAM的核心在于其自适应八叉树体素地图（Adaptive Octree Voxel Map）和基于体素的Bundle Adjustment优化。与传统的KdTree最近邻匹配不同，本系统将地图本身作为优化因子，通过PointCluster充分统计量实现高效的增量更新和边缘化。

The core of Voxel-SLAM lies in its adaptive octree voxel map and voxel-based Bundle Adjustment. Unlike traditional KdTree nearest-neighbor matching, this system treats the map itself as optimization factors, using PointCluster sufficient statistics for efficient incremental updates and marginalization.

---

2. 自适应八叉树体素地图 / Adaptive Octree Voxel Map

2.1 OctoTree类结构 / OctoTree Class Structure

VERIFY: VoxelSLAM/src/voxel_map.hpp:935

OctoTree
├── SlideWindow* sw              // 滑动窗口数据 (per-voxel sliding window)
├── PointCluster pcr_add         // 所有点的充分统计量 (all points statistics)
├── PointCluster pcr_fix         // 已边缘化点的统计量 (marginalized points statistics)
├── PVec point_fix               // 已边缘化的原始点 (marginalized raw points)
├── Eigen::Matrix<double,9,9> cov_add  // 点测量协方差累积
├── OctoTree* leaves[8]          // 八叉树子节点
├── Plane plane                  // 拟合平面参数
├── eig_value, eig_vector        // 协方差矩阵特征值/向量
├── layer, octo_state            // 层级, 状态(0=叶, 1=已分裂)
├── voxel_center[3]              // 体素中心坐标
├── quater_length                // 四分之一边长
├── jour                         // 时间戳(用于过期回收)
└── mutex mVox                   // 线程安全锁

2.2 全局参数 / Global Parameters

VERIFY: VoxelSLAM/src/voxel_map.hpp:83-89

参数 / Parameter	默认值 / Default	说明 / Description
max_layer	2	最大八叉树层数 / Maximum octree depth
max_points	100	边缘化阈值 / Marginalization threshold
voxel_size	1.0	体素边长(米) / Voxel size in meters
min_eigen_value	-	平面判断绝对阈值 / Absolute plane threshold
plane_eigen_value_thre	-	各层平面判断相对阈值 / Per-layer relative threshold
min_point	-	各层最少点数 / Min points per layer
min_ba_point	20	BA最少点数 / Min points for BA

2.3 平面判定 / Plane Judgment

VERIFY: VoxelSLAM/src/voxel_map.hpp:1015-1018

对每个体素节点，使用PointCluster::cov()计算协方差矩阵的特征值分解：

For each voxel node, eigenvalue decomposition of the covariance matrix from PointCluster::cov():

λ₀ ≤ λ₁ ≤ λ₂  (三个特征值 / three eigenvalues)

平面判定条件 / Plane judgment criteria:

is_plane = (λ₀ < min_eigen_value) AND (λ₀/λ₂ < plane_eigen_value_thre[layer])

双重条件确保：(1) 最小特征值足够小（点分布足够扁平），(2) 最小与最大特征值之比足够小（各向异性足够强）。

The dual condition ensures: (1) smallest eigenvalue is small enough (flat distribution), and (2) ratio of smallest to largest is small enough (strong anisotropy).

2.4 点插入流程 / Point Insertion Flow

allocate() --- 递归插入

VERIFY: VoxelSLAM/src/voxel_map.hpp:1021-1046

allocate(ord, pv, pw, sws):
  if octo_state == 0:        // 叶节点
    push(ord, pv, pw, sws)   // 直接插入
  else:                       // 已分裂节点
    compute child index:      // 根据pw与voxel_center比较
      leafnum = 4*xyz[0] + 2*xyz[1] + xyz[2]
    create child if nullptr
    leaves[leafnum]->allocate(ord, pv, pw, sws)  // 递归

push() --- 实际插入

VERIFY: VoxelSLAM/src/voxel_map.hpp:969-993

push(ord, pv, pw, sws):
  lock(mVox)
  if sw == nullptr:
    sw = sws.pop_back() or new SlideWindow(wdsize)  // 对象池复用
  mord = mp[ord]                    // 窗口内排列映射
  sw->points[mord].push_back(pv)    // 存局部坐标点(仅叶节点以下)
  sw->pcrs_local[mord].push(pv.pnt) // 更新局部PointCluster
  pcr_add.push(pw)                  // 更新全局PointCluster(世界坐标)
  cov_add += Bf_var(pv, pw)         // 累积测量噪声协方差
  unlock(mVox)

关键设计 / Key design: SlideWindow从对象池（sws向量）复用，避免频繁new/delete。

cut_voxel() --- 体素级分配

VERIFY: VoxelSLAM/src/voxel_map.hpp:1504-1533

对每个点 pw:
  1. 计算体素坐标: loc[j] = pw[j] / voxel_size
  2. 查找哈希表: feat_map.find(position)
  3. 已存在 → iter->second->allocate()
  4. 不存在 → new OctoTree(0, wdsize), 设置中心和四分之一边长

2.5 recut() --- 自适应分裂

VERIFY: VoxelSLAM/src/voxel_map.hpp:1148-1194

在每帧BA之前调用，决定每个叶节点是否需要分裂成更细的子节点：

Called before each frame's BA to decide whether leaf nodes need subdivision:

recut(win_count, x_buf, sws):
  if octo_state == 0 (叶节点):
    if pcr_add.N <= min_point[layer]: not a plane, return
    计算特征值分解: saes(pcr_add.cov())
    if plane_judge(eig_value):
      return  // 是平面，保持叶节点
    elif layer >= max_layer:
      return  // 达到最大深度，停止分裂
    else:
      // 需要分裂
      fix_divide(sws)           // 分配已边缘化的点到子节点
      for i in 0..win_count:
        subdivide(i, x_buf[i])  // 分配窗口内各帧的点到子节点
      回收sw到对象池
      octo_state = 1            // 标记为已分裂
  
  // 递归处理子节点
  for i in 0..7:
    if leaves[i] != nullptr:
      leaves[i]->recut(...)

2.6 tras_opt() --- 提取BA因子

VERIFY: VoxelSLAM/src/voxel_map.hpp:1308-1333

遍历所有叶节点，将满足条件的平面体素提取为LidarFactor:

Traverses all leaf nodes, extracting qualifying plane voxels as LidarFactors:

tras_opt(vox_opt):
  if octo_state == 0 (叶节点):
    条件: isexist AND plane.is_plane AND sw != nullptr
    额外条件: eig_value[0]/eig_value[1] > 0.12 → skip (退化平面)
    
    提取PointCluster: pcrs[i] = sw->pcrs_local[mp[i]]
    记录: opt_state = vox_opt.plvec_voxels.size()
    调用: vox_opt.push_voxel(pcrs, pcr_fix, coe, eig_value, eig_vector, pcr_add)
  else:
    递归子节点

---

3. PointCluster充分统计量 / PointCluster Sufficient Statistics

3.1 数学基础 / Mathematical Foundation

VERIFY: VoxelSLAM/src/tools.hpp:304-365

PointCluster存储三个量：

P = Σᵢ pᵢ pᵢᵀ    (3×3矩阵, 二阶矩)
v = Σᵢ pᵢ         (3×1向量, 一阶矩)
N = count          (整数, 零阶矩)

协方差矩阵可直接计算：

Cov = P/N - (v/N)(v/N)ᵀ = P/N - μμᵀ

3.2 刚体变换 / Rigid Body Transform

VERIFY: VoxelSLAM/src/tools.hpp:357-363

当点集经过刚体变换 q = Rp + t 后，统计量变换为：

N' = N
v' = R·v + N·t
P' = R·P·Rᵀ + (R·v)·tᵀ + t·(R·v)ᵀ + N·t·tᵀ

代码实现：

void transform(const PointCluster &sigv, const IMUST &stat) {
    N = sigv.N;
    v = stat.R*sigv.v + N*stat.p;
    Matrix3d rp = stat.R * sigv.v * stat.p.transpose();
    P = stat.R*sigv.P*stat.R.transpose() + rp + rp.transpose() + N*stat.p*stat.p.transpose();
}

3.3 增量更新与边缘化 / Incremental Update & Marginalization

PointCluster支持 += 和 -= 操作，使得：

• 添加新帧 : pcr_total += pcr_new_frame → O(1)
• 边缘化旧帧 : pcr_total -= pcr_old_frame → O(1)
• 无需存储原始点集即可维护精确的协方差

这是整个系统效率的关键：传统方法需要重新计算整个点集的协方差，而PointCluster只需常数时间。

This is key to system efficiency: traditional methods require recomputing covariance from the full point set, while PointCluster needs only constant time.

---

4. 点面残差与Jacobian推导 / Point-to-Plane Residual & Jacobian Derivation

4.1 残差定义 / Residual Definition

BA的残差定义为每个体素内所有点形成的协方差矩阵的最小特征值 λ₀：

The BA residual is defined as the smallest eigenvalue λ₀ of the covariance matrix formed by all points in each voxel:

r = Σ_voxel  cₐ · λ₀(Cov_voxel)

其中 cₐ 为权重系数。

直觉：如果点完美落在一个平面上，最小特征值为0。偏离越大，特征值越大。

Intuition: If points lie perfectly on a plane, the smallest eigenvalue is 0. Greater deviation leads to larger eigenvalue.

4.2 acc_evaluate2() 解析Jacobian

VERIFY: VoxelSLAM/src/voxel_map.hpp:132-241

核心公式：对于第i帧点集（局部坐标），经变换后的世界坐标点为：

pʷ = Rᵢ · pˡ + tᵢ

协方差矩阵：

C = P_total/N - v̄·v̄ᵀ,  v̄ = v_total/N

残差 r = uₖᵀ · C · uₖ = λₖ (最小特征值方向)

对第i帧的旋转和平移的Jacobian：

Aᵢ[0:3, 0:3] = ((Rᵢ·Pᵢ + tᵢ_v·vᵢᵀ) · [Rᵢᵀuₖ]× - Rᵢ·combo1) / N
Aᵢ[0:3, 3:6] = (combo2 · uₖᵀ + (combo2·uₖ)·I₃) / N

其中：

combo1 = [Pᵢ·RᵢᵀUₖ]× + [vᵢ]× · (uₖᵀtᵢ_v)
combo2 = Rᵢ·vᵢ + nᵢ·tᵢ_v
tᵢ_v = tᵢ - v̄

Jacobian向量：

Jᵢ = Aᵢᵀ · uₖ     [6×1]

VERIFY: VoxelSLAM/src/voxel_map.hpp:202-203

4.3 Hessian矩阵 / Hessian Matrix

对角块 (6i, 6i):

VERIFY: VoxelSLAM/src/voxel_map.hpp:206-212

H[i,i] = Aᵢᵀ · M · Aᵢ + correction_terms

其中 M = Σ_{m≠k} 2/(λₖ-λₘ) · uₘ·uₘᵀ 是二阶校正项。

校正项包括：

Block(0,0) += 2/N·(combo1-[Rᵢᵀuₖ]×·Pᵢ)·[Rᵢᵀuₖ]× 
            - 2/N²·(vᵢ×·RᵢᵀUₖ)·(vᵢ×·RᵢᵀUₖ)ᵀ 
            - 0.5·[J_rot]×

Block(0,3) += 2/N·(1-nᵢ/N)·(vᵢ×·RᵢᵀUₖ)·uₖᵀ
Block(3,3) += 2/N·(nᵢ-nᵢ²/N)·uₖ·uₖᵀ

非对角块 (6i, 6j):

VERIFY: VoxelSLAM/src/voxel_map.hpp:215-231

H[i,j] = Aᵢᵀ · M · Aⱼ + cross_correction_terms

最终对称化:

VERIFY: VoxelSLAM/src/voxel_map.hpp:237-239

H[j,i] = H[i,j]ᵀ    (for j > i, copy transpose)

---

5. LM阻尼迭代优化 / Levenberg-Marquardt Damping Iteration

5.1 Lidar_BA_Optimizer (6N自由度)

VERIFY: VoxelSLAM/src/voxel_map.hpp:293-444

求解步骤:

1. 多线程计算 Hessian H 和 Jacobian J:
   divide_thread() → 将voxel因子均分给thd_num个线程
   每个线程调用 acc_evaluate2()

2. 固定第一帧 (gauge fixing):
   H.topRows(6).setZero()
   H.leftCols(6).setZero()
   H.block<6,6>(0,0).setIdentity()
   J.head(6).setZero()

3. LM求解:
   D = diag(H)
   Δx = (H + μ·D)⁻¹ · (-J)    // LDLT分解

4. 状态更新:
   Rᵢ_new = Rᵢ · Exp(Δx[6i:6i+3])
   tᵢ_new = tᵢ + Δx[6i+3:6i+6]

5. 判断接受/拒绝:
   q = r₁ - r₂   (实际下降)
   q₁ = 0.5·Δxᵀ·(μ·D·Δx - J)  (预测下降)
   
   if q > 0:  接受, 调整μ
     v = 2
     q = q/q₁
     μ *= max(1/3, 1-(2q-1)³)
   else:  拒绝
     μ *= v; v *= 2

6. 收敛判断: |r₁-r₂|/r₁ < 1e-6

VERIFY: VoxelSLAM/src/voxel_map.hpp:367-442

5.2 LI_BA_Optimizer (15N自由度)

VERIFY: VoxelSLAM/src/voxel_map.hpp:450

在Lidar BA的基础上，增加IMU预积分约束：

On top of LiDAR BA, adds IMU preintegration constraints:

Hessian结构: 15N × 15N
  - LiDAR因子: 6×6块 → 嵌入15×15块
  - IMU因子: 连续帧间的15×15约束

状态向量: [δθ₁, δp₁, δv₁, δbg₁, δba₁, ..., δθₙ, δpₙ, δvₙ, δbgₙ, δbaₙ]

IMU因子通过 IMU_PRE::give_evaluate() 提供：

VERIFY: VoxelSLAM/src/preintegration.hpp:137

残差 = [Δθ, Δp, Δv] 预积分预测 vs 实际状态差

Jacobian结构:
  对帧i: [J_rot_i, J_p_i, J_v_i, J_bg_i, J_ba_i]
  对帧j: [J_rot_j, J_p_j, J_v_j, J_bg_j, J_ba_j]

5.3 LI_BA_OptimizerGravity (15N+3自由度)

VERIFY: VoxelSLAM/src/voxel_map.hpp:658

进一步增加重力方向优化。额外的3个自由度对应重力向量 g = [gx, gy, gz]。

Adds gravity direction optimization. The extra 3 DOF correspond to gravity vector g.

使用 IMU_PRE::give_evaluate_g() 计算包含重力的Jacobian。

VERIFY: VoxelSLAM/src/preintegration.hpp:214

---

6. 边缘化策略 / Marginalization Strategy

6.1 margi() 详细流程

VERIFY: VoxelSLAM/src/voxel_map.hpp:1196-1305

边缘化是滑动窗口SLAM的核心操作。当最旧的帧需要移出窗口时：

Marginalization is the core operation of sliding window SLAM. When the oldest frame needs to leave the window:

margi(win_count, mgsize, x_buf, vox_opt):
  if 叶节点 (octo_state == 0):
    lock(mVox)
    
    // 步骤1: 使用BA优化后的统计量（如果有）
    if opt_state >= 0:
      pcr_add = vox_opt.pcr_adds[opt_state]  // 用优化后的值
      eig_value = vox_opt.eig_values[opt_state]
      eig_vector = vox_opt.eig_vectors[opt_state]
    else:
      // 从头重新计算
      pcr_add = pcr_fix
      for i in 0..win_count:
        pcr_add += transform(sw->pcrs_local[mp[i]], x_buf[i])
    
    // 步骤2: 更新平面参数
    if pcr_fix.N < max_points AND plane.is_plane:
      if pcr_add.N - last_num >= 5:
        plane_update()  // 更新平面法向量和协方差
    
    // 步骤3: 转移点到fixed集
    if pcr_fix.N < max_points:
      for i in 0..mgsize:  // 对每个要边缘化的帧
        pcr_fix += pcrs_world[i]          // 累积统计量
        point_fix.append(transformed_points)  // 保留原始点
    else:
      // fixed已满，不再保留原始点
      for i in 0..mgsize:
        pcr_add -= pcrs_world[i]  // 反向移除
    
    // 步骤4: 清除窗口内该帧的数据
    for i in 0..mgsize:
      sw->pcrs_local[mp[i]].clear()
      sw->points[mp[i]].clear()
    
    // 步骤5: 判断是否还有活跃数据
    isexist = (pcr_fix.N < pcr_add.N)
    
    unlock(mVox)

6.2 关键洞察 / Key Insights

为什么pcr_fix有上限(max_points)?

当pcr_fix.N ≥ max_points时，系统不再保留原始点（point_fix被释放），只保留PointCluster统计量。这节省了大量内存，因为一个PointCluster只占3×3+3+1 = 13个double，而原始点集可能有几百个点。

When pcr_fix.N ≥ max_points, the system no longer keeps raw points (point_fix is released), only PointCluster statistics. This saves substantial memory since one PointCluster is only 13 doubles vs. hundreds of raw points.

mp[]排列数组的作用

VERIFY: VoxelSLAM/src/voxel_map.hpp:934

mp是一个全局排列数组，用于将逻辑窗口索引映射到物理存储索引。当边缘化最旧帧后，不需要移动所有数据，只需更新mp的映射关系。

mp is a global permutation array mapping logical window indices to physical storage indices. After marginalizing the oldest frame, data doesn't need to be moved --- just update the mp mapping.

---

7. 点匹配 / Point Matching

7.1 match() 函数

VERIFY: VoxelSLAM/src/voxel_map.hpp:1335-1392

用于EKF更新时的点到平面匹配：

Used for point-to-plane matching during EKF update:

match(wld, pla, max_prob, var_wld, sigma_d, oc):
  if 叶节点 AND plane.is_plane:
    dis_to_plane = |n·(wld - center)|       // 点面距离
    dis_to_center = |center - wld|²          // 点到中心距离
    range_dis = dis_to_center - dis_to_plane²  // 面内距离
    
    if range_dis ≤ 3²·plane.radius:          // 在平面范围内
      // 计算概率化距离
      J_nq = [wld-center, -normal]           // 1×6 Jacobian
      σ_l = J_nq · plane_var · J_nqᵀ        // 平面不确定性
      σ_l += nᵀ · var_wld · n               // 加上点的不确定性
      
      if dis_to_plane < 3·√σ_l:             // 3σ检验
        return matched plane
  else:
    // 递归到对应子节点
    leafnum = child_index(wld, voxel_center)
    leaves[leafnum]->match(...)

关键特点 / Key features:

1. 概率化匹配 : 考虑了平面不确定性 (plane_var) 和点测量不确定性 (var_wld)
2. 3σ准则: 使用统计意义上的3倍标准差作为阈值
3. 单路搜索: 只搜索点所在的子节点，O(log N)复杂度

---

8. 多线程体素操作 / Multi-Threaded Voxel Operations

8.1 multi_recut()

在voxelslam.cpp中（非voxel_map.hpp），将feat_map中的voxel分配到多个线程并行执行recut：

将feat_map的entry均分为thd_num份
每个线程独立处理其分配到的OctoTree::recut()
无需同步（每个OctoTree独立）

8.2 divide_thread() (BA)

VERIFY: VoxelSLAM/src/voxel_map.hpp:298-335

BA的Hessian/Jacobian计算是最耗时的操作。通过将voxel因子（plvec_voxels）均分给多个线程：

每个线程: acc_evaluate2(x_stats, part*i, part*(i+1), hessians[i], jacobins[i], resis[i])
主线程: 累加所有线程的结果
  Hess += hessians[i]; JacT += jacobins[i]; residual += resis[i]

---

9. 平面协方差传播 / Plane Covariance Propagation

9.1 plane_update()

VERIFY: VoxelSLAM/src/voxel_map.hpp:1118-1146

平面参数（法向量n和中心c）的协方差通过特征值扰动理论传播：

Plane parameter (normal n and center c) covariance is propagated via eigenvalue perturbation theory:

对于法向量 n = u₀ (最小特征值对应的特征向量):
  ∂u₀/∂C = Σ_{k≠0} 1/(λ₀-λₖ) · uₖ · (∂C/∂param · u₀)

其中 ∂C/∂param 通过 Bf_var() 计算的 cov_add 传播。

VERIFY: VoxelSLAM/src/voxel_map.hpp:91-106

Bf_var() 计算点测量噪声对协方差矩阵的贡献：

Bf_var(pv, bcov, vec):
  Bi = ∂(vec·vecᵀ)/∂vec  // 3→6的Jacobian (上三角)
  bcov[0:6, 0:6] = Bi · pv.var · Biᵀ
  bcov[0:6, 6:9] = Bi · pv.var
  bcov[6:9, 0:6] = (Bi · pv.var)ᵀ
  bcov[6:9, 6:9] = pv.var

---

10. 验证清单 / Verification Checklist

• OctoTree类定义位置: voxel_map.hpp:935
• plane_judge()双重条件: voxel_map.hpp:1015-1018
• push()中SlideWindow复用: voxel_map.hpp:969-993
• allocate()递归插入: voxel_map.hpp:1021-1046
• recut()分裂逻辑: voxel_map.hpp:1148-1194
• tras_opt()因子提取: voxel_map.hpp:1308-1333
• acc_evaluate2()解析Jacobian: voxel_map.hpp:132-241
• damping_iter() LM优化: voxel_map.hpp:367-442
• margi()边缘化: voxel_map.hpp:1196-1305
• match()概率化匹配: voxel_map.hpp:1335-1392
• PointCluster::transform(): tools.hpp:357-363
• Bf_var()噪声传播: voxel_map.hpp:91-106
• plane_update()协方差: voxel_map.hpp:1118-1146
• Lidar_BA_Optimizer: voxel_map.hpp:293
• LI_BA_Optimizer: voxel_map.hpp:450
• LI_BA_OptimizerGravity: voxel_map.hpp:658