---
title: 'C³P-VoxelMap: Compact Probabilistic Voxel Mapping'
url: https://www.emergentmind.com/topics/c-p-voxelmap
type: topic
---

# C³P-VoxelMap: Compact Probabilistic Voxel Mapping

C³P-VoxelMap denotes a **compact, cumulative and coalescible probabilistic voxel mapping** method for direct LiDAR odometry that was introduced to improve **performance, accuracy and memory efficiency** while preserving the probabilistic plane modeling associated with VoxelMap-style registration [2406.01195]. Its central claim is that probabilistic voxel mapping had previously required storing past point clouds and re-iterating on them to update planar uncertainty, which consumed large memory space and CPU cycles. C³P-VoxelMap addresses that cost with a **point-free** voxel representation based on fixed sufficient statistics, then further reduces map redundancy by **lazily merging** voxels that likely belong to the same physical plane through a locality-sensitive hash [2406.01195].

## 1. Problem formulation and nomenclature

C³P-VoxelMap is situated in the setting of voxel-based LiDAR odometry, in which 3D space is partitioned into voxels and each voxel stores a local geometric model, often a plane. Incoming scan points are associated with voxels in the map, and the pose is optimized so points align with the local voxel planes. In the **probabilistic** variant of this paradigm, a voxel plane is represented not only by its geometry but also by an uncertainty model reflecting LiDAR measurement noise and pose estimation noise of points transformed into the world frame [2406.01195].

The method’s title terms are definitional rather than rhetorical. **Compact** means that each voxel stores only a fixed set of sufficient statistics rather than raw historical points. **Cumulative** means that voxel plane parameters and their uncertainty can be updated recursively when new points arrive, without revisiting past points. **Coalescible** means that different voxels that likely correspond to the same physical plane can be merged so that the map adapts to structures such as walls, floors, and ceilings [2406.01195].

| Term | Meaning | Mechanism |
|---|---|---|
| Compact | Fixed storage per voxel | Point-free sufficient statistics |
| Cumulative | Recursive update without revisiting past points | Updates of \(\mathbf{q}\), \(\mathbf{A}\), and uncertainty statistics |
| Coalescible | Merge voxels on the same physical plane | Lazy locality-sensitive hash triggering |

The motivation is explicitly computational. Prior probabilistic voxel mapping is described as **non-cumulative probabilistic updating** with time complexity \(O(MN)\) and space complexity \(O(N)\), where \(N\) is the number of points in a voxel and \(M\) is the number of update iterations. C³P-VoxelMap replaces this with constant per-voxel storage and cumulative statistics, changing update behavior to \(O(N)\) total over \(N\) points and per-voxel storage to \(O(1)\) [2406.01195].

A common misunderstanding is to treat C³P-VoxelMap as a new state estimator. The paper instead states that the **state estimation algorithm remains the same as prior IESKF-based methods, especially VoxelMap/Fast-LIO2**; the novelty lies in map representation and map management rather than in a new filtering architecture [2406.01195].

## 2. Probabilistic voxel representation

Each voxel in C³P-VoxelMap represents a local planar feature using a plane normal \(\mathbf{n}\), a center point \(\mathbf{q}\), a covariance matrix for plane uncertainty, and a fixed set of sufficient statistics that support cumulative update. The method follows the probabilistic plane representation of VoxelMap, with a joint covariance partitioned as
\[
\boldsymbol{\Sigma}_{\mathbf{n},\mathbf{q}} =
\begin{bmatrix}
\boldsymbol{\Sigma}_{\mathbf{n}\mathbf{n}} & \boldsymbol{\Sigma}_{\mathbf{n}\mathbf{q}} \\
\boldsymbol{\Sigma}_{\mathbf{n}\mathbf{q}}^T & \boldsymbol{\Sigma}_{\mathbf{q}\mathbf{q}}
\end{bmatrix}.
\]
This representation makes the plane itself a random object rather than a deterministic least-squares fit [2406.01195].

Given points \(\{\mathbf{p}_i\}_{i=1}^N\) in a voxel, the paper defines the voxel mean and covariance as
\[
\mathbf{q}=\frac{1}{N}\sum_{i=1}^N \mathbf{p}_i, \qquad
\mathbf{A}=\frac{1}{N}\sum_{i=1}^N \mathbf{p}_i\mathbf{p}_i^T-\mathbf{q}\mathbf{q}^T.
\]
An eigendecomposition is then applied:
\[
\mathbf{A}=\mathbf{U}\operatorname{diag}(\lambda_1,\lambda_2,\lambda_3)\mathbf{U}^T, \qquad
\mathbf{U}=[\mathbf{u}_1,\mathbf{u}_2,\mathbf{u}_3],
\]
with eigenvalues ordered as
\[
\lambda_1 \ge \lambda_2 \ge \lambda_3.
\]
The plane normal is the minor principal direction,
\[
\mathbf{n}=\mathbf{u}_3.
\]
This is the local PCA plane model used throughout the method [2406.01195].

Point uncertainty enters through a first-order perturbation model. Each point is treated as \(\mathbf{p}_i+\boldsymbol{\delta}_{\mathbf{p}_i}\), with zero-mean Gaussian noise and covariance \(\boldsymbol{\Sigma}_{\mathbf{p}_i}\in\mathbb{R}^{3\times 3}\). The plane parameters are then linearized as
\[
[\mathbf{n}^{gt},\mathbf{q}^{gt}]^T \approx [\mathbf{n},\mathbf{q}]^T +\sum_{i=1}^N \frac{\partial \mathbf{f}}{\partial \mathbf{p}_i}\boldsymbol{\delta}_{\mathbf{p}_i},
\]
leading to
\[
\boldsymbol{\Sigma}_{\mathbf{n},\mathbf{q}} = \sum_{i=1}^N \frac{\partial \mathbf{f}}{\partial \mathbf{p}_i} \boldsymbol{\Sigma}_{\mathbf{p}_i} \frac{\partial \mathbf{f}}{\partial \mathbf{p}_i}^T,
\qquad
\frac{\partial \mathbf{f}}{\partial \mathbf{p}_i} =
\left[
\frac{\partial \mathbf{n}}{\partial \mathbf{p}_i},
\frac{\partial \mathbf{q}}{\partial \mathbf{p}_i}
\right].
\]
The Jacobians are given as
\[
\frac{\partial \mathbf{n}}{\partial \mathbf{p}_i} =
\mathbf{U}
\begin{bmatrix}
(\mathbf{p}_i-\mathbf{q})^T \mathbf{F}_1 \\
(\mathbf{p}_i-\mathbf{q})^T \mathbf{F}_2 \\
(\mathbf{p}_i-\mathbf{q})^T \mathbf{F}_3
\end{bmatrix},
\qquad
\frac{\partial \mathbf{q}}{\partial \mathbf{p}_i} =
\operatorname{diag}\left(\frac{1}{N},\frac{1}{N},\frac{1}{N}\right),
\]
with
\[
\mathbf{F}_m=
\begin{cases}
\frac{1}{N(\lambda_3-\lambda_m)} (\mathbf{u}_m\mathbf{n}^T+\mathbf{n}\mathbf{u}_m^T), & m\neq 3,\\
\mathbf{0}_{1\times 3}, & m=3.
\end{cases}
\]
These formulas are inherited from first-order perturbation analysis of PCA plane fitting [2406.01195].

The representation is termed **point-free** because the original point set and per-point covariances are not retained after their information has been absorbed into sufficient statistics. This does not mean that no points were used; rather, it means that the map no longer stores them after accumulation [2406.01195].

## 3. Compact cumulative uncertainty update

The central technical contribution of C³P-VoxelMap is the derivation of cumulative updates for probabilistic voxel planes without retaining historical points. For geometric moments, when a new point \(\mathbf{p}\) arrives in a voxel with current count \(N\), mean \(\mathbf{q}\), and covariance matrix \(\mathbf{A}\), the updated mean is
\[
\mathbf{q}' = \frac{N}{N+1}\mathbf{q}+\frac{1}{N+1}\mathbf{p},
\]
and the updated covariance matrix is
\[
\mathbf{A}'= \frac{N}{N+1}(\mathbf{A}+\mathbf{q}\mathbf{q}^T) + \frac{1}{N+1}\mathbf{p}\mathbf{p}^T - \mathbf{q}'{\mathbf{q}'}^T.
\]
The new normal \(\mathbf{n}'\) is then obtained as the third singular vector of \(\mathbf{A}'\) [2406.01195].

The difficulty is not the recursive update of \(\mathbf{q}\) or \(\mathbf{A}\), but the recursive update of plane uncertainty. In the original probabilistic formulation,
\[
\boldsymbol{\Sigma}_{\mathbf{n},\mathbf{q}} = \sum_{i=1}^N \frac{\partial \mathbf{f}}{\partial \mathbf{p}_i} \boldsymbol{\Sigma}_{\mathbf{p}_i} \frac{\partial \mathbf{f}}{\partial \mathbf{p}_i}^T,
\]
the Jacobians depend on the current mean \(\mathbf{q}\), the current eigensystem \((\mathbf{U},\lambda_1,\lambda_2,\lambda_3)\), and each historical point \(\mathbf{p}_i\). C³P-VoxelMap resolves this by rewriting the covariance so that all point-dependent terms can be accumulated once into fixed statistics [2406.01195].

The sufficient statistics are
\[
\mathbf{X}_{j,k} = \sum_{i=1}^N \mathbf{e}_j^T \boldsymbol{\Sigma}_{\mathbf{p}_i}\mathbf{e}_k\, \mathbf{p}_i\mathbf{p}_i^T, \qquad \forall j,k\in\{1,2,3\},
\]
\[
\mathbf{Y}_j = \sum_{i=1}^N \boldsymbol{\Sigma}_{\mathbf{p}_i}\mathbf{e}_j \mathbf{p}_i^T, \qquad \forall j\in\{1,2,3\},
\]
\[
\mathbf{Z} = \sum_{i=1}^N \boldsymbol{\Sigma}_{\mathbf{p}_i}.
\]
Here \(\mathbf{e}_1,\mathbf{e}_2,\mathbf{e}_3\) are the standard basis vectors in \(\mathbb{R}^3\). Rather than storing all \(\mathbf{p}_i\) and \(\boldsymbol{\Sigma}_{\mathbf{p}_i}\), the voxel stores these accumulators together with \(N\), \(\mathbf{q}\), and \(\mathbf{A}\) [2406.01195].

For the normal covariance block, the paper gives
\[
\boldsymbol{\Sigma}_{\mathbf{n}\mathbf{n}} = \mathbf{U}\mathbf{B}\mathbf{U}^T,
\]
where the entries of \(\mathbf{B}\) are
\[
b_{mn} = \sum_{i=1}^N (\mathbf{p}_i-\mathbf{q})^T \mathbf{F}_m \boldsymbol{\Sigma}_{\mathbf{p}_i} \mathbf{F}_n^T (\mathbf{p}_i-\mathbf{q}).
\]
A key rewritten term is
\[
b_{mn}^{pp} = \sum_{j=1}^3\sum_{k=1}^3 \operatorname{tr} \left( \mathbf{X}_{j,k}\mathbf{F}_m\mathbf{e}_j\mathbf{e}_k^T\mathbf{F}_n^T \right),
\]
which makes explicit that the point-dependent content can be carried entirely by \(\mathbf{X}_{j,k}\). The paper states that all covariance blocks can be reconstructed from \(\mathbf{X}_{j,k}\), \(\mathbf{Y}_j\), and \(\mathbf{Z}\) plus the current PCA state [2406.01195].

The compactness claim is quantified. A symmetric \(3\times 3\) matrix stores 6 scalars, an asymmetric \(3\times 3\) matrix stores 9, there are 6 distinct \(\mathbf{X}_{j,k}\) matrices if symmetry is exploited, 3 matrices \(\mathbf{Y}_j\), and 1 symmetric \(\mathbf{Z}\). The total is
\[
6\times 6 + 3\times 9 + 6 = 69.
\]
Thus the uncertainty of a probabilistic plane can be represented by **69 scalars**, independent of the number of points in the voxel [2406.01195].

This cumulative reformulation is presented as the reason the method reduces runtime complexity from \(O(MN)\) to \(O(N)\) and space complexity from \(O(N)\) to \(O(1)\) [2406.01195].

## 4. Coalescing planar structure

The second major component of C³P-VoxelMap is voxel coalescing. Regular voxel maps split large physical planes into many separate local estimates, which increases memory use and produces noisy inter-voxel variation. C³P-VoxelMap addresses this by merging voxels that likely belong to the same physical plane, with the explicit aim of both reducing redundancy and improving map quality through **cross-voxel denoising** [2406.01195].

The method does not scan the entire map continuously for merge candidates. Instead, it uses a **locality-sensitive hash** on plane descriptors and triggers merging only when a bucket has accumulated enough likely candidates. A plane is parameterized by spherical coordinates of the normal, \(\theta\) and \(\varphi\), together with the signed offset
\[
d = -\mathbf{n}^T\mathbf{q}.
\]
Because \((\theta,\varphi,d)\) alone cannot distinguish distant but similar planes, the paper also introduces in-plane locality coordinates \((u,v)\). Using
\[
\mathbf{R}= \begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\cos\varphi & 0 & \sin\varphi \\
0 & 1 & 0 \\
-\sin\varphi & 0 & \cos\varphi
\end{bmatrix},
\]
the in-plane coordinates are
\[
\begin{bmatrix} u\\ v \end{bmatrix}
=
\begin{bmatrix}
\mathbf{e}_2^T \mathbf{R}\mathbf{q} \\
\mathbf{e}_3^T \mathbf{R}\mathbf{q}
\end{bmatrix}.
\]
The resulting 5D quantized key is
\[
\mathbf{k} = \left(
\left\lfloor\frac{\theta}{\Delta\theta}\right\rfloor,
\left\lfloor\frac{\varphi}{\Delta\varphi}\right\rfloor,
\left\lfloor\frac{d}{\Delta d}\right\rfloor,
\left\lfloor\frac{u}{\Delta u}\right\rfloor,
\left\lfloor\frac{v}{\Delta v}\right\rfloor
\right)^T.
\]
Voxels with similar orientation, offset, and in-plane locality hash into the same bucket [2406.01195].

When a bucket accumulates enough voxels, a merge operation is triggered. The paper states that the voxel with the most points is selected as the reference, because a voxel with more points has lower plane uncertainty. Candidate voxels are then tested against a planarity criterion based on the covariance eigenvalues:
\[
\frac{\lambda_3}{\lambda_1} < \eta, \qquad \frac{\lambda_3}{\lambda_2} < \eta,
\]
where \(\eta\) is a small threshold near zero [2406.01195].

Once a voxel is merged into the reference, the merged voxel’s own parameters \(\mathbf{n}\), \(\mathbf{q}\), and \(\boldsymbol{\Sigma}_{\mathbf{n},\mathbf{q}}\) are released, and future points falling into that voxel update the reference voxel cumulatively. For the merged moments of two point sets \(\mathbb{P}\) and \(\mathbb{Q}\), with sizes \(M\) and \(N\), means \(\boldsymbol{\mu}_p,\boldsymbol{\mu}_q\), and covariances \(\boldsymbol{\Sigma}_p,\boldsymbol{\Sigma}_q\), the paper gives
\[
\boldsymbol{\mu}_{pq}=t\boldsymbol{\mu}_p+(1-t)\boldsymbol{\mu}_q, \qquad t=\frac{M}{M+N},
\]
\[
\boldsymbol{\Sigma}_{pq} = t\boldsymbol{\Sigma}_p+(1-t)\boldsymbol{\Sigma}_q +t(1-t)(\boldsymbol{\mu}_p-\boldsymbol{\mu}_q)(\boldsymbol{\mu}_p-\boldsymbol{\mu}_q)^T.
\]
Since \(\mathbf{X}_{j,k}\), \(\mathbf{Y}_j\), and \(\mathbf{Z}\) are accumulators, a plausible implication is that their merged values follow by addition; the paper states this principle for the accumulated statistics but does not box those equations explicitly [2406.01195].

This merge strategy differs materially from the union-find plane-merging design of "VoxelMap++" [2308.02799]. VoxelMap++ stores one plane feature with a 3DOF representation per voxel and merges **kid planes** into **father planes** incrementally through covariance-aware coplanarity tests and union-find [2308.02799]. C³P-VoxelMap, by contrast, preserves VoxelMap-style probabilistic planes and performs **lazy** merging through a locality-sensitive hash, with bucket-triggered coalescing rather than constant scanning [2406.01195].

## 5. Odometry integration and position within the VoxelMap lineage

C³P-VoxelMap is integrated into a direct LiDAR odometry pipeline whose estimation backbone remains the **Iterative Error State Kalman Filter (IESKF)** used in prior systems. The paper states that incoming scan points are transformed using the current pose estimate, associated to voxel planes directly by hashing into the voxel map, used in probabilistic plane residuals for IESKF state update, and then inserted to update or create voxels in the map [2406.01195].

The probabilistic side of this formulation is clarified in the derivation note "On the Derivation of Tightly-Coupled LiDAR-Inertial Odometry with VoxelMap" [2603.15471]. That note formalizes a VoxelMap-based tightly-coupled LIO system in which each voxel maintains one probabilistic plane feature, LiDAR points are matched through point-to-plane residuals, and the residuals are fused directly inside an IESKF rather than through a loosely coupled LiDAR odometry estimate [2603.15471]. C³P-VoxelMap does not replace that estimator; it changes the map representation so that the probabilistic plane model becomes compact and cumulative [2406.01195].

The paper explicitly attributes part of its runtime advantage to direct voxel-plane lookup. It notes a runtime advantage over k-d-tree methods and approximate NNS voxel methods because no nearest-neighbor search is needed in the same way and each voxel directly stores a plane [2406.01195]. This places C³P-VoxelMap within a broader evolution of voxel-based registration systems that use per-voxel geometric primitives instead of raw point neighborhoods at query time.

Within the VoxelMap lineage, the method can be summarized as follows. VoxelMap supplied the probabilistic voxel-plane idea but required historical point storage and re-iteration. VoxelMap++ added mergeable coplanar plane processing across voxels, using a union-find structure and a 3DOF plane representation [2308.02799]. C³P-VoxelMap retains the probabilistic plane representation associated with VoxelMap, makes it **point-free** and **cumulative**, and adds **LSH-based lazy coalescing** to reduce map size and improve denoising [2406.01195].

A common misconception is that coalescing here is merely a memory-compression device. The paper presents it as more than compression: on-demand merging is said to reduce memory footprint with minimal computational overhead **and** improve localization accuracy thanks to cross-voxel denoising [2406.01195].

## 6. Evaluation, limitations, and significance

The experimental evaluation uses **KITTI odometry**, **UTBM**, and **self-collected indoor/structured data** with a **Livox Mid-360** solid-state LiDAR. The compared baselines are **Fast-LIO2**, **Faster-LIO**, **VoxelMap**, **VoxelMap++**, and **LiTAMIN2**. The reported metrics are **Absolute Trajectory Error (ATE)** in meters, state estimation time, map update time, total time, and memory [2406.01195].

The paper’s headline summary is that experiments exhibit **20% higher accuracy, 20% faster performance and 70% lower memory consumption than the state-of-the-art** [2406.01195]. The detailed tables show that on **UTBM** the average ATE is **11.09** for C³P-VoxelMap, compared with **13.13** for VoxelMap, **15.38** for Faster-LIO, and **14.9** for Fast-LIO2. On **KITTI**, the weighted average ATE is **2.74** for C³P-VoxelMap, compared with **3.95** for VoxelMap, **5.25** for Faster-LIO, **6.24** for Fast-LIO2, and **5.1** for LiTAMIN2 [2406.01195].

The timing results separate the effects of cumulative updating and merging. The average total time on KITTI is **40.43 ms** for **Ours (with merge)**, **36.97 ms** for **Ours (without merge)**, **50.84 ms** for **VoxelMap**, **43.69 ms** for **Fast-LIO2**, and **56.75 ms** for **Faster-LIO**. For map update specifically, the times are **9.06 ms** for **Ours w/ merge**, **5.38 ms** for **Ours w/o merge**, and **17.66 ms** for **VoxelMap**. State estimation time is **31.37 ms** for **Ours w/ merge** and **33.18 ms** for **VoxelMap** [2406.01195].

The memory results are a primary part of the paper’s argument. On **Sequence 04**, memory is **243.7 MB** for C³P-VoxelMap and **521.0 MB** for VoxelMap. On **Sequence 06**, the values are **315.9 MB** and **1433.6 MB** respectively. On **Sequence 09**, they are **878.7 MB** and **2150.4 MB**. In the **Indoor** case, the reported memory is **229.6 MB** for C³P-VoxelMap, **1433.6 MB** for VoxelMap, **5017.6 MB** for VoxelMap++, and **292.8 MB** for Faster-LIO [2406.01195]. The paper also states that its memory remains constant with respect to the maximum number of voxel points \(N\), while VoxelMap grows linearly [2406.01195].

The effective ablation structure is implicit rather than formal. **Ours (without merge)** isolates the cumulative point-free update and shows that it already yields major speed and memory gains over VoxelMap. **Ours (with merge)** adds coalescing, which slightly increases map-update overhead relative to the no-merge variant but improves accuracy and further reduces map footprint by reducing voxel count [2406.01195].

Several boundaries of the method are explicit in the text. First, the approach remains a **plane-only voxel feature model**; it is designed around PCA planar structure and probabilistic plane uncertainty, not a richer nonplanar local descriptor. Second, the provided text does not numerically specify the planarity threshold \(\eta\), the LSH bucket widths \(\Delta\theta,\Delta\varphi,\Delta d,\Delta u,\Delta v\), or the number of voxels required to trigger a merge. Third, the exact IESKF residual equations are not restated in the paper excerpt, because the method keeps the same estimator as prior work and refers readers to VoxelMap and Fast-LIO2 for those details [2406.01195].

The broader significance of C³P-VoxelMap is therefore architectural. It demonstrates that **probabilistic voxel mapping need not be point-caching**, that voxel-plane uncertainty can be maintained from a constant-size summary, and that inter-voxel planar redundancy can be exploited without continuous global scanning [2406.01195]. A plausible implication is that the method narrows a longstanding tradeoff in voxel-based LiDAR odometry: probabilistic modeling no longer necessarily entails linear memory growth with historical points, and map compactness no longer necessarily requires abandoning local probabilistic plane features.

Source: https://www.emergentmind.com/topics/c-p-voxelmap