LSG-CPD: Local Surface Geometry Registration

• LSG-CPD is a probabilistic rigid point cloud registration algorithm that integrates local geometry by adaptively blending point-to-point and point-to-plane penalties.
• It employs an anisotropic Gaussian mixture model using PCA-derived surface flatness cues to drive a GPU-accelerated Expectation-Maximization optimization.
• Benchmark results show significant improvements in accuracy and robustness over traditional CPD methods on datasets such as Stanford Bunny and KITTI-07.

Local Surface Geometry CPD (LSG-CPD) is a probabilistic rigid point cloud registration algorithm that integrates local surface geometric information into the Coherent Point Drift (CPD) framework. LSG-CPD adaptively blends point-to-point and point-to-plane penalties by modeling each Gaussian mixture component in the target point cloud with an anisotropic covariance shaped according to local surface flatness. The method is formulated as a maximum likelihood estimation problem and solved via Expectation-Maximization (EM) with specialized GPU-accelerated routines, achieving significant improvements in accuracy, robustness, and computational efficiency over previous CPD-type methods (Liu et al., 2021).

1. Probabilistic Formulation and GMM Model

LSG-CPD considers two point clouds: the “target” $Y = \{y_m\}_{m=1}^M$ (with estimated normals $n_m$) and the “source” $X = \{x_n\}_{n=1}^N$. The goal is to find a rigid transformation $g \in SE(3)$, parameterized as $g(x) = R x + t$, that maximizes the likelihood of the transformed source points $g(X)$ under a Gaussian mixture model (GMM) placed on the target points, plus a uniform outlier distribution.

The resulting density for an observed point $x$ is: $$p(x) = w_n \, p_o(x) + (1-w_n) \sum_{m=1}^M \pi_m\, p(x|m)$$ where $\pi_m$ are the mixture weights (typically uniform), $w_n$ is the per-point outlier weight, and $p_o(x) = 1/V$ is a uniform density over volume $V$.

Standard CPD and similar approaches use isotropic covariances, $p(x|m) = \mathcal{N}(x; y_m, \sigma^2 I)$, which penalizes only Euclidean distance. LSG-CPD, in contrast, employs anisotropic covariances modulated by local surface geometry: $$p(x|m) = c_m \exp\bigl[-\tfrac{1}{2}(x-y_m)^\top \Sigma_m^{-1}(x-y_m)\bigr]$$

$$\Sigma_m^{-1} = \frac{1}{\sigma^2}\bigl[I + \alpha_m n_m n_m^\top\bigr]$$

where $\alpha_m$ determines the degree of anisotropy and incorporates surface flatness information.

2. Local Surface Geometry Estimation and Covariance Adaptation

LSG-CPD quantifies the flatness of the local surface around each target point using Principal Component Analysis (PCA) of neighborhoods. For each $y_m$, the $K$-nearest neighbors are used to compute the $3 \times 3$ covariance matrix; the eigenvalues $\lambda_1 \geq \lambda_2 \geq \lambda_3 \geq 0$ then yield the surface variation: $$\kappa_m = \frac{\lambda_3}{\lambda_1+\lambda_2+\lambda_3}, \quad \kappa_m \in [0, 1/3]$$ Small $\kappa_m$ indicates a locally planar region; large $\kappa_m$ corresponds to curved or noisy regions.

The anisotropy $\alpha_m$ is set by a sigmoid function of $1/\kappa_m$: $$\alpha_m = \alpha_{\mathrm{max}}\, \frac{1 - \exp\bigl(\lambda(3-1/\kappa_m)\bigr)}{1 + \exp\bigl(\lambda(3-1/\kappa_m)\bigr)}$$ As $\kappa_m \to 0$, $\alpha_m$ approaches $\alpha_{\mathrm{max}}$ (maximal point-to-plane penalty); as $\kappa_m \to 1/3$, $\alpha_m$ approaches $0$ (isotropic behavior). This mechanism enables LSG-CPD to blend between point-to-point and point-to-plane registration adaptively, leveraging local geometric cues.

3. Expectation–Maximization Algorithm for Registration

The registration is performed by maximizing the (expected) complete-data log-likelihood $Q(g, \sigma^2)$ via EM:

• E-step: Calculate the posterior responsibilities for assignment of source points to GMM components:

$$P_{mn} = \frac{(1-w_n)\pi_m\, p(g_{\mathrm{old}}(x_n) | m)}{w_n\,p_o + (1-w_n)\sum_{k=1}^{M}\pi_k\, p(g_{\mathrm{old}}(x_n) | k)}$$

Closed-form and efficient GPU-friendly matrix expressions are derived by precomputing matrices $A$, $B$, $C$, $D$ (with $A$, $B$ incorporating geometric and covariance terms, and $C_{mn}=(1-w_n)/w_n \pi_m$) and evaluating elementwise and matrix operations.

• M-step: Update the transformation $g$ by maximizing $Q(g, \sigma^2)$. There is no closed-form for anisotropic covariances, so $g$ is updated in $SE(3)$ via Newton optimization on its Lie algebra. The right-derivatives and Hessian of $Q$ are computed in closed form:

$$E_i^r Q = 2\,\sum_{m,n} P_{mn} (\tilde g \tilde x_n - \tilde y_m)^\top \tilde \Sigma_m^{-1} \tilde g E_i \tilde x_n$$

The transformation is updated as:

$$\tilde g \leftarrow \tilde g \cdot \exp\left\{ \frac{1}{2} (H+H^\top)^{-1} \nabla Q \right\}$$

The scale parameter $\sigma^2$ is optimized in one dimension by solving $\partial Q/\partial \sigma^2 = 0$.

This EM routine typically converges in 10–20 iterations, with each iteration dominated by $O(MN)$ matrix operations.

4. Implementation Considerations and Computational Complexity

LSG-CPD is implemented with dense GPU arrays for point coordinates, normals, surface variation, and related matrices. Key routines:

• Formation of $A$, $B$ matrices via matrix multiplies and elementwise kernel operations
• Evaluation of the elementwise exponential and normalization to form $K$ and $P$
• Accumulation of the EM objective, gradient, and Hessian

Complexity per EM iteration is $O(MN)$ for both the E-step and the gradient/Hessian computation in the M-step. Solving the $6 \times 6$ system for the SE(3) update is negligible in cost.

On commodity hardware (i9 CPU + RTX2080Ti GPU), LSG-CPD achieves substantial speedups over legacy CPD implementations. For example, without outliers, LSG-CPD completes in 89 ms versus 450 ms; with a 1:1 outlier ratio, 346 ms versus 9.94 s.

5. Performance Evaluation and Benchmarking

LSG-CPD has been benchmarked on several standard datasets and tasks:

• Stanford Bunny (synthetic outliers/noise): At outlier-ratio 0.5, CPD/FilterReg errors are 2–3 mm, while LSG-CPD achieves 0.6 mm mean registration error.
• Multi-view object modeling (Dragon, Happy, Armadillo): Chain registration error (in mm): LSG-CPD obtains 0.97/1.08/0.43, outperforming TrICP, FilterReg, and EMPMR.
• Stanford Lounge (RGB-D indoor): Achieves mean Euler-angle deviation of 0.42° at 19 FPS with confidence filtering and truncation (baseline best ≈1.0°).
• KITTI-07 (LiDAR Odometry): Relative rotation/translation per frame: LSG-CPD (0.062°/0.022 m) improves upon ICP-pt2pt, GICP, and FilterReg. Last-frame drift: LSG-CPD 0.382 m, FilterReg 5.37 m, ICP-pt2pl 58.9 m.

Increased robustness is observed for outliers and noise, with the method reverting to isotropic covariance (standard CPD) in highly curved or noisy regions via modulation of $\alpha_m$. The inclusion of confidence filtering and the outlier model further enhances resilience.

6. The Role of Local Geometry in Registration Accuracy

Incorporation of local surface geometry provides significant benefits:

• For planar regions, the anisotropic covariance $\Sigma_m$ (large $\alpha_m$) enforces a strong point-to-plane constraint, improving alignment along tangent directions.
• In curved or noisy regions, $\alpha_m \to 0$, yielding isotropic Gaussians and mitigating the influence of unreliable geometric information.
• The blended penalty structure provides improved accuracy over both point-to-point and fixed point-to-plane probabilistic methods, as empirically demonstrated in multiple tasks (Liu et al., 2021).

A plausible implication is that adaptive consideration of local geometry can generalize to other registration and probabilistic modeling frameworks beyond rigid transformations.

7. Summary and Availability

LSG-CPD introduces an adaptively anisotropic GMM for point cloud registration that unifies the robustness of probabilistic matching with the geometric precision of local surface-aware penalties. The method leverages efficient GPU-accelerated matrix operations in EM, achieves state-of-the-art accuracy and speed across diverse datasets, and adapts dynamically to local surface properties, resulting in improved registration performance in adversarial scenarios. Source code is publicly available at https://github.com/ChirikjianLab/LSG-CPD.git (Liu et al., 2021).