# Localization and Offline Mapping of High-Voltage Substations in Rough
Terrain Using a Ground Vehicle

Ioannis Alamanos∗, George P. Moustris and Costas S. Tzafestas All authors are
with the School of Electrical & Computer Engineering, National Technical
University of Athens, Greece, Corresp. author email:
<EMAIL_ADDRESS>
###### Abstract

This paper proposes an efficient hybrid localization framework for the
autonomous navigation of an unmanned ground vehicle in uneven or rough
terrain, as well as techniques for detailed processing of 3D point cloud data.
The framework is an extended version of FAST-LIO2 algorithm aiming at robust
localization in known point cloud maps using Lidar and inertial data. The
system is based on a hybrid scheme which allows the robot to not only localize
in a pre-built map, but concurrently perform simultaneous localization and
mapping to explore unknown scenes, and build extended maps aligned with the
existing map. Our framework has been developed for the task of autonomous
ground inspection of high-voltage electrical substations residing in rough
terrain. We present the application of our algorithm in field trials, using a
pre-built map of the substation, but also analyze techniques that aim to
isolate the ground and its traversable regions, to allow the robot to approach
points of interest within the map and perform inspection tasks using visual
and thermal data.

##  I Introduction

The localization problem refers to the pose estimation of a mobile robot
within a prior map. This research topic, although necessary for autonomous
navigation of robots in a known environment, it has not attracted enough
attention throughout the last years, especially in the case of outdoor
navigation in uneven or rough terrain, which requires the use of the three-
dimensional information of the surroundings, and not just a 2D slice. For this
reason, most of the currently used localization algorithms are just wrappers
of well-known simultaneous localization and mapping (SLAM) algorithms which
emphasize the incremental creation of a 3D map with concurrent estimation of
the pose of the robot. Besides, these extended versions of the SLAM algorithms
focus on the exclusive localization within a known map. However, in a dynamic
environment where the map continually changes, such a system would fail to
localize as it would not find enough correspondences between 3D points, or
present many mismatches. Only if the algorithm updates the map, can it
accurately localize within it, with reference to the prior map.

Figure 1: Generated Map of HVSS.

Crafting a utilizable 3D point cloud is a crucial task. Every current Lidar-
inertial odometry algorithm builds a noisy 3D point cloud. The noise comes
from errors of the Lidar measurements, the motion undistortion from the
Inertial Measurement Unit (IMU) measurements, as well as the misregistration
of points. As a result the surfaces of the objects can have an undesirable
thickness which hinders crucial processes, such as raycasting, used to
determine the visibility region of points of interest, or even accurately
define then within the map. Evidently, generating a sharp de-noised 3D model
is necessary to perform inspection tasks within a known environment.

After acquiring an accurate point cloud and determining the regions of
interest, the next task is to successfully navigate within the map of the
high-voltage electric substation (HVSS). To form a safe and feasible path, the
traversable regions of the ground must be established. The precise extraction
of the ground in a point cloud is not a widely researched topic. Almost every
algorithm is based on the cloth simulation filter (CSF) (e.g. [1, 2]) which
takes the inverted surface and places above cloth nodes which interact with
the points that belong to the ground. Another approach is the three
dimensional random sample consensus (RANSAC) [3]; however it would only be
successful if the ground plane is the dominant plane of the point cloud.

The topic of modeling a point cloud of the ground to obtain information
regarding traversability has been widely investigated in the last years.
Implementations which take advantage of the normals of the point cloud have
been developed to estimate safe regions, even in real-time, with significant
accuracy.

This work describes a complete pipeline for crafting high-quality maps from
collected point clouds and navigating within a rough terrain inside an HVSS
using ground vehicles. The main contributions of this paper are:

  * •

An extended version of FAST-LIO2 able to localize within a known environment
and even update the pre-built map through a hybrid scheme.

  * •

Techniques to smooth and de-noise 3D point clouds generated from real-time
SLAM algorithms and extract the ground map.

  * •

Application of methods to determine the traversable regions of rough ground
terrain for safe navigation within a HVSS.

For the benefit of the community we have made the code open source and can be
found on https://github.com/iral-ntua/FAST_LIO_LOCALIZATION.

##  II Related Work

In recent years, the development of algorithms focusing exclusively on
localization within a known 3D point cloud has been limited, with one notable
exception being the 3D Adaptive Monte Carlo Localization (AMCL) [4]. This
algorithm serves as an extension of the widely employed 2D AMCL, employing a
probabilistic approach with a particle filter. However, its efficiency is
contingent on the availability of precise odometry data to serve as an initial
guess for the optimization problem. Despite its computational efficiency,
obtaining accurate odometric information in 3D often necessitates the use of
SLAM algorithms, which can be computationally demanding.

Conventional localization methods often rely on iterative closest point (ICP)
[5] for aligning each Lidar scan with the prior map. However, these methods
can struggle, particularly with large point clouds, as they may not meet real-
time requirements. In contrast, recent 3D SLAM algorithms utilizing Lidar
sensors have proven both computationally efficient and accurate, alleviating
much of the computational burden. One notable example is the LIORF module
based on the LIO-SAM framework [6], which takes a prior map as input. Another
wrapper involves an extended version of FAST-LIO2, incorporating a module that
asynchronously performs ICP on the known map, adjusting the pose and the map
to the prior. However, the continuous execution of traditional ICP remains
computationally demanding for real-time applications.

To address the challenge of generating a reliable point cloud, various
techniques have been devised for point cloud de-noising. Many traditional
approaches, rooted in image processing, formulate an optimization problem by
leveraging the geometric attributes of the point cloud, such as normals, while
discarding outliers. Among these, the bilateral filter [7] stands out. This
method posits that the de-noised point cloud results from the original point
cloud with a displacement factor for each point. This factor is derived from
point normals via principal component analysis and Gaussian weights. However,
adjusting the parameters of these weights can sometimes be challenging for
users aiming to achieve desirable results.

Another notable method is the guided filter [8], which assumes a linear local
model around each 3D point. It tackles the de-noising task by solving a least
squares problem to minimize the reconstruction residual. The guided filter
excels in preserving edges and sharp shapes, distinguishing itself from other
smoothing filters. Nevertheless, it exhibits less tolerance towards noisy
point clouds, as it is inherently tied to explicit normal estimation.

Recent advancements in point cloud de-noising involve the integration of deep
learning-based models. For instance, the Neural Projection Denoising algorithm
(NPD) [9] estimates reference planes for sets of points by computing the
normal vector for each point using both local and global information. This
approach enhances robustness to noise intensity and curvature variation.
Another innovative approach is the Total Denoising Neural Network [10], which
focuses on unsupervised learning, utilizing only noisy point cloud data for
training. Despite its unsupervised nature, this method produces comparable
experimental results to supervised techniques.

Proper modeling of the ground is a critical task for both wheeled and legged
robots, as it forms an integral part of the core components, along with the
localization module, for ensuring the safe navigation of Unmanned Ground
Vehicles (UGV). A notable work that stands out for its real-time capabilities
is presented in [11]. This approach employs a Bayesian generalized kernel
inference method based on the work of Vega-Brown et al. [12]. The method
involves assigning grid cells to the point cloud and executing the Bayesian
generalized kernel inference in two steps. First, a regression is performed to
obtain a dense elevation map, and second, a classification is carried out to
determine traversable regions.

An extension of this algorithm is discussed in [13], which introduces
additional functionality for semantic segmentation using visual information
from a camera. Segmenting the map based on visual cues to identify traversable
regions becomes crucial, especially as the algorithm, by estimating the
roughness of the terrain, may sometimes overlook movable objects (such as
small plants) or impassable obstacles.

##  III System Overview

Figure 2: Overview of the general pipeline of the system.

The proposed system overview is depicted in Fig. 2. The initial map of the
HVSS is generated by the SLAM algorithm, resulting in a point cloud which
further undergoes a smoothing post-processing step. Subsequently, a ground
extraction technique is applied to separate the terrain, forming the basis for
constructing the traversability map. This map becomes crucial for planning the
motion of the robot, enabling it to explore specific user-defined regions of
interest. Concurrently, taking as input the SLAM-generated point cloud, a
localization module estimates the pose of the UGV within the prior map. This
localization information is essential for executing the motion commands
generated by the planner. The proposed scheme functions as a subsystem within
the broader system, specifically designed for inspection purposes.

##  IV Method

###  IV-A Localization

Our localization framework is based on the architecture of FAST-LIO2 [14].
Unlike most of Lidar SLAM algorithms, FAST-LIO2 is computationally more
efficient and precise due to its novel functionalities. Its basic pipeline for
pose estimation and 3D point cloud creation is the following:

  1. 1.

Scan-to-scan motion undistortion of the raw Lidar points through back-
propagation by using the measurements from an inertial measurement unit (IMU).

  2. 2.

Pose estimation on each scan. To estimate the pose of the UGV on each scan, a
non-linear least squares problem is formulated to minimize the residuals
resulting from the registration of the current scan points with those of the
map. While many methods commonly employ techniques such as Levenberg-Marquardt
or Gauss-Newton, FAST-LIO2 employs an Extended Kalman Filter (EKF). In the
state prediction and covariance calculation steps, the algorithm utilizes IMU
measurements between Lidar scans. During the update step, the residuals from
point-to-plane registrations are minimized. The algorithm incorporates a novel
incremental kd-tree (ikd tree) [15] that demonstrates good performance for
k-nearest neighbors searches in Lidar odometry. In practice, the algorithm
forms a plane from the five nearest neighbors of each current Lidar scan point
on the map. If the point-to-plane distance falls below a predefined threshold,
it registers this point to the plane formed by its neighbors. This plane is
essentially described by its centroid and point normals. By adopting this
approach, FAST-LIO2 effectively refines the UGV’s pose estimation,
demonstrating a balance between computational efficiency and accurate
registration.

  3. 3.

After the state estimation, the method updates the overall point cloud and its
ikd-tree with the new odometry-registered scan.

The contribution of our extended version of FAST-LIO2 can be summarized as
follows:

  1. 1.

Robust localization on pre-built maps.

  2. 2.

Pose initialization within the known map using an initial guess and ICP.

  3. 3.

Publication of complete odometry messages for real-world applications.

  4. 4.

Improved memory handling regarding Lidar and IMU data in case deprecated
messages are received.

Our localization method maintains the core of FAST-LIO2 regarding motion
undistoriton and state estimation. Nevertheless, the ikd-tree of the overall
active map that is used for each scan’s registration, is not formed and
updated incrementally from each scan but is loaded from a prior point cloud
that serves as the initial map. The reason for using the ikd-tree and not a
static kd-tree is that the user has the ability to update the tree and
essentially perform SLAM with a prior map by assigning the desired time to a
parameter. This allows the robot to localize in a known environment and
sequentially explore a new scene, generating an updated point cloud aligned
with the prior one. To the best of our knowledge, there is no other hybrid
SLAM-Localization algorithm that can perform in such scenarios. The only
required input from the user is the prior point cloud and an initial guess of
the pose of the robot relative to the frame of the map. Relevant operation of
our method is demonstrated in Fig. 3.

One of the main challenges of solving the localization problem is to estimate
the initial pose of the robot within the prior map. Given two sets of points
$P=\\{p_{1},...,p_{N}\\},Q=\\{q_{1},...,q_{M}\\}\in\mathbb{R}^{3}$, we seek to
optimize a rigid transformation matrix $T\in\mathbb{R}^{4\times 4}$, comprised
of a rotation matrix $R\in\mathbb{R}^{3\times 3}$ and a translation vector
$t\in\mathbb{R}^{3}$, in order to align P with Q.

$\boldsymbol{T}^{*}=\operatorname*{argmin}_{\boldsymbol{R},\boldsymbol{t}}\sum_{i=1}^{N}||\boldsymbol{R}p_{i}+\boldsymbol{t}-\hat{q_{i}}||^{2}+I_{SO(d)}(\boldsymbol{R}),$
(1)

where $\hat{q_{i}}\in Q$ is the corresponding point of $p_{i}$,
$||\boldsymbol{R}p_{i}+\boldsymbol{t}-\hat{q_{i}}||$ is the distance from the
transformed source point to the target corresponding point, and
$I_{SO(d)}(\boldsymbol{R})$ is an indicator function for the special
orthogonal group $SO(d)$, which requires $\boldsymbol{R}$ to be a rotation
matrix:

$I_{SO(d)}(\boldsymbol{R})=\begin{cases}0,\text{ if
}\boldsymbol{R}^{T}\boldsymbol{R}=\mathbf{I}\text{ and
}det({\boldsymbol{R}})=1,\\\ +\infty,\text{ otherwise}.\end{cases}$ (2)

To estimate the desired transformation matrix $\boldsymbol{T}^{*}$, first we
accumulate the first ten scans (source point cloud) while keeping the Lidar
static, and then perform ICP between these scans and the prior map (target
point cloud) using the PCL library [16]. The ICP method uses an iterative
approach, and assuming that we are on the $k_{th}$ iteration, the problem is
solved in the two following steps:

  1. 1.

Corresponding points: find the closest point $\hat{q_{i}}^{(k)}\in Q$ for each
transformed point $p_{i}$:

$\hat{q_{i}}^{(k)}=\operatorname*{argmin}_{q\in Q}||R^{(k)}p_{i}+t^{(k)}-q||.$
(3)

  2. 2.

Transformation update: optimize the transformation matrix by minimizing the 3D
euclidean distance between the corresponding sets of points:

$\begin{multlined}\boldsymbol{T}^{(k+1)}=\\\
\operatorname*{argmin}_{\boldsymbol{R}^{(k)},\boldsymbol{t}^{(k)}}\sum_{i=1}^{N}||\boldsymbol{R}^{(k)}p_{i}+\boldsymbol{t}^{(k)}-\hat{q_{i}}^{(k)}||^{2}+I_{SO(d)}(\boldsymbol{R}).\end{multlined}\boldsymbol{T}^{(k+1)}=\\\
\operatorname*{argmin}_{\boldsymbol{R}^{(k)},\boldsymbol{t}^{(k)}}\sum_{i=1}^{N}||\boldsymbol{R}^{(k)}p_{i}+\boldsymbol{t}^{(k)}-\hat{q_{i}}^{(k)}||^{2}+I_{SO(d)}(\boldsymbol{R}).$
(4)

To iteratively determine the optimized transformation matrix, ICP solves
equation (4) in closed form via Singular Value Decomposition [17]. The
drawback of this method is that an initial guess of the robot pose is needed
for the ICP to converge and accurately localize the robot to the correct map
position. Considering this constraint, except for the convergence of the ICP,
to ensure an exact initialization within the map, we take advantage of a
Euclidean fitness score which expresses the mean of squared distances from the
source to the target as shown in equation (5). Here $x_{i}$ and $\hat{x_{i}}$
are corresponding points of the source and target clouds. The pose
initialization is considered successful only if the Euclidean fitness score is
below a predefined threshold, set to 0.01 in our case.

$FS=\frac{\sum_{i=1}^{N}(x_{i}-\hat{x_{i}})^{2}}{N}$ (5)

Figure 3: Application of our method within a HVSS.

###  IV-B Map Crafting

To obtain an initial 3D point cloud we used the FAST-LIO2 SLAM algorithm.
However the resulting map, which is formulated by accumulated scans registered
to the odometry frame, is not usable as it contains considerable amount of
noise due to sensor and algorithmic imperfections. Thus, there is an
undesirable thickness to every surface of the point cloud which renders it
impossible to utilize some core methods needed for the autonomous inspection
of the HVSS, e.g.raycasting between points of interest and the ground. To
address this problem and de-noise the point cloud we perform a two step
filtering procedure:

  1. 1.

First, we apply a uniform sampling filter to assign voxels to the 3D
continuous space. For each point we only keep the voxel that is closer to it,
if that exists. By uniformly discretizing the three dimensional space, not
only we remove noise but also keep only necessary meaningful information by
making the point cloud more sparse and with specified structure.

  2. 2.

Following, we use the Moving Least Squares (MLS) [18] algorithm of PCL library
to de-noise and smooth the point cloud. To define surfaces (planes) through
sets of points, MLS minimizes the weighted least square error which best fits
the points to a surface. Specifically, the problem can be solved by minimizing
the following term:

$\sum_{i=1}^{N}(\langle n_{i}p_{i}\rangle-D)^{2}\theta(||p_{i}-q||),$ (6)

where the local plane is defined as: $\kappa={x|\langle
n_{i}p_{i}\rangle-D=0,x\in\mathbb{R}^{3}},n\in\mathbb{R}^{3},||n||=1$, $p_{i}$
is a 3D point, $n_{i}$ its corresponding normal, $D$ the plane model, q the
projection onto the plane, and $\theta$ a smooth monotonous decreasing
function. By aligning every point set to its local surface, the noise from the
point cloud diminishes and concurrently the normal computation of the points
is enhanced. The main drawback of this method is that sharp edges within the
point cloud are slightly smoothed. Nevertheless, as demonstrated in Fig. 4,
this two-step filtering yields significantly enhanced and denoised results.

Figure 4: Raw and filtered part of map (upper and lower correspondingly).

###  IV-C Traversability Mapping

Defining the traversable regions of the rough terrain of a HVSS is essential
for the safe navigation of the UGV. For the precise modeling of the ground,
the input point cloud needs to be noise-free and the ground meticulously
divided from the overground components. To attain this, we first utilize the
previously described method to acquire a ’clean’ point cloud with carefully
computed point normals; following, we use a CSF filter with appropriate
parametrization of cloth resolution and classification threshold, which
expresses the size of each cluster of the map that will be classified as
ground or non-ground, to isolate the terrain (Fig. 5).

Figure 5: (UP) Ground point cloud extracted from CSF. (DOWN) Grid map with
overground structures.

To this end, we utilized the Grid Map open-source library [19], which focuses
on online surface reconstruction and interpretation of rough terrain. Through
this library, we converted the terrain point cloud to a Grid Map object,
essentially interpreting the ground as a grid of cells, assigning to each one
a value expressing its elevation (Fig. 5). To determine the traversable ground
regions, three more filters were applied to corresponding layers,

  1. 1.

Surface Normals Filter: Estimates the normal of each cell and is vital for the
operation of the next filters.

  2. 2.

Slope Filter: Calculates the slope of each cell by directly taking advantage
of the surface normals.

  3. 3.

Roughness Filter: Computes the roughness of each cell by utilizing the
information about the normals surrounding that cell.

By normalizing the values of the Slope and Roughness layers and using
appropriate thresholds to reflect the capability of the ground vehicle to pass
through rough terrain, we can assign a cost to each cell according to its
traversability and thus specify traversable regions. This produces a 2D
costmap which can be used in ROS for subsequent navigation.

##  V Experimental Results

###  V-A Metric and Experimental Setup

In this section we assess the performance of our localization method in
comparison to LIORF and traditional ICP localization. Given that there is no
ground-truth for the evaluation, the metric used for the comparison of the
algorithms is the mean cloud-to-cloud distance of the point clouds generated
using the localization algorithms and the prior map. Taking into account that
the localization point clouds are formed from odometry-registered undistorted
scans and that the purpose of a localization algorithm is the accurate
positioning of the robot within a known map, cloud-to-cloud distance is a
representative metric for the performance of the methods. Cloud-to-cloud
distance is computed by determining corresponding closest points between the
two point clouds as expressed in equation (3), and computing the mean error
between these point sets (same as in equation (5)). Rejecting as outliers
corresponding points whose distance is above from a predefined threshold, is a
key step as new scenes maybe have been mapped during localization and would
adulterate the final results.

All the experiments have been conducted on a laptop computer with an Intel
Core i9-9980 CPU and 32 GB RAM, and the data have been collecting using
Robosense’s RS-LiDAR-16 and Vectornav’s VN-100 IMU. We did not perform any
further processing time evaluation, since the core of the method remains the
same as in FAST-LIO2.

###  V-B Evaluation

For the evaluation of the algorithms we collected two distinct datasets from
the HVSS. From the first dataset we generated the prior map through FAST-LIO2,
while the second was used to assess the localization methods. The experimental
results are presented in Table I.

TABLE I: Performance comparison of localization methods Method | Mean Error (m) | Standard Deviation (m)
---|---|---
ICP | - | -
LIORF | 0.050 | 0.069
FAST-LIO-LOC. | 0.026 | 0.049

From the obtained results, it is evident that our method outperforms the
others in terms of accuracy. Traditional ICP fails to localize the sensor
after a few seconds due to its high computational burden. Although Lidar
operates at 10hz the algorithm is able to perform at 2Hz at most, on an i9
cpu. On the other hand, LIORF is much more effective and localizes the robot
during the whole dataset with low drift. However, it is considerably demanding
on computational resources. In contrast, our method is more precise in terms
of accuracy, but also significantly more lightweight as presented on [14]
(comparison of FAST-LIO2 with LIO-SAM). The alignment of the corresponding
point clouds is presented in Fig. 6. As is evident in the middle depiction,
the red point cloud from LIORF is much more dominant due to the existing noise
around the surfaces, even though significantly more sparse, as only the scans
from the keyframes are projected to the map. On the other hand, on the bottom
picture where every scan from FAST-LIO-LOCALIZATION is aligned to the map, the
colors are balanced as the points lie almost exactly on the same positions
relative to the prior map.

Figure 6: (UP) Raw prior map; (MIDDLE) prior map (Blue) along with point cloud
from LIORF (Red); (BOTTOM) prior map (Blue) along with point cloud from FAST-
LIO-LOCALIZATION (Red).

##  VI Conclusion

This paper proposed a hybrid localization-SLAM method, and an effective scheme
for filtering raw point clouds to generate noise-free maps. Our localization
method is based on the framework of FAST-LIO2, while map crafting incorporates
a two step filtering including a uniform sampling filter, and a smoothing
filter which utilizes moving least squares algorithm. The fabrication of a
noise-free point cloud enables the efficient development of essential tasks
for the inspection of points of interest within the HVSS, and the localization
module is necessary for the safe navigation of the UGV. The demonstrated
material corroborates the effectiveness of our map crafting method, while the
presented results related to localization confirm the superiority of our
method in comparison with the current most robust localization algorithms.

## References

  * [1] Wuming Zhang, Jianbo Qi, Peng Wan, Hongtao Wang, Donghui Xie, Xiaoyan Wang, and Guangjian Yan.  An easy-to-use airborne lidar data filtering method based on cloth simulation.  Remote sensing, 8(6):501, 2016.
  * [2] Shangshu Cai, Sisi Yu, Zhenyang Hui, and Zhanzhong Tang.  Icsf: An improved cloth simulation filtering algorithm for airborne lidar data based on morphological operations.  Forests, 14(8):1520, 2023.
  * [3] Martin A Fischler and Robert C Bolles.  Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography.  Communications of the ACM, 24(6):381–395, 1981.
  * [4] Francisco J Perez-Grau, Fernando Caballero, Antidio Viguria, and Anibal Ollero.  Multi-sensor three-dimensional monte carlo localization for long-term aerial robot navigation.  International Journal of Advanced Robotic Systems, 14(5):1729881417732757, 2017.
  * [5] K Somani Arun, Thomas S Huang, and Steven D Blostein.  Least-squares fitting of two 3-d point sets.  IEEE Transactions on pattern analysis and machine intelligence, (5):698–700, 1987.
  * [6] Tixiao Shan, Brendan Englot, Drew Meyers, Wei Wang, Carlo Ratti, and Daniela Rus.  Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping.  In 2020 IEEE/RSJ international conference on intelligent robots and systems (IROS), pages 5135–5142. IEEE, 2020.
  * [7] Carlo Tomasi and Roberto Manduchi.  Bilateral filtering for gray and color images.  In Sixth international conference on computer vision (IEEE Cat. No. 98CH36271), pages 839–846. IEEE, 1998.
  * [8] Kaiming He, Jian Sun, and Xiaoou Tang.  Guided image filtering.  IEEE transactions on pattern analysis and machine intelligence, 35(6):1397–1409, 2012.
  * [9] Chaojing Duan, Siheng Chen, and Jelena Kovacevic.  3d point cloud denoising via deep neural network based local surface estimation.  In ICASSP 2019-2019 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), pages 8553–8557. IEEE, 2019.
  * [10] Pedro Hermosilla, Tobias Ritschel, and Timo Ropinski.  Total denoising: Unsupervised learning of 3d point cloud cleaning.  In Proceedings of the IEEE/CVF international conference on computer vision, pages 52–60, 2019.
  * [11] Tixiao Shan, Jinkun Wang, Brendan Englot, and Kevin Doherty.  Bayesian generalized kernel inference for terrain traversability mapping.  In Conference on Robot Learning, pages 829–838. PMLR, 2018.
  * [12] William R Vega-Brown, Marek Doniec, and Nicholas G Roy.  Nonparametric bayesian inference on multivariate exponential families.  Advances in Neural Information Processing Systems, 27, 2014.
  * [13] Lupeng Zhou, Jikai Wang, Shiqi Lin, and Zonghai Chen.  Terrain traversability mapping based on lidar and camera fusion.  In 2022 8th International Conference on Automation, Robotics and Applications (ICARA), pages 217–222, 2022.
  * [14] Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, and Fu Zhang.  Fast-lio2: Fast direct lidar-inertial odometry.  IEEE Transactions on Robotics, 38(4):2053–2073, 2022.
  * [15] Yixi Cai, Wei Xu, and Fu Zhang.  ikd-tree: An incremental kd tree for robotic applications.  arXiv preprint arXiv:2102.10808, 2021.
  * [16] Radu Bogdan Rusu and Steve Cousins.  3d is here: Point cloud library (pcl).  In 2011 IEEE international conference on robotics and automation, pages 1–4. IEEE, 2011.
  * [17] Olga Sorkine-Hornung and Michael Rabinovich.  Least-squares rigid motion using svd.  Computing, 1(1):1–5, 2017.
  * [18] Marc Alexa, Johannes Behr, Daniel Cohen-Or, Shachar Fleishman, David Levin, and Claudio T. Silva.  Computing and rendering point set surfaces.  IEEE Transactions on visualization and computer graphics, 9(1):3–15, 2003.
  * [19] Péter Fankhauser and Marco Hutter.  A universal grid map library: Implementation and use case for rough terrain navigation.  Robot Operating System (ROS) The Complete Reference (Volume 1), pages 99–120, 2016.