Semantically Assisted Loop Closure in SLAM Using NDT Histograms


Anestis Zaganidis

Alexandros Zerntev

Tom Duckett

Grzegorz Cielniak

LCAS logo

Lincoln Centre For Autonomous Systems, University of Lincoln, UK

The proposed pipeline




  • Point cloud registration: SE-NDT1
  • Semantic segmentation: PointNet++2
  • Map structure: modified Submap per Perspective (SuPer) NDT OM3
  • Loop detection: Semantically assisted NDT Histograms
Semantically assisted SLAM pipeline

1A. Zaganidis, L. Sun, T. Duckett, and G. Cielniak, Integrating deep semantic segmentation into 3-D point cloud registration. IEEE Robotics and Automation Letters, 3(4), pp.2942-2949 2018.

2C.R. Qi, L. Yi, H. Su and L.J. Guibas, Pointnet++: Deep hierarchical feature learning on point sets in a metric space. Advances in neural information processing systems (pp. 5099-5108) 2017.

3D. Adolfsson, S. Lowry, M. Magnusson, A. J. Lilienthal, and H. Andreasson, A Submap per Perspective – Selecting Subsets for SuPer Mapping that Afford Superior Localization Quality ECMR 2019

Point cloud registration

Semantic assisted Normal Distributions Transform



Distributions instead of points

NDT per segment

Correspondences only between components of the same class

Semantic Segmentation

PointNet++



Original SE-NDT used PointNet

Hierarchical processing of the cloud. Division into blocks.
x10 times faster

Trained to recognize 8 classes, Semantic3d.net

Loop closure detection

NDT Histograms




Histogram of component shapes.

  • Linear
  • Planar
  • Spherical

Planar are binned further by direction.
Golden ratio spiral sphere coverage.
Spherical by distance.

All binned also by range.

Orientation buckets on unit sphere

Loop closure detection

NDT Histogram Similarity measure

n dominant directions per descriptor
$\mathcal D_1=\{\mathbf d^1_1...\mathbf d^1_n\}$ and $\mathcal D_2=\{\mathbf d^2_1...\mathbf d^2_n\}$

for all permutations between pairs of the sets
$\mathbf d^1_a,\mathbf d^1_b$ and $\mathbf d^2_c,\mathbf d^2_d$
align $\mathbf d^1_a\times\mathbf d^1_b$ to $\mathbf d^2_c\times\mathbf d^2_d$ (closed form)

Similarity: the minimum of $\|T(\mathbf h_1,\mathbf R)-\mathbf h_2\|_2$

Thresholded to recognise loop

4T. D. Stoyanov, M. Magnusson, H. Andreasson, and A. J. Lilienthal, Fast and Accurate Scan Registration through Minimization of the Distance between Compact 3D NDT Representations. Int. J. Robotics Research, pp. 1–17, 2012.

Semantically assisted NDT Histograms



Extended by 1 dimension.

Total number: $N_{classes}*N_{ranges}*(N_{spherical}+N_{planar}+1)$

Alignment with the same method.

Similarity measure: Kullback-Leibler divergence (P map descriptor, Q local descriptor

\[D_{KL}(P\| Q)=-\sum_{x \in \mathcal X}P(x)log\left(\frac{Q(x)}{P(x)}\right)\]

Semantically assisted NDT Histograms

Decriptor Results (KITTI 00)

Descriptor comparison

Semantically assisted NDT Histograms

Similarity matrix (KITTI 00)

Descriptor comparison

Map representation

Scrolling Submap per Perspective NDT-OM

  • Graph based map.
  • Cells associated with the first node that they were visible.
  • Cells continuously updated (occupancy+covariance).
  • Unloaded and stored when no longer in range.
  • Reloaded only for loop closure validation.
  • Nodes: poses where the place descriptor entropy is a local minima.
  • Range of search from accumulated pose uncertainty.

System validation

Descriptor comparison Descriptor comparison

Realtime, low memory, ROS integration

Thank you

Questions?