【Voxel-SLAM】Data Structures / 数据结构文档（二）

love在水一方2026-04-29 12:48

Voxel-SLAM Data Structures / 数据结构文档

Table of Contents / 目录

# Structure File Section
1 IMUST tools.hpp [1](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
2 PointCluster tools.hpp [2](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
3 pointVar voxel_map.hpp [3](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
4 Plane voxel_map.hpp [4](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
5 LidarFactor voxel_map.hpp [5](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
6 Lidar_BA_Optimizer voxel_map.hpp [6](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
7 LI_BA_Optimizer voxel_map.hpp [7](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
8 LI_BA_OptimizerGravity voxel_map.hpp [8](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
9 Keyframe voxel_map.hpp [9](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
10 SlideWindow voxel_map.hpp [10](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
11 OctoTree voxel_map.hpp [11](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
12 IMU_PRE preintegration.hpp [12](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
13 ScanPose loop_refine.hpp [13](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
14 PGO_Edge / PGO_Edges loop_refine.hpp [14](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
15 OctreeGBA loop_refine.hpp [15](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)
16 BTC types BTC.h [16](# Structure File Section 1 IMUST tools.hpp 1 2 PointCluster tools.hpp 2 3 pointVar voxel_map.hpp 3 4 Plane voxel_map.hpp 4 5 LidarFactor voxel_map.hpp 5 6 Lidar_BA_Optimizer voxel_map.hpp 6 7 LI_BA_Optimizer voxel_map.hpp 7 8 LI_BA_OptimizerGravity voxel_map.hpp 8 9 Keyframe voxel_map.hpp 9 10 SlideWindow voxel_map.hpp 10 11 OctoTree voxel_map.hpp 11 12 IMU_PRE preintegration.hpp 12 13 ScanPose loop_refine.hpp 13 14 PGO_Edge / PGO_Edges loop_refine.hpp 14 15 OctreeGBA loop_refine.hpp 15 16 BTC types BTC.h 16)

1. IMUST

15-dimensional IMU state / 15维IMU状态

VERIFY: VoxelSLAM/src/tools.hpp:135

复制代码 
struct IMUST

The fundamental state representation for a single pose in the sliding window. Each

element of the vector<IMUST> x_stats sliding window is one IMUST.

IMUST 是滑动窗口中单帧位姿的基本状态表示。滑动窗口 vector<IMUST> x_stats 的每个

元素就是一个 IMUST。

Fields / 字段

Field Type Meaning / 含义
t double Timestamp / 时间戳
R Matrix3d SO(3) rotation body-to-world / 旋转矩阵 (体坐标到世界)
p Vector3d Position in world frame / 世界坐标系下的位置
v Vector3d Velocity in world frame / 世界坐标系下的速度
bg Vector3d Gyroscope bias / 陀螺仪偏置
ba Vector3d Accelerometer bias / 加速度计偏置
g Vector3d Gravity vector (default 0,0,-9.8) / 重力向量
cov Matrix<double,15,15> State covariance (DIM=15) / 状态协方差矩阵

VERIFY: VoxelSLAM/src/tools.hpp:138-145

State vector layout / 状态向量布局 (DIM=15)

复制代码 
index:  [0..2]   [3..5]   [6..8]   [9..11]   [12..14]
field:  delta_R   delta_p  delta_v   delta_bg   delta_ba

Operators / 运算符

Operator Signature Description / 描述
+= IMUST& operator+=(Matrix<double,15,1>&) Apply increment: R *= Exp(dR), p += dp, v += dv, bg += dbg, ba += dba. 增量更新: 旋转用指数映射左乘，其余直接加。
- Matrix<double,15,1> operator-(const IMUST&) Compute error: Log(b.R^T * R), p-b.p, v-b.v, bg-b.bg, ba-b.ba. 计算两个状态之间的误差向量。
= IMUST& operator=(const IMUST&) Deep copy all fields including cov. 深拷贝所有字段。

VERIFY: VoxelSLAM/src/tools.hpp:154-186

Methods / 方法

  • setZero() -- Reset to identity rotation, zero vectors, covariance to 0.0001I (with 0.00001 I for bias blocks).
    重置为单位旋转、零向量，协方差初始化为 0.0001I（偏置块为 0.00001 I）。

    VERIFY: VoxelSLAM/src/tools.hpp:188-197

2. PointCluster

Sufficient statistics for a point set / 点集的充分统计量

VERIFY: VoxelSLAM/src/tools.hpp:304

复制代码 
class PointCluster

Stores the scatter matrix P = sum(p_i * p_i^T), the sum vector v = sum(p_i), and the

count N. This is the key enabler of O(1) marginalization -- adding/removing a scan from

a voxel only requires one += or -= on these three fields, not re-iterating over all

points.

存储散布矩阵 P = sum(p_i * p_i^T)、求和向量 v = sum(p_i) 及点计数 N。这是实现 O(1)

边缘化的关键------对体素增减一帧扫描只需对这三个字段做 += 或 -=，无需重新遍历所有点。

Fields / 字段

Field Type Meaning / 含义
P Matrix3d Scatter matrix sum(p*p^T) / 散布矩阵
v Vector3d Sum of points sum§ / 点坐标求和
N int Point count / 点数

VERIFY: VoxelSLAM/src/tools.hpp:308-310

Methods / 方法

Method Description / 描述
push(vec) Accumulate one point: N++, P += vec*vec^T, v += vec. 累加一个点。
cov() Return covariance: P/N - mean*mean^T. 返回协方差矩阵。
operator+= Merge another cluster: P+=, v+=, N+=. 合并另一个统计量。
operator-= Remove a cluster: P-=, v-=, N-=. 移除一个统计量 (marginalization).
transform(sigv, stat) Transform cluster by IMUST pose: v' = Rv + N p, P' = RPR^T + ... 将局部坐标系统计量变换到世界坐标系。
clear() Reset P, v, N to zero. 清零。

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

Key insight: O(1) marginalization / 关键洞察: O(1) 边缘化

复制代码 
Marginalization (remove oldest scan from voxel):
边缘化 (从体素中移除最旧帧):

  pcr_fix  -= pcrs_world[oldest]    // O(1): subtract 3x3 + 3x1 + int
  pcr_add  -= pcrs_world[oldest]    // O(1)

Instead of re-computing from N points: O(N)
而不是从 N 个点重新计算: O(N)

3. pointVar

Point with measurement covariance / 带测量协方差的点

VERIFY: VoxelSLAM/src/voxel_map.hpp:13

复制代码 
struct pointVar

Fields / 字段

Field Type Meaning / 含义
pnt Vector3d 3D point coordinate (local or world) / 三维点坐标
var Matrix3d 3x3 measurement covariance / 测量协方差矩阵

VERIFY: VoxelSLAM/src/voxel_map.hpp:16-17

Type aliases: PVec = vector<pointVar>, PVecPtr = shared_ptr<vector<pointVar>>.

VERIFY: VoxelSLAM/src/voxel_map.hpp:20-21

The covariance var is computed from the LiDAR range model and propagated into the

plane fitting uncertainty via Bf_var().

协方差 var 来自 LiDAR 距离模型，通过 Bf_var() 传播到平面拟合的不确定度中。

4. Plane

Fitted plane parameters / 拟合平面参数

VERIFY: VoxelSLAM/src/voxel_map.hpp:66

复制代码 
struct Plane

Fields / 字段

Field Type Meaning / 含义
center Vector3d Plane centroid / 平面中心点
normal Vector3d Unit normal (eigenvector of smallest eigenvalue) / 单位法向量
plane_var Matrix<double,6,6> Joint covariance of [normal; center] / [法向量; 中心点]的联合协方差
radius float Largest eigenvalue of covariance (spread) / 最大特征值 (平面展开尺度)
is_plane bool Whether the voxel has converged to a plane / 该体素是否收敛为平面

VERIFY: VoxelSLAM/src/voxel_map.hpp:69-73

Covariance structure / 协方差结构

复制代码 
plane_var (6x6):
  [ Cov(normal, normal)   Cov(normal, center) ]    (3x3  3x3)
  [ Cov(center, normal)   Cov(center, center) ]    (3x3  3x3)

This is computed in OctoTree::plane_update() using the Jacobian of eigendecomposition

with respect to the sufficient statistics.

该矩阵在 OctoTree::plane_update() 中通过充分统计量的特征分解雅可比计算。

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

5. LidarFactor

LiDAR BA factor: per-voxel plane constraints / LiDAR BA因子: 逐体素平面约束

VERIFY: VoxelSLAM/src/voxel_map.hpp:108-109

复制代码 
class LidarFactor

Aggregates all voxel-level plane constraints for bundle adjustment. Each entry

represents one voxel's planarity cost (the smallest eigenvalue of the combined

scatter matrix).

聚合所有体素级别的平面约束用于BA优化。每条记录代表一个体素的平面性代价

（合并散布矩阵的最小特征值）。

Fields / 字段

Field Type Meaning / 含义
plvec_voxels vector<vector<PointCluster>> Per-voxel, per-frame local PointClusters / 每体素每帧的局部统计量
sig_vecs vector<PointCluster> Fixed (marginalized) PointCluster per voxel / 每体素的已边缘化固定统计量
coeffs vector<double> Weight coefficient per voxel / 每体素的权重系数
eig_values PLV(3) Cached eigenvalues per voxel (3x1 vectors) / 缓存的每体素特征值
eig_vectors PLM(3) Cached eigenvectors per voxel (3x3 matrices) / 缓存的每体素特征向量
pcr_adds vector<PointCluster> Total (fixed + window) PointCluster per voxel / 每体素的总统计量
win_size int Number of frames in sliding window / 滑动窗口帧数

VERIFY: VoxelSLAM/src/voxel_map.hpp:112-118

Methods / 方法

Method Description / 描述
push_voxel(...) Append one voxel's constraint data. 添加一个体素的约束数据。
acc_evaluate2(xs, head, end, Hess, JacT, residual) Accumulate Hessian (6N x 6N), Jacobian, and residual for voxels [head, end). Uses analytic second-order derivatives. 累积海森矩阵、雅可比和残差，使用解析二阶导数。
evaluate_only_residual(xs, head, end, residual) Evaluate residual only (for line-search), also update cached eig_values/eig_vectors. 仅计算残差（用于线搜索），同时更新缓存的特征值/向量。
clear() Reset all vectors. 清空所有向量。

VERIFY: VoxelSLAM/src/voxel_map.hpp:122-288

Residual formulation / 残差公式

复制代码 
For each voxel v:
  sigma_v = pcr_fix + sum_i{ transform(pcr_local[i], x[i]) }
  covariance = sigma_v.P / sigma_v.N - mean * mean^T
  eigenvalues = sorted_eig(covariance)
  residual_v  = coeff_v * lambda_min         (smallest eigenvalue)

Total residual = sum_v { residual_v }

6. Lidar_BA_Optimizer

LiDAR-only BA with Levenberg-Marquardt / 纯LiDAR BA的LM优化器

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

复制代码 
class Lidar_BA_Optimizer

State dimension: 6N (3 rotation + 3 translation per frame, N = win_size).

状态维度: 6N (每帧3旋转+3平移)。

Fields / 字段

Field Type Meaning / 含义
win_size int Window size / 窗口大小
jac_leng int = win_size * 6, total Jacobian length / 雅可比总长度
thd_num int Thread count (default 2) / 线程数

Methods / 方法

Method Description / 描述
divide_thread(...) Multi-threaded Hessian/Jacobian accumulation. 多线程累积海森/雅可比。
only_residual(...) Multi-threaded residual evaluation. 多线程残差计算。
damping_iter(x_stats, voxhess, hess, resis, max_iter) LM iteration: solve (H + uD)dx = -J^T. First frame is fixed (top 6 rows/cols zeroed). Returns convergence flag. LM迭代: 求解 (H+uD)dx=-J^T。第一帧被固定。返回收敛标志。

VERIFY: VoxelSLAM/src/voxel_map.hpp:296-443

LM update strategy / LM更新策略

复制代码 
if residual decreases (q > 0):
    accept step, reduce u: u *= max(1/3, 1-(2q-1)^3)
else:
    reject step, increase u: u *= v, v *= 2
Convergence: |r1-r2|/r1 < 1e-6

7. LI_BA_Optimizer

LiDAR-Inertial BA optimizer / LiDAR-惯性 BA 优化器

VERIFY: VoxelSLAM/src/voxel_map.hpp:449-450

复制代码 
class LI_BA_Optimizer

State dimension: 15N (15 per frame: R, p, v, bg, ba).

状态维度: 15N (每帧15维: R, p, v, bg, ba)。

Combines LidarFactor (plane constraints) with IMU_PRE (preintegration constraints)

in a joint cost function. The IMU term is scaled by imu_coef (default 1e-4).

将 LidarFactor（平面约束）与 IMU_PRE（预积分约束）联合优化。IMU项由 imu_coef

（默认1e-4）缩放。

Key methods / 关键方法

Method Description / 描述
divide_thread(x_stats, voxhess, imus_factor, Hess, JacT) Multi-threaded evaluation: LiDAR Hessian mapped into 15N space via hess_plus(), plus IMU factors between consecutive frames. 多线程计算: LiDAR 海森矩阵映射到 15N 空间，加上相邻帧间的IMU因子。
damping_iter(x_stats, voxhess, imus_factor, hess) LM solver with 3 max iterations. Updates R, p, v, bg, ba and IMU preintegration bias correction (dbg, dba). LM求解器，最多3次迭代。更新R, p, v, bg, ba及预积分偏置修正。

VERIFY: VoxelSLAM/src/voxel_map.hpp:453-655

Hessian structure / 海森矩阵结构

复制代码 
                 frame_0 (15)    frame_1 (15)    ...   frame_N (15)
frame_0 (15)  [ LiDAR_00+IMU_01  IMU_01           ...       0       ]
frame_1 (15)  [   IMU_01^T     LiDAR_11+IMU_01+12 ...       0       ]
   ...        [      ...            ...            ...      ...      ]
frame_N (15)  [       0              0             ... LiDAR_NN+IMU  ]

LiDAR contributes 6x6 blocks (rotation+translation only).
IMU contributes 15x15 blocks between consecutive frames.

8. LI_BA_OptimizerGravity

LiDAR-Inertial BA with gravity estimation / 带重力估计的LiDAR-惯性 BA

VERIFY: VoxelSLAM/src/voxel_map.hpp:657-658

复制代码 
class LI_BA_OptimizerGravity

State dimension: 15N + 3 (the extra 3 are for gravity vector optimization).

状态维度: 15N + 3 (额外3维用于重力向量优化)。

Extends LI_BA_Optimizer by adding a shared gravity variable appended at the end

of the state vector. Uses give_evaluate_g() from IMU_PRE which includes gravity

Jacobians.

在 LI_BA_Optimizer 基础上增加了共享的重力变量，附加在状态向量末尾。使用 IMU_PRE

的 give_evaluate_g() 方法获取包含重力雅可比的因子。

Differences from LI_BA_Optimizer / 与 LI_BA_Optimizer 的区别

  • imu_leng = win_size * DIM + 3 (3 extra for gravity)

    VERIFY: VoxelSLAM/src/voxel_map.hpp:779

  • IMU Hessian uses give_evaluate_g() with 2*DIM+3 sized blocks, gravity terms
    are accumulated into the bottom-right 3x3 corner and cross-blocks.

    VERIFY: VoxelSLAM/src/voxel_map.hpp:698-711

  • After LM step, gravity update x_stats_temp[0].g += dxi.tail(3) is propagated
    to all frames: x_stats_temp[j].g = x_stats_temp[0].g.

    VERIFY: VoxelSLAM/src/voxel_map.hpp:813-822

9. Keyframe

Aggregated scan for loop closure / 聚合扫描帧（用于回环检测）

VERIFY: VoxelSLAM/src/voxel_map.hpp:866-867

复制代码 
struct Keyframe

Typically 10 scans merged into one keyframe (see comment "10 scans merge into a

keyframe"). Used for loop closure detection and global mapping.

通常将10帧扫描合并为一个关键帧（见代码注释"10 scans merge into a keyframe"）。

用于回环检测和全局建图。

Fields / 字段

Field Type Meaning / 含义
x0 IMUST Reference pose of the keyframe / 关键帧参考位姿
plptr PointCloud<PointType>::Ptr Merged point cloud / 合并后的点云
exist int Existence flag / 存在标志
id int Keyframe ID / 关键帧ID
mp int Map index / 地图索引
jour float Journal/odometry distance / 里程计距离

VERIFY: VoxelSLAM/src/voxel_map.hpp:870-874

Methods / 方法

  • generate(pl_send, rot, tra) -- Transform stored points by rot/tra and append
    to pl_send. 将存储的点通过旋转/平移变换后追加到点云。

    VERIFY: VoxelSLAM/src/voxel_map.hpp:881-891

10. SlideWindow

Per-voxel sliding window storage / 体素级滑动窗口存储

VERIFY: VoxelSLAM/src/voxel_map.hpp:895-896

复制代码 
class SlideWindow

Each OctoTree leaf node owns a SlideWindow that stores raw points and local-frame

PointClusters for each frame in the sliding window.

每个八叉树叶节点拥有一个 SlideWindow，存储滑动窗口中每帧的原始点和局部坐标系

PointCluster。

Fields / 字段

Field Type Meaning / 含义
points vector<PVec> points[i] = raw pointVar list for frame i (with covariance) / 第i帧的原始点列表（带协方差）
pcrs_local vector<PointCluster> pcrs_local[i] = local-frame PointCluster for frame i / 第i帧的局部坐标系统计量

VERIFY: VoxelSLAM/src/voxel_map.hpp:900-901

Methods / 方法

Method Description / 描述
SlideWindow(wdsize) Allocate wdsize slots, reserve 20 points each. 分配wdsize个槽位，每个预留20点空间。
resize(wdsize) Resize if size changed. 如尺寸变化则调整大小。
clear() Clear all points and clusters. 清空所有点和统计量。

VERIFY: VoxelSLAM/src/voxel_map.hpp:903-930

11. OctoTree

THE CORE: adaptive octree for odometry and local mapping /
核心结构: 用于里程计和局部建图的自适应八叉树

VERIFY: VoxelSLAM/src/voxel_map.hpp:932-935

复制代码 
class OctoTree

The central data structure of Voxel-SLAM. Each top-level voxel (stored in
unordered_map<VOXEL_LOC, OctoTree*>) is an OctoTree root that adaptively

subdivides when points do not fit a plane.

Voxel-SLAM的核心数据结构。每个顶层体素（存于 unordered_map<VOXEL_LOC, OctoTree*>）

是一个八叉树根节点，当点不能拟合平面时自适应细分。

Architecture / 架构

复制代码 
unordered_map<VOXEL_LOC, OctoTree*>
         |
         v
   [OctoTree root]  layer=0, voxel_size=1.0
    /    |    \
  [leaf] ... [OctoTree]  layer=1, voxel_size=0.5
                /    \
             [leaf] [OctoTree]  layer=2 (max_layer)

Fields / 字段

Field Type Meaning / 含义
sw SlideWindow* Per-frame point storage (nullptr when subdivided) / 每帧点存储（细分后为空）
pcr_add PointCluster Total statistics: fixed + windowed / 总统计量 = 固定 + 窗口内
cov_add Matrix<double,9,9> Accumulated measurement covariance (for plane_var) / 累积测量协方差（用于plane_var）
pcr_fix PointCluster Marginalized (fixed) statistics / 已边缘化的固定统计量
point_fix PVec Fixed points with covariance (for subdivision) / 固定点及协方差（用于细分）
layer int Depth in octree (0 = root) / 八叉树深度
octo_state int 0 = leaf, 1 = subdivided / 0=叶节点, 1=已细分
wdsize int Window size / 窗口大小
leaves[8] OctoTree*[8] 8 children (nullptr if absent) / 8个子节点
voxel_center[3] double[3] Octant center xyz / 八分体中心坐标
quater_length float Half-size of child voxel / 子体素半尺寸
jour double Odometry journal distance / 里程计距离
plane Plane Fitted plane (valid when is_plane=true) / 拟合平面
isexist bool Has active (non-marginalized) data / 是否有活跃数据
eig_value Vector3d Eigenvalues of pcr_add covariance / 总统计量协方差的特征值
eig_vector Matrix3d Eigenvectors of pcr_add covariance / 总统计量协方差的特征向量
last_num int Point count at last plane_update / 上次平面更新时的点数
opt_state int Index into LidarFactor arrays (-1 = not in opt) / LidarFactor中的索引
mVox mutex Thread-safety lock / 线程安全锁

VERIFY: VoxelSLAM/src/voxel_map.hpp:939-959

Methods / 方法

Method Description / 描述
push(ord, pv, pw, sws) Thread-safe insertion: add point to SlideWindow, accumulate pcr_add and cov_add. 线程安全插入: 添加点到滑动窗口，累积统计量和协方差。
push_fix(pv) Add a marginalized point (both pcr_fix and pcr_add). 添加边缘化点。
allocate(ord, pv, pw, sws) Route point to correct child if subdivided, else push. 根据细分状态路由点到正确子节点或直接push。
recut(win_count, x_buf, sws) Subdivision decision: compute eigenvalues, if planar keep as leaf; otherwise subdivide into 8 children, redistribute points. 细分决策: 计算特征值，若为平面则保持叶节点；否则细分为8个子节点并重新分配点。
plane_update() Recompute plane normal/center/plane_var from pcr_add using eigendecomposition Jacobians. 重新计算平面参数及其不确定度。
tras_opt(vox_opt) Extract optimization factor: for planar leaves with good conditioning (lambda0/lambda1 < 0.12), add to LidarFactor. 提取优化因子: 条件数好的平面叶节点添加到LidarFactor。
margi(win_count, mgsize, x_buf, vox_opt) Marginalization: merge oldest frames into pcr_fix, update pcr_add. If pcr_fix saturated (>max_points), stop accumulating raw points. 边缘化: 将最旧帧合并到固定统计量，更新总统计量。
match(wld, pla, max_prob, var_wld, sigma_d, oc) EKF matching: find plane at world point, compute point-to-plane distance with probabilistic gating (3-sigma). EKF匹配: 在世界坐标点处查找平面，用概率门控（3-sigma）计算点到面距离。
tras_ptr(octos_release) Traverse and collect all descendant pointers for memory management. 遍历收集所有后代指针用于内存管理。

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

Lifecycle diagram / 生命周期图

复制代码 
  New points arrive
       |
       v
  allocate() ---> push() to SlideWindow
       |
  [After all points inserted]
       |
       v
  recut() ---> Plane test via eigenvalue
       |           |
       |     [is_plane=true]  --> Keep as leaf, plane_update()
       |           |
       |     [is_plane=false] --> Subdivide into 8 children
       |
  [Optimization]
       |
       v
  tras_opt() --> Collect LidarFactor entries from planar leaves
       |
       v
  BA solve (Lidar_BA / LI_BA / LI_BA_Gravity)
       |
       v
  margi() --> Fold oldest frame(s) into pcr_fix, clear from SlideWindow
       |
  [EKF update]
       |
       v
  match() --> Point-to-plane matching with probabilistic gating

12. IMU_PRE

IMU preintegration / IMU预积分

VERIFY: VoxelSLAM/src/preintegration.hpp:10-11

复制代码 
class IMU_PRE

Implements IMU preintegration between consecutive keyframes. Stores the preintegrated

measurements and first-order bias-correction Jacobians so that the preintegrated term

can be corrected for bias changes without re-integration.

实现相邻关键帧间的IMU预积分。存储预积分测量值和一阶偏置修正雅可比，使得偏置变化时

无需重新积分即可修正预积分项。

Fields / 字段

Field Type Meaning / 含义
R_delta Matrix3d Preintegrated rotation / 预积分旋转
p_delta Vector3d Preintegrated position / 预积分位置
v_delta Vector3d Preintegrated velocity / 预积分速度
bg Vector3d Gyroscope bias at linearization point / 线性化点的陀螺偏置
ba Vector3d Accelerometer bias at linearization point / 线性化点的加速度偏置
R_bg Matrix3d Jacobian dR/dbg / 旋转对陀螺偏置的雅可比
p_bg Matrix3d Jacobian dp/dbg / 位置对陀螺偏置的雅可比
p_ba Matrix3d Jacobian dp/dba / 位置对加速度偏置的雅可比
v_bg Matrix3d Jacobian dv/dbg / 速度对陀螺偏置的雅可比
v_ba Matrix3d Jacobian dv/dba / 速度对加速度偏置的雅可比
dtime double Total integration time / 总积分时间
dbg, dba Vector3d Bias correction deltas / 偏置修正增量
dbg_buf, dba_buf Vector3d Backup for LM rollback / LM回退备份
cov Matrix<double,15,15> Preintegration covariance / 预积分协方差
_imus deque<ImuPtr> Raw IMU message buffer / 原始IMU消息缓冲

VERIFY: VoxelSLAM/src/preintegration.hpp:14-29

Methods / 方法

Method Description / 描述
push_imu(imus) Process a deque of IMU messages with midpoint integration. 中值积分处理IMU消息队列。
add_imu(gyr, acc, dt) Single-step preintegration update (R, p, v, Jacobians, covariance). 单步预积分更新。
give_evaluate(st1, st2, jtj, gg, jac_enable) Compute IMU residual and (optionally) Jacobians between two IMUST states. Returns weighted squared residual r^T * Cov^{-1} * r. 计算两个IMUST状态间的IMU残差及雅可比。返回加权残差。
give_evaluate_g(st1, st2, jtj, gg, jac_enable) Same as above but with gravity Jacobian (for LI_BA_OptimizerGravity). 同上但包含重力雅可比。
update_state(dxi) Apply bias correction: dbg += dxi[9:12], dba += dxi[12:15], with rollback buffer. 应用偏置修正并保存回退备份。
merge(imu2) Merge two consecutive preintegrations into one. 合并两个连续预积分。

VERIFY: VoxelSLAM/src/preintegration.hpp:49-322

Preintegration residual / 预积分残差

复制代码 
R_correct = R_delta * Exp(R_bg * dbg)
p_correct = p_delta + p_bg*dbg + p_ba*dba
v_correct = v_delta + v_bg*dbg + v_ba*dba

res_R = Log( R_correct^T * R1^T * R2 )
res_p = R1^T * (p2 - p1 - v1*dt - 0.5*g*dt^2) - p_correct
res_v = R1^T * (v2 - v1 - g*dt) - v_correct
res_bg = bg2 - bg1
res_ba = ba2 - ba1

13. ScanPose

Pose + point data for PGO / 位姿+点云数据（用于位姿图优化）

VERIFY: VoxelSLAM/src/loop_refine.hpp:16-17

复制代码 
struct ScanPose

Used in the loop closure pipeline. Each ScanPose holds a keyframe's pose and its

associated point cloud.

用于回环闭合流程。每个 ScanPose 持有关键帧的位姿及关联点云。

Fields / 字段

Field Type Meaning / 含义
x IMUST Full IMU state (pose) / 完整IMU状态
pvec PVecPtr Shared pointer to point cloud with covariance / 带协方差点云的共享指针
v6 Matrix<double,6,1> 6-DOF correction vector / 6自由度修正向量

VERIFY: VoxelSLAM/src/loop_refine.hpp:19-21

Methods / 方法

Method Description / 描述
update(dx) Apply rigid transform: v = dx.Rv, p = dx.Rp + dx.p, R = dx.R*R. 应用刚体变换。
set_state(pose) Set from GTSAM Pose3 (for PGO result). 从GTSAM Pose3设置状态。

VERIFY: VoxelSLAM/src/loop_refine.hpp:28-42

14. PGO_Edge / PGO_Edges

Loop closure constraint storage / 回环约束存储

PGO_Edge

VERIFY: VoxelSLAM/src/loop_refine.hpp:163

复制代码 
struct PGO_Edge

A single loop closure edge between two map segments, potentially with multiple

scan-level matches.

两个地图段之间的单条回环边，可能包含多组扫描级别的匹配。

Field Type Meaning / 含义
m1, m2 int Map segment indices / 地图段索引
ids1, ids2 vector<int> Scan IDs within each segment / 各段内的扫描ID
rots PLM(3) Relative rotations per match / 每次匹配的相对旋转
tras PLV(3) Relative translations per match / 每次匹配的相对平移
covs PLV(6) 6-DOF covariance diagonal per match / 每次匹配的6自由度协方差对角线

VERIFY: VoxelSLAM/src/loop_refine.hpp:166-170

Methods: push(id1, id2, rot, tra, v6), is_adapt(maps, step).

PGO_Edges

VERIFY: VoxelSLAM/src/loop_refine.hpp:206

复制代码 
struct PGO_Edges

Container for all PGO_Edge instances. Maintains an adjacency list mates for graph

connectivity queries.

所有 PGO_Edge 的容器。维护邻接表 mates 用于图连通性查询。

Field Type Meaning / 含义
edges vector<PGO_Edge> All loop closure edges / 所有回环边
mates vector<vector<int>> Adjacency list (bidirectional) / 邻接表（双向）

VERIFY: VoxelSLAM/src/loop_refine.hpp:209-210

Methods: push(...), connect(root, ids) (BFS/DFS to find connected component),
tras(ord, ids) (recursive traversal).

VERIFY: VoxelSLAM/src/loop_refine.hpp:237-266

15. OctreeGBA

Simplified octree for global BA / 简化的八叉树用于全局BA

VERIFY: VoxelSLAM/src/loop_refine.hpp:273

复制代码 
class OctreeGBA

A lighter-weight octree used for global bundle adjustment after loop closure. Unlike

OctoTree, it does not store measurement covariances or maintain a sliding window --

it simply stores local and world coordinates for each frame and performs a single

recut+optimize cycle.

轻量级八叉树，用于回环后的全局BA。与 OctoTree 不同，不存储测量协方差或维护滑动窗口，

只存储每帧的局部/世界坐标并执行单次切分+优化。

Fields / 字段

Field Type Meaning / 含义
locals vector<PLV(3)> locals[i] = local points for frame i / 第i帧的局部坐标点
worlds vector<PLV(3)> worlds[i] = world points for frame i / 第i帧的世界坐标点
pcr_add PointCluster Accumulated statistics / 累积统计量
layer int Octree depth / 八叉树深度
octo_state int 0=leaf, 1=subdivided / 0=叶节点, 1=已细分
wdsize int Number of frames / 帧数
leaves[8] OctreeGBA*[8] Children / 子节点
voxel_center[3] double[3] Octant center / 八分体中心
quater_length float Half-child-size / 子体素半尺寸
is_plane bool Planarity flag / 平面标志

VERIFY: VoxelSLAM/src/loop_refine.hpp:277-283

Methods / 方法

Method Description / 描述
push(ord, local, world) Add point for frame ord. 添加第ord帧的点。
subdivide(oct_buf) Redistribute all points to 8 children (reuses memory from oct_buf). 将所有点重新分配到8个子节点（复用oct_buf中的内存）。
recut(vox_opt, oct_buf) Eigenvalue test: if plane and well-conditioned, add to LidarFactor; else subdivide. 特征值测试: 若为平面且条件数好则添加到LidarFactor, 否则细分。
tras_ptr(oct_buf) Collect descendant pointers for memory reuse. 收集后代指针用于内存复用。
cut_voxel(feat_map, xc, plptr, ...) Static: insert a point cloud into the hash map, creating OctreeGBA nodes as needed. 静态方法: 将点云插入哈希表。

VERIFY: VoxelSLAM/src/loop_refine.hpp:287-475

16. BTC Types

Binary Triangle Combined (BTC) descriptor structures /
二进制三角组合描述符结构

All types defined in BTC.h. Used for place recognition / loop closure detection.

所有类型定义在 BTC.h 中，用于位置识别/回环检测。

ConfigSetting

VERIFY: VoxelSLAM/src/BTC.h:21

复制代码 
typedef struct ConfigSetting { ... } ConfigSetting;

Central configuration for the BTC descriptor pipeline.

BTC描述符流程的核心配置。

Category / 类别 Key fields / 关键字段
Point cloud pre-processing / 点云预处理 useful_corner_num_ (30)
Keypoint detection / 关键点检测 plane_detection_thre_ (0.01), voxel_size_ (1.0), voxel_init_num_ (10), proj_plane_num_ (3), proj_image_resolution_ (0.5)
STD descriptor / STD描述符 descriptor_near_num_ (10), descriptor_min_len_ (1), descriptor_max_len_ (10), non_max_suppression_radius_ (3.0), std_side_resolution_ (0.2)
Place recognition / 位置识别 skip_near_num_ (20), candidate_num_ (50), rough_dis_threshold_ (0.03), similarity_threshold_ (0.7), icp_threshold_ (0.5)

VERIFY: VoxelSLAM/src/BTC.h:22-56

BinaryDescriptor

VERIFY: VoxelSLAM/src/BTC.h:58

复制代码 
typedef struct BinaryDescriptor { ... } BinaryDescriptor;
Field Type Meaning / 含义
occupy_array_ vector<bool> Binary occupancy grid from 2D projection / 二维投影的二进制占据网格
summary_ unsigned char Hash summary for fast filtering / 快速过滤的哈希摘要
location_ Vector3d 3D position of the corner point / 角点的三维位置

STD (Stable Triangle Descriptor)

VERIFY: VoxelSLAM/src/BTC.h:72

复制代码 
typedef struct STD { ... } STD;

A triangle formed by three corner points. The primary descriptor for loop detection.

由三个角点构成的三角形，回环检测的主要描述符。

Field Type Meaning / 含义
triangle_ Vector3d Sorted side lengths / 排序后的边长
angle_ Vector3d Sorted angles / 排序后的角度
center_ Vector3d Triangle centroid / 三角形重心
frame_number_ int Source frame ID / 来源帧ID
binary_A/B/C_ BinaryDescriptor Descriptor at each vertex / 每个顶点的描述符

BTCPlane

VERIFY: VoxelSLAM/src/BTC.h:85

复制代码 
typedef struct BTCPlane { ... } BTCPlane;
Field Type Meaning / 含义
center_ Vector3d Plane centroid / 平面中心
normal_ Vector3d Plane normal / 平面法向量
covariance_ Matrix3d Point covariance / 点协方差
radius_, d_ float Plane extent and offset / 平面范围和偏移
is_plane_ bool Validity flag / 有效性标志
points_size_ int Number of inlier points / 内点数

STDescManager

VERIFY: VoxelSLAM/src/BTC.h:228

复制代码 
class STDescManager

The main interface for the BTC loop detection system.

BTC回环检测系统的主接口。

Field Type Meaning / 含义
config_setting_ ConfigSetting All parameters / 所有参数
current_frame_id_ unsigned int Frame counter / 帧计数器
data_base_ unordered_map<STD_LOC, vector<STD>> Hash table of all descriptors / 所有描述符的哈希表
plane_cloud_vec_ vector<PointCloud::Ptr> Plane clouds per keyframe / 每关键帧的平面点云

VERIFY: VoxelSLAM/src/BTC.h:232-253

Method Description / 描述
GenerateSTDescs(cloud, stds, id) Extract STD descriptors from point cloud. 从点云提取STD描述符。
SearchLoop(stds, result, transform, pairs, pl_cur) Search for loop closure candidates and compute relative pose. 搜索回环候选并计算相对位姿。
AddSTDescs(stds) Insert descriptors into the hash database. 将描述符插入哈希数据库。
PlaneGeomrtricIcp(src, tgt, transform) Plane-to-plane ICP for geometric verification. 平面到平面ICP用于几何验证。

VERIFY: VoxelSLAM/src/BTC.h:258-274

Relationship Diagram / 结构关系图

复制代码 
+------------------+     contains      +----------------+
|   VOXEL_LOC      |<-- hash key ----->|   OctoTree     |
| (int64 x, y, z)  |                   |  (adaptive)    |
+------------------+                   +-------+--------+
                                               |
                              +----------------+----------------+
                              |                |                |
                        +-----v-----+   +------v------+  +-----v------+
                        | SlideWindow|   | PointCluster|  |   Plane    |
                        | points[]   |   | P, v, N     |  | normal,    |
                        | pcrs_local[]   | (sufficient |  | center,    |
                        +-----+------+   |  statistics)|  | plane_var  |
                              |          +------+------+  +-----+------+
                              |                 |               |
                        +-----v------+          |       used by |
                        |  pointVar  |          |               |
                        | pnt, var   |          |          +----v-------+
                        +------------+          +--------->| LidarFactor|
                                                           | plvec_voxels
                                                           | sig_vecs   |
                                     +-------------------->| coeffs     |
                                     |                     +-----+------+
                                     |                           |
                               +-----+--------+           +-----v----------+
                               | IMU_PRE      |           | BA Optimizers  |
                               | R/p/v_delta  |           | Lidar_BA       |
                               | Jacobians    +---------->| LI_BA          |
                               | cov (15x15)  |           | LI_BA_Gravity  |
                               +--------------+           +----------------+
                                                                  |
                                     +----------------------------+
                                     |
                               +-----v------+       +-----------+
                               |   IMUST     |<----->|  Keyframe |
                               | R,p,v,bg,ba |       | x0, plptr |
                               | g, cov      |       +-----------+
                               +-----+-------+            |
                                     |                     v
                               +-----v------+     +-------+--------+
                               |  ScanPose   |     |  STDescManager |
                               | x, pvec     |     |  (BTC loop     |
                               +-----+-------+     |   detection)   |
                                     |              +-------+--------+
                                     v                      |
                               +-----+------+     +--------v-------+
                               | PGO_Edges  |     |  OctreeGBA     |
                               | edges[]    |     | (global BA)    |
                               | mates[]    |     +----------------+
                               +------------+

Global Variables / 全局变量

Several global variables control octree behavior.

若干全局变量控制八叉树行为。

Variable Type Default Meaning / 含义
max_layer int 2 Maximum octree depth / 最大八叉树深度
max_points int 100 Max points in pcr_fix before stopping accumulation / pcr_fix停止累积的最大点数
voxel_size double 1.0 Top-level voxel size (meters) / 顶层体素尺寸（米）
min_ba_point int 20 Minimum points for BA / BA的最小点数
min_point Vector4d -- Per-layer minimum point threshold / 每层最小点数阈值
min_eigen_value double -- Planarity eigenvalue threshold / 平面性特征值阈值
plane_eigen_value_thre vector<double> -- Per-layer eigenvalue ratio threshold / 每层特征值比率阈值
imu_coef double 1e-4 IMU factor weight in joint optimization / 联合优化中IMU因子权重
mp int* -- Frame index remapping array / 帧索引重映射数组

VERIFY: VoxelSLAM/src/voxel_map.hpp:82-88

VERIFY: VoxelSLAM/src/voxel_map.hpp:446

VERIFY: VoxelSLAM/src/voxel_map.hpp:934

Utility Types / 工具类型

VOXEL_LOC

VERIFY: VoxelSLAM/src/tools.hpp:23

Spatial hash key for voxel indexing. Three int64 coordinates with a polynomial hash

function: ((z * HASH_P) % MAX_N + y) * HASH_P) % MAX_N + x.

体素空间索引的哈希键。三个 int64 坐标，使用多项式哈希函数。

Macro definitions / 宏定义

Macro Expansion Meaning / 含义
PLM(a) vector<Matrix<double,a,a>, aligned_allocator<...>> Aligned vector of a x a matrices / 对齐的 a x a 矩阵向量
PLV(a) vector<Matrix<double,a,1>, aligned_allocator<...>> Aligned vector of a x 1 vectors / 对齐的 a x 1 向量向量
DIM 15 State dimension / 状态维度
G_m_s2 9.8 Gravity constant / 重力常数
NMATCH 5 Number of matches / 匹配数
DVEL 6 Pose dimension (R+t) in LiDAR BA / LiDAR BA中的位姿维度

VERIFY: VoxelSLAM/src/tools.hpp:11-16

VERIFY: VoxelSLAM/src/voxel_map.hpp:448

Lie group helpers / 李群工具函数

Function Location Description / 描述
Exp(ang) tools.hpp:50 so(3) -> SO(3) exponential map (Rodrigues) / 指数映射
Exp(ang_vel, dt) tools.hpp:67 Angular velocity to rotation over dt / 角速度积分为旋转
Log(R) tools.hpp:85 SO(3) -> so(3) logarithm map / 对数映射
hat(v) tools.hpp:92 Vector to skew-symmetric matrix / 向量转反对称矩阵
jr(vec) tools.hpp:101 Right Jacobian of SO(3) / SO(3)右雅可比
jr_inv(R) tools.hpp:117 Inverse of right Jacobian / 右雅可比的逆
相关推荐
ConardLi1 小时前
开源我的 GPT-Image2 生图 Skill，附大量玩法指南
前端·人工智能·后端
QYR_111 小时前
2026卷绕式扣式电池产业洞察：智能制造如何重塑微型储能格局？
人工智能·市场调研
白熊1881 小时前
【大模型Agent】LangGraph 深度科普：为智能体而生的“有状态”编排框架
人工智能·langchain·agent·langgraph
数智工坊1 小时前
【SIoU Loss论文阅读】：引入角度感知的框回归损失，让检测收敛更快更准
论文阅读·人工智能·深度学习·机器学习·数据挖掘·回归·cnn
Via_Neo1 小时前
乘积最大问题
数据结构·算法
bloglin999991 小时前
向量大模型升级可能改变向量空间（需要回归）
人工智能·数据挖掘·回归
AI技术增长1 小时前
Pytorch图像去噪实战（三）：ResUNet图像去噪模型实战，解决UNet深层训练不稳定问题
人工智能·pytorch·深度学习
TDengine （老段）1 小时前
工业软件的未来：构建在工业数据底座之上的 AI Agent
大数据·数据库·人工智能·时序数据库·tdengine
aLTttY1 小时前
Spring Boot集成AI大模型实战：从0到1打造智能应用
人工智能·spring boot·后端
热门推荐
01GitHub 镜像站点02近期有什么ai的新消息，新动态？ 2026.4月032026年4月AI大事件深度解读：大模型竞争进入“深水区“04Codex 接入 DeepSeek API 完整配置文档052026年AI编程工具终极横评：Cursor vs Claude Code vs Copilot06【AI】2026 年具身智能模型和世界模型总结07在Windows 11上安装Docker的踩坑记录08零基础教你claude code 接入 deepseek V4092026年AI前瞻：量子AI、具身智能与科学发现的新纪元10VSCode + Copilot下：配置并使用 DeepSeek