# Fast and Robust Lidar-Inertial Odometry by Tightly-Coupled Iterated Kalman
Smoother and Robocentric Voxels

Jun Liu∗, Yunzhou Zhang, Xiaoyu Zhao∗, Zhengnan He1 1Jun Liu, Yunzhou Zhang,
Xiaoyu Zhao, Zhengnan He are with College of Information Science and
Engineering, Northeastern University, Shenyang 110819, China (Email:
zhangyunzhou@mail.neu.edu.cn).∗These authors contributed equally to this work
and should be considered co-first authors.

###### Abstract

This paper presents a fast lidar-inertial odometry (LIO) that is robust to
aggressive motion. To achieve robust tracking in aggressive motion scenes, we
exploit the continuous scanning property of lidar to adaptively divide the
full scan into multiple partial scans (named sub-frames) according to the
motion intensity. And to avoid the degradation of sub-frames resulting from
insufficient constraints, we propose a robust state estimation method based on
a tightly-coupled iterated error state Kalman smoother (ESKS) framework.
Furthermore, we propose a robocentric voxel map (RC-Vox) to improve the
system’s efficiency. The RC-Vox allows efficient maintenance of map points and
k nearest neighbor (k-NN) queries by mapping local map points into a fixed-
size, two-layer 3D array structure. Extensive experiments are conducted on 27
sequences from 4 public datasets and our own dataset. The results show that
our system can achieve stable tracking in aggressive motion scenes (angular
velocity up to 21.8 rad/s) that cannot be handled by other state-of-the-art
methods, while our system can achieve competitive performance with these
methods in general scenes. Furthermore, thanks to the RC-Vox, our system is
much faster than the most efficient LIO system currently published.

##  I INTRODUCTION

Figure 1: The aggressive motion test on our own dataset (compared with FAST-
LIO2) and the speed test on the M2DGR dataset (compared with FAST-LIO2 and
Faster-LIO). (a)-(c) and (d)-(f) are the point cloud reconstructed by our
system and FAST-LIO2 respectively on _indoor_01-03_ of our own dataset. (g) is
the system speed test on _street_1-10_ of the M2DGR dataset, where _(paral)_
means that the system is parallelized, and _(single)_ means that the system is
running in single thread mode.

Simultaneous localization and mapping (SLAM) is a crucial technique for
autonomous mobile robots to navigate in an unknown environment. SLAM can be
divided into vision-based SLAM and lidar-based SLAM, among which lidar-based
SLAM is widely used because it can provide fast, accurate, and dense 3D
reconstruction. In lidar-based SLAM, the odometry module significantly impacts
the system’s overall performance, so this paper only focuses on the odometry
part.

The performance of the LO/LIO system is mainly determined by accuracy,
robustness, and efficiency. In terms of accuracy, some approaches improve
registration accuracy through more accurate probabilistic map representation
[1], more refined lidar points segmentation [2][3], or selecting more useful
lidar points for registration [4]. In addition, some approaches improve the
accuracy by reducing or avoiding motion distortion of the lidar points [5][6].

As for the robustness, it mainly focuses on the ability of the system to deal
with various corner cases, such as scene degradation, aggressive motion, and
so on. In degraded scenes, only using lidar cannot solve this problem well.
Some approaches [7, 8, 9, 10] achieve stable tracking in lidar-degraded scenes
by fusing vision. In aggressive motion scenes, the limitation of the IMU
frequency prevents it from capturing the full motion information, resulting in
inaccurate motion compensation of lidar points and poor initial pose of
registration. Although some approaches can cope with a certain degree of
aggressive motion [6][11], there is not yet a reliable solution to this
problem. In this paper, we reduce the IMU integration error by adaptively
dividing the full lidar scan into multiple sub-frames according to the motion
intensity. To deal with the possible insufficient constraints of the sub-
frames, we propose a tightly-coupled iterated ESKS framework for state
estimation, which smoothes the degraded sub-frame pose by using both the
historical and future constraints. Using the above approach, we achieve robust
tracking in aggressive motion scenes.

For the system efficiency, the chosen spatial data structure, the state
estimator formation, and the residual metric for matching affect the
computation time cost significantly [12]. Among these factors, the spatial
data structure is the most time-consuming module of the system. Hence, a
better spatial data structure is crucial to keep the system lightweight and
efficient. Compared with the tree structure, the hash voxel’s insertion,
deletion, and query operation have lower time complexity. However, due to the
complex calculations in the hash function and the inevitable hash collisions,
the hash table does not really achieve the same efficient access as the array
structure. Although the array structure has a fast access performance, its
rapidity is achieved through a large memory consumption, which makes it rarely
used for the large-scale point cloud representation. In this paper, we make it
possible to use the array structure in the LIO system by designing a fixed-
size two-layer 3D array structure, local map points within the lidar range are
maintained by this fixed-size structure with a modulo operation. In addition,
a method for refining the k-NN process is proposed, which further improves the
system’s efficiency.

In summary, the contributions of this paper are as follows:

  * •

We make the LIO system robust to aggressive motion via an adaptive lidar
points division. In addition, we propose an iterated ESKS-based state
estimation framework to avoid the degradation of the sub-frame and guarantee
the smoothness of the trajectory.

  * •

We propose a robocentric voxel map structure, called RC-Vox, which enables
fast incremental mapping through a fixed-size, two-layer 3D array structure.
Moreover, a refined k-NN search strategy is proposed to accelerate the point
cloud registration.

  * •

We verify our system on 4 public datasets and our own dataset. Experiments
show that our system achieves competitive performance against the state-of-
the-art LIO system in general scenes and is robust to aggressive motion.
Moreover, the single-thread version of our system is even faster than the
parallel accelerated version of the most efficient LIO system.

##  II RELATED WORK

In this section, we review the state-of-the-art LIO systems and the spatial
data structures commonly employed in LIO systems.

###  II-A Lidar-Inertial Odometry

In recent years, some outstanding LIO systems have emerged, we only discuss
the parts that are relatively robust to aggressive motion. FAST-LIO2 [13]
downsamples the original lidar points and performs scan-to-map registration
directly in an iterated error state Kalman filter framework. This framework
follows the new Kalman gain calculation method proposed in FAST-LIO [14] to
speed up the system. Although FAST-LIO2 is assisted by an IMU, it does not
handle the overly aggressive motion. The continuous-time method like CLINS [6]
is able to ignore the motion compensation error caused by the aggressive
motion through the non-rigid registration strategy. However, this requires
more control points to fit the violently varying trajectory, which greatly
affects the system’s efficiency. LLOL [11] treats spinning lidar as a
streaming sensor by dividing the lidar points. The problem of insufficient
constraints on partial scans is avoided by using a circular buffer to ensure a
full scan of measurements are used for registration. Because it needs to
estimate the pose by the amount of data in the complete frame when processing
each partial scan, the system becomes less efficient as the number of partial
scans increases. Therefore, it is also unable to handle larger degrees of
aggressive motion.

###  II-B Spatial Data Structure

Many approaches [15, 16, 14, 6, 17] use the kd-tree for map maintenance, which
can achieve fast k-NN search. Nevertheless, the whole k-d tree has to be
reconstructed when new points are added or old points are deleted, which
greatly reduces the efficiency of the system. The ikd-Tree structure proposed
by FAST-LIO2 [13] greatly reduces the time consumption of map update compared
with the static k-d tree structure. However, partial reconstruction is still
required to achieve balance when adding or deleting points that lead to an
imbalance in the tree. Compared with the k-d tree structure, the hash voxel is
more efficient for map maintenance and has a faster query speed. The iVox
structure proposed in Faster-LIO [12] organizes voxels through a hash table
and a LRU cache, whose parallel accelerated version can achieve a performance
that completely surpasses the performance of the ikd-Tree. However, due to the
existence of collisions, the hash structure are not as fast to access as the
array structure, which leaves the potential for the voxel map to be further
improved.

##  III System Overview

The pipeline of the system is shown in Fig. 2. The lidar scan is fed into the
system together with the IMU measurements. By detecting the sensor motion
intensity, the sub-frame generation module first determines how many sub-
frames the full lidar scan will be divided into and then generate multiple
sub-frames (see IV-A). Then, we performs forward propagation of the IMU
measurements and motion compensation (see IV-B), after which we uses the
undistorted lidar points to perform state update with a direct method (see
IV-C). Backward smooth is then performed using sub-frames corresponding to at
least three full scans (see IV-D). Finally, the smoothed poses are sent to the
constraint integrity detection module to check if the constraints are
sufficient (see IV-E), and the pose with no degradation is used to register
the lidar points to the RC-Vox (see V).

##  IV State Estimation

###  IV-A Sub-frame Generation

The system adaptively determines the number of sub-frames to be generated
based on the motion intensity. Although generating more sub-frames can further
reduce the impact of IMU integration error on motion compensation
theoretically, the presence of dynamic objects in the environment and
cluttered points will greatly reduce the system stability if only a few number
of points are used for registration. Therefore, it only makes sense to divide
full scan into more sub-frames in aggressive motion scenes. Based on the above
reasons, we count the standard deviation of acceleration and angular velocity
for different motion intensities and simply assume that the number of sub-
frames is proportional to the standard deviation value. Assume that
$\sigma_{acc\\_max}$, $\sigma_{gyr\\_max}$ are the maximum standard deviations
of acceleration and angular velocity in all directions derived from statistics
respectively, $\sigma_{acc\\_curr}$, $\sigma_{gyr\\_curr}$ are the standard
deviations of acceleration and angular velocity corresponding to the current
scan, $n_{max}$ is the maximum number of sub-frames allowed to be divided from
a full lidar scan, $n_{max}$ depends on the total number of points in each
frame, and we use different $n_{max}$ for lidar sensors from different
manufacturers. then the number of sub-frames to be divided $n_{curr}$ is:

$n_{curr}=\mathrm{ceil}\left(n_{max}*max\left(\frac{\sigma_{acc\\_curr}}{\sigma_{acc\\_max}},\frac{\sigma_{gyr\\_curr}}{\sigma_{gyr\\_max}}\right)\right)$
(1)

where $\mathrm{ceil}()$ stands for rounding up.

###  IV-B Propagation

The system nominal state $\bm{x}$, error state $\bm{\delta x}$, IMU input
$\bm{u}$ and noise $\bm{w}$ are defined as follows:

$\displaystyle\mathcal{M}\triangleq\mathbb{R}^{6}\times
SO(3)\times\mathbb{R}^{9}$ (2)
$\displaystyle\bm{x}\triangleq\begin{bmatrix}{}^{G}\bm{p}_{b}^{T}&{}^{G}\bm{v}_{b}^{T}&{}^{G}\bm{q}_{b}^{T}&\bm{b}_{a}^{T}&\bm{b}_{g}^{T}&{}^{G}\bm{g}^{T}\end{bmatrix}^{T}\in\mathcal{M}$
$\displaystyle\delta\bm{x}\triangleq\begin{bmatrix}{}^{G}\delta\bm{p}_{b}^{T}&{}^{G}\delta\bm{v}_{b}^{T}&{}^{G}\delta\bm{\theta}_{b}^{T}&\delta\bm{b}_{a}^{T}&\delta\bm{b}_{g}^{T}&{}^{G}\delta\bm{g}^{T}\end{bmatrix}^{T}\in\mathbb{R}^{18}$
$\displaystyle\bm{u}\triangleq\begin{bmatrix}\bm{a}_{m}^{T}&\bm{\omega}_{m}^{T}\end{bmatrix}^{T}$
$\displaystyle\bm{w}\triangleq\begin{bmatrix}\bm{n}_{a}^{T}&\bm{n}_{\omega}^{T}&\bm{n}_{ba}^{T}&\bm{n}_{b\omega}^{T}\end{bmatrix}^{T}$

where ${}^{G}\bm{p}_{b}$, ${}^{G}\bm{v}_{b}$, ${}^{G}\bm{q}_{b}$ are the IMU
position, velocity, attitude in the global frame, $\bm{b}_{a}$, $\bm{b}_{g}$
are the IMU biases and ${}^{G}\bm{g}$ is the gravity in the global frame.
$\bm{a}_{m}$, $\bm{\omega}_{m}$ are IMU measurements. $\bm{n}_{a}$,
$\bm{n}_{\omega}$ are the measurement noise of $\bm{a}_{m}$, $\bm{\omega}_{m}$
and $\bm{n}_{ba}$, $\bm{n}_{b\omega}$ are the random walk noise of IMU biases.
Using the IMU measurements as input, the nominal state values are predicted
using a discrete-time propagation model:

$\bm{x}_{k+1}=f(\bm{x}_{k},\bm{u}_{k})$ (3)

with a linearized discrete-time error state transfer equation (4), the
covariance matrix of the error state is forward propagated as equation (5).

$\delta\bm{x}_{k+1}=(I+\bm{F}_{k}\Delta t)\delta\bm{x}_{k}+(\bm{C}_{k}\Delta
t)\bm{w}$ (4) $\bm{P}_{k+1}=(I+\bm{F}_{k}\Delta
t)\bm{P}_{k}(I+\bm{F}_{k}\Delta t)^{T}+(\bm{C}_{k}\Delta
t)\bm{Q}_{k}(\bm{C}_{k}\Delta t)^{T}$ (5)

For the detailed forms of $f(\bm{x}_{k},\bm{u}_{k})$, $\bm{F}_{k}$,
$\bm{C}_{k}$ and $\bm{Q}_{k}$, readers can refer to [18]. In addition, the
result of the forward propagation are used for motion compensation and
registration.

Figure 2: System overview.

###  IV-C Update

We follow the direct method of FAST-LIO2 [13] by downsampling the sub-frame
lidar points and searching the 5 nearest neighbors for each point (see V-C),
then constructing point-to-plane residuals after fitting the local plane
patches:

$h_{j}\left(\bm{x}_{k}^{\kappa}\right)=^{G}\bm{n}_{j}^{T}\left({}^{G}\bm{q}_{b_{k}}^{\kappa}\left({}^{b}q_{l}{{}^{l}\bm{p}_{j}}+^{b}\bm{p}_{l}\right)+^{G}\bm{p}_{b_{k}}^{\kappa}-^{G}\bm{c}_{j}\right)$
(6)

where $h_{j}\left(\bm{x}_{k}^{\kappa}\right)$ is the j-th row of
$h\left(\bm{x}_{k}^{\kappa}\right)$, ${}^{b}\bm{q}_{l}$ and ${}^{b}\bm{p}_{l}$
are the pre-calibrated extrinsic between lidar and IMU, ${}^{G}\bm{n}_{j}$ and
${}^{G}\bm{c}_{j}$ are normal vector and centroid of the j-th plane patch
respectively, ${}^{l}\bm{p}_{j}$ is the corresponding j-th lidar point,
$\kappa$ means the $\kappa$-th iteration.

Using the point-to-plane residuals as measurements, data association and error
state update operations are performed iteratively [13]:

$\bm{K}=(\bm{H}^{T}\bm{R}^{-1}\bm{H}+\bm{P}_{k}^{-1})^{-1}\bm{H}^{T}\bm{R}^{-1}$
(7)
$\bm{x}_{k}^{\kappa+1}=\bm{x}_{k}^{\kappa}+\bm{K}\left(\bm{0}-h(\bm{x}_{k}^{\kappa})\right)-\left(\bm{I}-\bm{K}\bm{H}\right)\left(\bm{x}_{k}^{\kappa}-\bm{x}_{k}^{0}\right)$
(8)

During the above iterations, we have simultaneously performed correction of
the nominal state and reset of the error state. When the iterations converge,
the state covariance is updated with the following equation:

$\bm{P}_{k}=\left(\bm{I}-\bm{K}\bm{H}\right)\bm{P}_{k}$ (9)

###  IV-D Backward Smooth

Since the original lidar scan is divided into sub-frames, which may be
degraded resulting from insufficient constraints. Once the system has finished
processing all the sub-frames corresponding to the current scan, we perform a
backward smooth on the currently existing sub-frames in the system:

$\bm{x}_{k+1}^{-}=f(\bm{x}_{k},\bm{u}_{k})$ (10)
$\begin{split}\bm{P}_{k+1}^{-}=\left(\bm{I}+\bm{F}_{k}\Delta
t\right)\bm{P}_{k}\left(\bm{I}+\bm{F}_{k}\Delta t\right)^{T}\\\
+\left(\bm{C}_{k}\Delta t\right)\bm{Q}_{k}\left(\bm{C}_{k}\Delta
t\right)^{T}\end{split}$ (11)
$\bm{G}_{k}=\bm{P}_{k}\left(\bm{I}+\bm{F}_{k}\Delta
t\right)^{T}\left[\bm{P}_{k+1}^{-}\right]^{-1}$ (12)
$\bm{x}_{k}^{s}=\bm{x}_{k}+\bm{G}_{k}\left[\bm{x}_{k+1}^{s}-\bm{x}_{k+1}^{-}\right]$
(13)
$\bm{P}_{k}^{s}=\bm{P}_{k}+\bm{G}_{k}\left[\bm{P}_{k+1}^{s}-\bm{P}_{k+1}^{-}\right]\bm{G}_{k}^{T}$
(14)

where $f(\bm{x}_{k},\bm{u}_{k})$, $\bm{F}_{k}$, $\bm{C}_{k}$ and $\bm{Q}_{k}$
are the same as in propagation, $\bm{x}_{k}^{s}$ and $\bm{P}_{k}^{s}$ are the
results of the backward smooth which is started from the last sub-frame at
time $T$, with $\bm{x}_{T}^{s}=\bm{x}_{T}$ and $\bm{P}_{T}^{s}=\bm{P}_{T}$. A
detailed introduction of the Kalman smoother can be found in [19]. In order to
apply Kalman smoother to the error state, we make some modifications to the
backward smooth part (as shown in equations (10)-(14)), the validity of these
equations is explained below.

The error state Kalman filter is equivalent to using the extended Kalman
filter to manipulate the error state, then using the error state to correct
the nominal state and reset the error state after each update. However, the
reset operation makes it unable to apply the extended Kalman smoother (EKS)
directly. To operate the error state in EKS, we temporarily cancel the reset
operation on the error state and perform forward propagation, update and
backward smooth on the unreseted error state. Meanwhile, we calculated
$\bm{F}_{k}$ with the sum of the unreseted error state and the uncorrected
nominal state. Under the assumption that the error state always varies around
0, EKS can be applied to the error state. Assume that $\delta\bm{x}_{k}^{p}$,
$\delta\bm{x}_{k}^{u}$, $\delta\bm{x}_{k}^{s}$ are the error states after
forward propagation, update and backward smooth at time k respectively,
$\delta\bm{x}_{k}^{\Delta u}$ and $\delta\bm{x}_{k}^{\Delta s}$ are the
corrected parts of update and backward smooth, $\bm{x}_{k}$ is the uncorrected
nominal state, the backward smooth part of the EKS is slightly different from
equations (10)-(14), where equations (10) and (13) are replaced with equations
(15) and (16).

$\delta\bm{x}_{k+1}^{p}=\left(1+F_{k}\Delta t\right)\delta\bm{x}_{k}^{u}$ (15)
$\delta\bm{x}_{k}^{s}=\delta\bm{x}_{k}^{u}+\bm{G}_{k}\left[\delta\bm{x}_{k+1}^{s}-\delta\bm{x}_{k+1}^{p}\right]$
(16)

Then, suppose the following equation holds approximately:

$\displaystyle\bm{x}_{k+1}^{s}$
$\displaystyle=\bm{x}_{k+1}+\delta\bm{x}_{k+1}^{s}$ (17)
$\displaystyle=\bm{x}_{k+1}+\delta\bm{x}_{k+1}^{u}+\delta\bm{x}_{k+1}^{\Delta
s}$
$\displaystyle=\bm{x}_{k+1}+\delta\bm{x}_{k+1}^{p}+\delta\bm{x}_{k+1}^{\Delta
u}+\delta\bm{x}_{k+1}^{\Delta s}$
$\displaystyle=f\left(\bm{x}_{k},\bm{u}_{k}\right)+\left(1+F_{k}\Delta
t\right)\delta\bm{x}_{k}^{u}+\delta\bm{x}_{k+1}^{\Delta
u}+\delta\bm{x}_{k+1}^{\Delta s}$ $\displaystyle\approx
f\left(\bm{x}_{k}+\delta\bm{x}_{k}^{u},\bm{u}_{k}\right)+\delta\bm{x}_{k+1}^{\Delta
u}+\delta\bm{x}_{k+1}^{\Delta s}$

we can obtain the following relationship:

$\displaystyle\delta\bm{x}_{k+1}^{s}-\delta\bm{x}_{k+1}^{p}$
$\displaystyle=\left(\bm{x}_{k+1}^{s}-\bm{x}_{k+1}\right)-\delta\bm{x}_{k+1}^{p}$
(18)
$\displaystyle=\bm{x}_{k+1}^{s}-\left(\bm{x}_{k+1}+\delta\bm{x}_{k+1}^{p}\right)$
$\displaystyle\approx\bm{x}_{k+1}^{s}-f\left(\bm{x}_{k}+\delta\bm{x}_{k}^{u},\bm{u}_{k}\right)$

where $\bm{x}_{k}+\delta\bm{x}_{k}^{u}$ is the corrected nominal state. Then,
by replacing $\delta\bm{x}_{k+1}^{s}-\delta\bm{x}_{k+1}^{p}$ in equation (16)
with
$\bm{x}_{k+1}^{s}-f\left(\bm{x}_{k}+\delta\bm{x}_{k}^{u},\bm{u}_{k}\right)$
and adding uncorrected $\bm{x}_{k}$ to both sides, we can obtain equation
(13). Then, the reset of the error state and the correction of the nominal
state will not affect the use of Kalman smoother anymore.

###  IV-E Constraint Integrity Detection

We use the covariance matrix of the system state to check the constraint
integrity of the sub-frame. For each sub-frame currently being processed, we
check it in time-order from old to new. If the minimum eigenvalue of the
covariance matrix is greater than a specified threshold, the constraint is
considered to be sufficient. Finally, the sub-frames that are no longer
degraded are sent to the RC-Vox for incremental mapping.

##  V RC-Vox: Robocentric Voxel Map

In this section, we introduce the proposed RC-Vox, a robocentric voxel map
structure. Unlike iVox [12] which uses a combination of hash table and LRU
cache, RC-Vox implements efficient k-NN search and incremental mapping
directly in a fixed-size, two-layer 3D array structure. We start by explaining
some terms for a better introduction to RC-Vox:

  * •

Global coordinate system is the one used in general LIO system, sometimes it
is aligned with the pose of the first scan.

  * •

RC-Vox maintains the local map points in a cube centered on the robot and with
twice the lidar range as edge length, which is sufficient for stable lidar
points registration. In addition, the local map points are described under the
global coordinate system.

  * •

Top-level array (TLA) and bottom-level array (BLA) together form a two-layer
3D array structure. Each element in the TLA is called a grid, the internal
space of each grid is filled by a BLA, and each element in a BLA is called a
voxel.

  * •

A TLA index corresponds to a grid in the TLA.

  * •

A BLA index corresponds to a voxel in a BLA.

Moreover, we explain some notations that will be used below. $\bm{r}$,
$\bm{m}$, $\bm{t}$, and $\bm{b}$ are the global coordinate of the robot, local
map origin, TLA origin, and BLA origin respectively. $\bm{\mathrm{IT}}$ and
$\bm{\mathrm{IB}}$ are the TLA index and the BLA index respectively. In
addition, all of the above are three-dimensional real or integer vectors.

###  V-A Data Structure of RC-Vox

The local map points are firstly organized by TLA. For memory consumption
consideration, the resolution of the TLA cannot be too high, usually at the
meter level. To prevent the excessive points present in each voxel, for a grid
with map points, a BLA is generated internally to achieve higher resolution
requirements. Assume that the lidar range is $l$, the grid size is $g$ and the
voxel size is $v$, the TLA can express a range of $2\lambda l\times 2\lambda
l\times 2\lambda l$ space $\left(\lambda>1\right)$, the number of grids in
each dimension of the TLA is $2\lambda l/g$, and the number of voxels in each
dimension of a BLA is $g/v$.

###  V-B Incremental Mapping

In the system initialization phase, we initialize the voxel map by placing the
robot at the center of the TLA, while making the local map coincide with the
TLA (as shown in Fig. 3). Assuming that the global coordinate of the robot is
$\bm{r}_{init}$, the global coordinate of the TLA origin $\bm{t}_{init}$ is:

$\bm{t}_{init}=\bm{r}_{init}-\lambda l\bm{I}_{3\times 1}$ (19)

The local map origin $\bm{m}_{init}$ has the same global coordinate as the TLA
origin at this moment and the TLA index of the local map origin
$\bm{\mathrm{IT}}_{m_{init}}$ is $\bm{0}_{3\times 1}$. It should be noted that
the position of the TLA in the global coordinate system is fixed for the whole
subsequent mapping period, and a modulo operation is used to map the
continually moving local map to the TLA.

With the robot moving, suppose that the current global coordinate of the robot
at a certain moment is $\bm{r}_{curr}$, since the local map moves with the
robot and is aligned to grids, the global coordinate of the local map origin
$\bm{m}_{curr}$ at this moment is:

$\bm{m}_{curr}=\mathrm{floor}\left(\left(\bm{r}_{curr}-\bm{t}_{init}\right)/g\right)g+\bm{t}_{init}$
(20)

where $\mathrm{floor}()$ stands for rounding down. Meanwhile, the TLA index of
the local map origin $\bm{\mathrm{IT}}_{m_{curr}}$ is:

$\bm{\mathrm{IT}}_{m_{curr}}=\mathrm{floor}\left(\left(\bm{r}_{curr}-\bm{t}_{init}\right)/g\right)\%\left(2\lambda
l/g\right)$ (21)

For each local map point $\bm{p}$, the TLA index $\bm{\mathrm{IT}}_{p}$ is:

$\bm{\mathrm{IT}}_{p}=\left(\mathrm{floor}\left(\left(\bm{p}-\bm{m}_{curr}\right)/g\right)+\bm{\mathrm{IT}}_{m_{curr}}\right)\%\left(2\lambda
l/g\right)$ (22)

It can be seen that the local map is remapped into the TLA by a modulo
operation. When we register lidar points to the map, we also use the equation
(22) to find the TLA index corresponding to the current point. If the grid
corresponding to this TLA index is not yet used, a BLA is generated in this
grid, and the global coordinate of this BLA origin is recorded, where the
global coordinate of the origin $\bm{b}_{ori}$ is:

$\bm{b}_{ori}=\mathrm{floor}\left(\left(\bm{p}-\bm{t}_{init}\right)/g\right)g+\bm{t}_{init}$
(23)

we proceed to calculate its BLA index, and record this point into the
corresponding voxel, where the BLA index $\bm{\mathrm{IB}}_{p}$ is:

$\bm{\mathrm{IB}}_{p}=\mathrm{floor}\left(\left(\bm{p}-\bm{b}_{ori}\right)/v)\right)$
(24)

As shown in Fig. 3, the TLA is set to the same size as the local map at the
initialization phase, therefore, the remapping operation will not cause
problem with overlapping of the local map points in the TLA. In addition, we
only maintain the local map points in a cube, so we can likewise calculate the
TLA index of the cube boundary. When a robot position change leads to the cube
moving in the TLA, a portion of the grids will move out of the cube while
these grids will re-enter the cube on the other side. At the same time, the
contents of these grids are reset, enabling the incremental construction and
deletion of the map points.

Figure 3: 2D demonstration of the RC-Vox. The left figure shows the
initialization phase, and the right figure shows the process of remapping the
local map parts beyond the TLA back to the TLA after the robot moves.

###  V-C K-NN Search

When conducting lidar points registration, we query the voxels as the approach
in equations (22), (24). In order to perform an accurate k-NN search, not only
the voxel corresponding to the current point but also the nearby voxels need
to be searched. Since multiple rounds of k-NN search are performed during the
lidar points registration, a significant portion of the time consumption of
the LIO system is spent on the k-NN search. RC-Vox uses iVox’s [12] k-NN
search strategy with some improvements by moving the process of searching
nearby points from the registration phase to the mapping phase. When
registering a lidar point to map, in addition to filling the corresponding
voxel it belongs to, this point is also recorded in the nearby 6(/18/26)
voxels. Since the mapping is incremental, only a small number of points need
to be added to the map in each round, so the increase in time consumption due
to this operation is nearly negligible. When conducting a k-NN search, we can
search only the corresponding voxel and get all the points in its neighbor,
then calculate the distance from the current point to each of these points and
select the k nearest points with the smallest distance as the result of the
k-NN search.

##  VI Experiment

To verify the effectiveness of the proposed method, we select 27 sequences
from our own dataset and 4 public datasets (NTU VIRAL [20], M2DGR [21],
UrbanNav [22], Newer College [23, 24]) for comparison with FAST-LIO2[13]
Faster-LIO[12] and Point-LIO[he2023point]. In addition, all the experiments
are performed on a desktop computer equipped with an AMD Ryzen7 4800H CPU (8
cores, 2.9 G Hz) and 16G memory. In the following, we first briefly introduce
the datasets we used. Subsequently, we verify the system’s ability to handle
aggressive motion on the Newer College dataset and our own dataset, and then
conduct an ablation experiment to illustrate why our system tracks robustly
under aggressive motion scene. In addition, We evaluate the accuracy and
efficiency of the whole system on the NTU VIRAL, M2DGR, and UrbanNav datasets.
Finally, we use the UrbanNav dataset again to further illustrate the high
efficiency of the RC-Vox.

TABLE I: Dataset Information

Dataset | Sequence ID | Sequence Infomation
---|---|---
Length (m) | Duratioin (s)
NTU VIRAL | eee_01-03 | 535 | 900
nya_01-03 | 724 | 1235
sbs_01-03 | 583 | 1116
M2DGR | street_01-10 | 7723 | 7809
UrbanNav | Mongkok | 4860 | 3367
TST | 3640 | 785
Whampoa | 4510 | 1536
Newer College |  | 06_dynamic_
---
spinning
97 | 119
| collection_1-
---
quad-hard
234 | 187
Ours | indoor_01-03 | 259 | 308
Total | 23165 | 17362

  * *

Angular velocities up to 3.3 rad/s in the Newer College dataset and 21.8 rad/s
in our own dataset.

###  VI-A Dataset

All these 4 public datasets are multi-sensor datasets, we only introduce the
lidar and IMU among them. The NTU VIRAL dataset is collected with an unmanned
aerial vehicle which has two 10 Hz Ouster OS1-16 Gen1 lidars (we only use the
horizontal lidar) and a 385 Hz VectorNav VN100 IMU. The M2DGR dataset is
collected with a ground robot which has a 10 Hz Velodyne VLP-32C lidar and a
150 Hz Handsfree A9 IMU. The UrbanNav dataset is collected with a human-
driving robocar which has three 10 Hz lidars (we only used the Velodyne
HDL-32E lidar among them) and a 400 Hz Xsens Mti 10 IMU. The Newer College
dataset has two parts. The first part uses an Ouster OS1-64 Gen 1 lidar with a
built-in 100Hz ICM-20948 IMU. The second part uses an Ouster OS0-128 lidar
with a built-in 100Hz ICM-20948 IMU. All of these two parts are collected by
handheld. And our own dataset is also collected by handheld with a Livox lidar
Avia. The details of all sequences we used are listed in Table I.

###  VI-B Aggressive Motion Test

To verify the system’s effectiveness in aggressive motion scence, qualitative
tests are conducted on our own dataset (_indoor_01-03_) and two aggressive
motion sequences of the Newer College dataset (_06_dynamic_spinning_ and
_collection_1-quad-hard_). Meanwhile, we compare the result with FAST-LIO2. As
shown in Fig. 1 and Fig. 4, point cloud reconstructed by our system is clearer
and shaper in the Newer College dataset. For our own dataset which has a more
intense motion, FAST-LIO2 can no longer cope with it and fails to track, while
our method still guarantees a high map accuracy. To further explore the reason
why our system can achieve better results, we conduct an ablation experiment
on _indoor_01_ sequence. In the experiment, we turn off the backward smooth
function and the sub-frame generation function (i.e. without using the lidar
points division) respectively. As shown in Fig. 5, the lidar points division
effectively reduces the accumulated error during IMU integration, thus
ensuring stable tracking. In addition, we can notice that the overall mapping
result still keeps a certain level of precision after turning off backward
smooth, which is because partial constraint information still exists in the
sub-frame and accurate alignment is still possible in the constraint
direction. Therefore, the overall mapping performance is almost unaffected
when the pose estimation error is tolerable in the degraded direction.
However, as shown by the map details and the trajectory, the degraded
direction causes ghosting in the map due to incorrect pose estimation and
terrible track smoothness.

Figure 4: Point cloud reconstructed by our system and FAST-LIO2 on two
aggressive motion sequences of the Newer College dataset. (a), (b) are
reconstructed by our system on _06_dynamic_spinning_ and (c) is reconstructed
by our system on _collection_1-quad-hard_. (d)-(f) are reconstructed by FAST-
LIO2 and correspond to (a)-(c). Figure 5: Ablation experiment on _indoor_01_
sequence. (a)-(c) are the point cloud reconstructed by the full system, the
system turning off backward smooth function and the system turning off sub-
frame generation function respectively. (d) and (e) are the map details of the
full system and the system turning off backward smooth function respectively.
(f) and (g) are the trajectory segments of the full system and the system
turning off backward smooth function respectively. TABLE II: Accuracy Test*

Sequence ID |  | FAST-LIO2
---
| Faster-LIO
---
| Point-LIO
---
| Ours
---
RMSE (m)
eee_01 | 0.135 | 0.144 | 0.195 | 0.132
eee_02 | 0.118 | 0.121 | 0.130 | 0.118
eee_03 | 0.164 | 0.162 | 0.182 | 0.164
nya_01 | 0.121 | 0.132 | 0.182 | 0.120
nya_02 | 0.140 | 0.172 | 0.164 | 0.139
nya_03 | 0.150 | 0.154 | 0.190 | 0.147
sbs_01 | 0.144 | 0.144 | 0.197 | 0.144
sbs_02 | 0.144 | 0.144 | 0.214 | 0.143
sbs_03 | 0.132 | 0.134 | 0.189 | 0.132
street_01 | 0.366 | 0.365 | 0.589 | 0.318
street_02 | 2.881 | 2.890 | 2.796 | 2.580
street_03 | 0.123 | 0.120 | 0.110 | 0.114
street_04 | 0.486 | 0.605 | 0.409 | 0.571
street_05 | 0.327 | 0.329 | 0.419 | 0.213
street_06 | 0.357 | 0.341 | 0.471 | 0.307
street_07 | 0.338 | 0.331 | 5.784 | 0.254
street_08 | 0.186 | 0.178 | 0.183 | 0.176
street_09 | 1.547 | 1.523 | 1.942 | 1.575
street_10 | 0.176 | 0.172 | 3.429 | 0.171
Mongkok | 0.638 | 0.655 | 0.470 | 0.657
TST | 0.401 | 0.403 | 0.410 | 0.380
Whampoa | 0.255 | 0.254 | 0.280 | 0.245

  * *

The NTU VIRAL and M2DGR datasets are evaluated using the ATE metric, while the
UrbanNav dataset is evaluated using the RTE metric (per 10 meters) because of
the longer trajectory and larger drift error.

TABLE III: Efficiency Test

Sequence ID |  | FAST-
---
LIO2
(Single)
| FAST-
---
LIO2
(Paral)
| Faster
---
-LIO
(Single)
| Faster
---
-LIO
(Paral)
| Ours*
---
(Single)
Time Cost Per Frame (ms)
eee_01 | 30.5 | 17.5 | 47.0 | 10.9 | 9.3
eee_02 | 27.7 | 17.3 | 41.1 | 11.1 | 8.9
eee_03 | 28.2 | 17.0 | 37.2 | 10.7 | 8.7
nya_01 | 23.7 | 15.7 | 45.7 | 11.5 | 7.1
nya_02 | 24.9 | 15.9 | 49.7 | 12.0 | 7.6
nya_03 | 23.1 | 15.7 | 50.4 | 12.1 | 7.4
sbs_01 | 23.5 | 15.8 | 35.8 | 10.1 | 7.9
sbs_02 | 23.9 | 15.8 | 40.8 | 10.9 | 8.1
sbs_03 | 24.0 | 16.1 | 41.1 | 10.5 | 7.6
street_01 | 45.0 | 25.5 | 68.5 | 16.5 | 13.2
street_02 | 39.4 | 23.2 | 53.9 | 14.8 | 12.2
street_03 | 52.0 | 28.5 | 92.7 | 19.4 | 14.1
street_04 | 42.4 | 25.4 | 58.1 | 15.5 | 12.7
street_05 | 36.7 | 22.8 | 51.6 | 14.7 | 11.8
street_06 | 39.8 | 24.5 | 54.8 | 15.5 | 12.5
street_07 | 45.5 | 26.8 | 69.6 | 17.0 | 12.7
street_08 | 49.8 | 29.0 | 89.3 | 19.8 | 13.8
street_09 | 44.1 | 26.5 | 64.2 | 16.7 | 13.0
street_10 | 39.3 | 25.3 | 57.1 | 16.1 | 11.2
Mongkok | 64.2 | 21.5 | 48.9 | 12.7 | 8.3
TST | 83.3 | 26.5 | 43.1 | 12.9 | 9.8
Whampoa | 73.3 | 23.8 | 42.4 | 12.7 | 8.6

  * *

For fairness, the time consumption of our system is not the processing time of
a single sub-frame, but the time to process the full scan (including sub-frame
generation, processing multiple sub-frames and mapping).

###  VI-C System Accuracy and Efficiency Test

We use a total of 22 sequences for system accuracy and efficiency evaluation.
In addition, we also test the performance of FAST-LIO2 and Faster-LIO with or
without parallelized acceleration in the efficiency test, the parallelized
acceleration version is represented by _(Paral)_ and the single-thread version
is represented by _(Single)_. In the following experiments, FAST-LIO2, Faster-
LIO, and our system all apply a 4-point skip operation and a 0.5m voxel filter
to the original point cloud.

Table II shows that our system achieves the same or higher accuracy compared
with other algorithms.Especially in _street_07_ sequence, the robot performs a
lot of rotations, and our system effectively improves the system accuracy
through lidar points division and the ESKS framework. However, in _street_04_
sequence and _Mongkok_ sequence, we can see that the accuracy of our system
and Faster-LIO are lower than FAST-LIO2, which is because that the k-d tree
based LIO method has a certain accuracy advantage over the voxel based method.

In terms of efficiency (as shown in Table III), we can see that the single-
thread version of FAST-LIO2 is slightly faster than Faster-LIO, but Faster-LIO
is more suitable for parallelized acceleration. Notably, the single-thread
version of our system is even faster than the parallelized version of Faster-
LIO. We also give the time consumption of each module of our system in the
UrbanNav dataset (as shown in Table IV).

TABLE IV: Time consumption for each module of our system Steps |  | Mongkok
---
(ms)
| TST
---
(ms)
| Whampoa
---
(ms)
Sub-frame generation | 1.04 | 1.11 | 1.02
| Propagation + Update
---
6.25 | 7.09 | 6.40
| Backward smooth +
---
Constraint integrity analysis
0.53 | 0.65 | 0.52
| Incremental mapping
---
\+ Map delete
0.47 | 1.02 | 0.64
Total runtime | 8.31 | 9.88 | 8.60
Figure 6: Speed test of ikd-Tree, iVox and RC-Vox

###  VI-D RC-Vox Speed Test

To further compare the efficiency of RC-Vox, ikd-Tree, and iVox, we modify
Faster-LIO with its map part to RC-Vox and ikd-Tree respectively. The time
consumption of each module is shown in Fig. 6. For the incremental mapping and
map delete phases, _ikd-Tree(Single)_ takes the longest time. Although _RC-
Vox(Single)_ needs to record points into nearby voxels, thanks to the array
structure which allows for fast access, it can achieve about the same time
consumption as _iVox(Paral)_. For the state estimation phase, _RC-Vox(Single)_
has the shortest time consumption due to the array structure and our
optimization of the k-NN search. Moreover, it can be seen that _RC-
Vox(Single)_ is even faster than _ikd-Tree(Paral)_ and _iVox(Paral)_. All the
above shows the superiority of the RC-Vox in terms of speed.

##  VII CONCLUSIONS

This paper proposes a fast and robust LIO system that can stably cope with
aggressive motion while ensuring tracking accuracy in general scenes. For
scenes with aggressive motion, we effectively reduce the IMU integration error
through an adaptive division of the lidar points. To cope with the possible
degradation of the sub-frames due to insufficient constraints, we propose a
tightly coupled iterated ESKS framework to conduct the pose estimation.
Regarding system efficiency, we propose a robocentric voxel map structure RC-
Vox, based on a fixed-size two-layer 3D array structure. Using a modulo
operation, RC-Vox enables efficient local map maintenance by mapping local map
points into this fixed-size array structure. In addition, we further optimize
the k-NN search by moving the search operation of nearby map points from the
state estimation phase to the mapping phase, thus further improving the
system’s efficiency.

## References

  * [1] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 3, pp. 8518–8525, 2022.
  * [2] Y. Pan, P. Xiao, Y. He, Z. Shao, and Z. Li, “Mulls: Versatile lidar slam via multi-metric linear least square,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 11 633–11 640.
  * [3] G. Chen, B. Wang, X. Wang, H. Deng, B. Wang, and S. Zhang, “Psf-lo: Parameterized semantic features based lidar odometry,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 5056–5062.
  * [4] J.-E. Deschaud, “Imls-slam: Scan-to-model matching based on 3d data,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 2480–2485.
  * [5] P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, “Ct-icp: Real-time elastic lidar odometry with loop closure,” in _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, 2022, pp. 5580–5586.
  * [6] J. Lv, K. Hu, J. Xu, Y. Liu, X. Ma, and X. Zuo, “Clins: Continuous-time trajectory estimation for lidar-inertial system,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2021, pp. 6657–6663.
  * [7] T. Shan, B. Englot, C. Ratti, and D. Rus, “Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping,” in _2021 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2021, pp. 5692–5698.
  * [8] J. Lin, C. Zheng, W. Xu, and F. Zhang, “R2live: A robust, real-time, lidar-inertial-visual tightly-coupled state estimator and mapping,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 7469–7476, 2021\.
  * [9] J. Lin and F. Zhang, “R3live: A robust, real-time, rgb-colored, lidar-inertial-visual tightly-coupled state estimation and mapping package,” in _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, 2022, pp. 10 672–10 678.
  * [10] C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “Fast-livo: Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry,” in _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2022, pp. 4003–4009.
  * [11] C. Qu, S. S. Shivakumar, W. Liu, and C. J. Taylor, “Llol: Low-latency odometry for spinning lidars,” in _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, 2022, pp. 4149–4155.
  * [12] C. Bai, T. Xiao, Y. Chen, H. Wang, F. Zhang, and X. Gao, “Faster-lio: Lightweight tightly coupled lidar-inertial odometry using parallel sparse incremental voxels,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4861–4868, 2022.
  * [13] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidar-inertial odometry,” _IEEE Transactions on Robotics_ , vol. 38, no. 4, pp. 2053–2073, 2022.
  * [14] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 3317–3324, 2021.
  * [15] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “Lins: A lidar-inertial state estimator for robust and efficient navigation,” in _2020 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2020, pp. 8899–8906.
  * [16] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in _2020 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. IEEE, 2020, pp. 5135–5142.
  * [17] K. Li, M. Li, and U. D. Hanebeck, “Towards high-performance solid-state-lidar-inertial odometry and mapping,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 5167–5174, 2021.
  * [18] J. Sola, “Quaternion kinematics for the error-state kalman filter,” _arXiv preprint arXiv:1711.02508_ , 2017.
  * [19] S. Särkkä, _Bayesian filtering and smoothing_. Cambridge university press, 2013, no. 3.
  * [20] T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, “Ntu viral: A visual-inertial-ranging-lidar dataset, from an aerial vehicle viewpoint,” _The International Journal of Robotics Research_ , vol. 41, no. 3, pp. 270–280, 2022.
  * [21] J. Yin, A. Li, T. Li, W. Yu, and D. Zou, “M2dgr: A multi-sensor and multi-scenario slam dataset for ground robots,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 2266–2273, 2021.
  * [22] L.-T. Hsu, N. Kubo, W. Wen, W. Chen, Z. Liu, T. Suzuki, and J. Meguro, “Urbannav: An open-sourced multisensory dataset for benchmarking positioning algorithms designed for urban areas,” in _Proceedings of the 34th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2021)_ , 2021, pp. 226–256.
  * [23] M. Ramezani, Y. Wang, M. Camurri, D. Wisth, M. Mattamala, and M. Fallon, “The newer college dataset: Handheld lidar, inertial and vision with ground truth,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2020, pp. 4353–4360.
  * [24] L. Zhang, M. Camurri, and M. Fallon, “Multi-camera lidar inertial extension to the newer college dataset,” _arXiv preprint arXiv:2112.08854_ , 2021.


$\\{\pi_{l},\pi_{r}\\}\in\operatorname{\mathbf{\Pi}}^{\prime}$, we have
$\operatorname{\mathcal{F}}_{tr}(\operatorname{\mathbf{\Pi}})=\operatorname{\mathcal{F}}_{tr}(\operatorname{\mathbf{\Pi}}^{\prime})$
if and only if for all $\\{x,x^{\prime}\\}\in\operatorname{\mathbf{X}}$,
$\pi^{\prime}\in\operatorname{\mathbf{\Pi}}^{\prime}$, and
$j\in\operatorname{\mathbf{J}}$ the following equations hold

$\displaystyle\Pr(x^{\prime},\pi^{\prime}|x,\pi_{l},j)$
$\displaystyle=\Pr(x^{\prime},\pi^{\prime}|x,\pi_{r},j)$
$\displaystyle\frac{\Pr(x,\pi_{l}|x^{\prime},\pi^{\prime},j)}{N(x,\pi_{l})}$
$\displaystyle=\frac{\Pr(x,\pi_{r}|x^{\prime},\pi^{\prime},j)}{N(x,\pi_{r})}$

###### Proof.

(a) Transition from the new sub-partitions

(b) Transition to the new sub-partitions

Figure 4:  The change in likelihood from transition-to and transition-from
perspective.

We proved in appendix A.1 that likelihood,
$\operatorname{\mathcal{L}}(\theta_{t})$, increases with any additional split.
We assumed a completely non-parametric form for the state transition part of
likelihood function. Therefore the state transition of likelihood function and
recursive partitioning objective function are the same, i.e.,
$\operatorname{\mathcal{L}}_{tr}(\operatorname{\mathbf{\Pi}})=\operatorname{\mathcal{F}}_{tr}(\operatorname{\mathbf{\Pi}})$.
Here we prove that the increment in likelihood, and consequently in
$\operatorname{\mathcal{F}}(\theta_{t})$, is equal to zero if and only if the
lemma’s equations hold. The split changes the likelihood by changing the
transition-to and average transition-from probabilities presented in figure 4.
We can calculate the changes in likelihood with respect to each of these
changes separately. First, according to (a) in figure 4 we have

$\displaystyle\operatorname{\mathcal{F}}(\operatorname{\mathbf{\Pi}}^{\prime})-\operatorname{\mathcal{F}}(\operatorname{\mathbf{\Pi}})=\sum_{x,x^{\prime}\in\operatorname{\mathbf{X}}}\sum_{\pi^{\prime}\in\operatorname{\mathbf{\Pi}}^{\prime}}\sum_{j\in\operatorname{\mathbf{J}}}N(x^{\prime},\pi^{\prime})\Bigg{(}$
$\displaystyle\frac{N(x^{\prime},\pi^{\prime},x,\pi_{r},j)}{N(x^{\prime},\pi^{\prime})}\log\frac{N(x^{\prime},\pi^{\prime},x,\pi_{r},j)}{N(x^{\prime},\pi^{\prime})N(x,\pi_{r},j)}$
$\displaystyle+$
$\displaystyle\frac{N(x^{\prime},\pi^{\prime},x,\pi_{l},j)}{N(x^{\prime},\pi^{\prime})}\log\frac{N(x^{\prime},\pi^{\prime},x,\pi_{l},j)}{N(x^{\prime},\pi^{\prime})N(x,\pi_{l},j)}$
$\displaystyle-$
$\displaystyle\frac{N(x^{\prime},\pi^{\prime},x,\pi_{p},j)}{N(x^{\prime},\pi^{\prime})}\log\frac{N(x^{\prime},\pi^{\prime},x,\pi_{p},j)}{N(x^{\prime},\pi^{\prime})N(x,\pi_{p},j)}\Bigg{)}$

According to log sum inequality the term in the parentheses is greater or
equal to zero. The left-hand side is equal to zero if and only if
$\frac{N(x^{\prime},\pi^{\prime},x,\pi_{l},j)}{N(x,\pi_{l},j)}=\frac{N(x^{\prime},\pi^{\prime},x,\pi_{r},j)}{N(x,\pi_{r},j)}=\frac{N(x^{\prime},\pi^{\prime},x,\pi_{p},j)}{N(x,\pi_{p},j)}$,
for every $\\{x,x^{\prime}\\}\in\operatorname{\mathbf{X}}$,
$j\in\operatorname{\mathbf{J}}$ and
$\pi^{\prime}\in\operatorname{\mathbf{\Pi}}^{\prime}$. This concludes the
first equation of the lemma.

We can conclude the second equation similarly with (b) in figure 4 as the
following

$\displaystyle\operatorname{\mathcal{F}}(\operatorname{\mathbf{\Pi}}^{\prime})-\operatorname{\mathcal{F}}(\operatorname{\mathbf{\Pi}})=\sum_{x,x^{\prime}\in\operatorname{\mathbf{X}}}\sum_{\pi^{\prime}\in\operatorname{\mathbf{\Pi}}^{\prime}}\sum_{j\in\operatorname{\mathbf{J}}}N(x^{\prime},\pi^{\prime},j)\Bigg{(}$
$\displaystyle\frac{N(x,\pi_{r},x^{\prime},\pi^{\prime},j)}{N(x^{\prime},\pi^{\prime},j)}\log\frac{N(x,\pi_{r},x^{\prime},\pi^{\prime},j)}{N(x,\pi_{r})N(x^{\prime},\pi^{\prime},j)}$
$\displaystyle+$
$\displaystyle\frac{N(x,\pi_{l},x^{\prime},\pi^{\prime},j)}{N(x^{\prime},\pi^{\prime},j)}\log\frac{N(x,\pi_{l},x^{\prime},\pi^{\prime},j)}{N(x,\pi_{l})N(x^{\prime},\pi^{\prime},j)}$
$\displaystyle-$
$\displaystyle\frac{N(x,\pi_{p},x^{\prime},\pi^{\prime},j)}{N(x^{\prime},\pi^{\prime},j)}\log\frac{N(x,\pi_{p},x^{\prime},\pi^{\prime},j)}{N(x,\pi_{p})N(x^{\prime},\pi^{\prime},j)}\Bigg{)}$

Again, according to log sum inequality the term in the parentheses is greater
or equal to zero. The left-hand side is equal to zero if and only if
$\frac{N(x^{\prime},\pi^{\prime},x,\pi_{l},j)}{N(x,\pi_{l})N(x^{\prime},\pi^{\prime},j)}=\frac{N(x^{\prime},\pi^{\prime},x,\pi_{r},j)}{N(x,\pi_{r})N(x^{\prime},\pi^{\prime},j)}=\frac{N(x^{\prime},\pi^{\prime},x,\pi_{p},j)}{N(x,\pi_{p})N(x^{\prime},\pi^{\prime},j)}$,
for every $\\{x,x^{\prime}\\}\in\operatorname{\mathbf{X}}$,
$j\in\operatorname{\mathbf{J}}$ and
$\pi^{\prime}\in\operatorname{\mathbf{\Pi}}^{\prime}$. This concludes the
second equation of the lemma. ∎

###### Proof for theorem 2.

Now we prove theorem 2 using lemmas 4 and 3. Assume that our algorithm stops
at iteration $r$. Given that the algorithm stops once no split increase the
$\operatorname{\mathcal{F}}$ function, for any additional split that generates
candidate partition $\operatorname{\mathbf{\Pi}}^{\prime}$, we have
$\operatorname{\mathcal{F}}(\operatorname{\mathbf{\Pi}}^{\prime})=\operatorname{\mathcal{F}}(\operatorname{\mathbf{\Pi}}_{r})$.
Therefore, according to lemmas 4 and 3 for any additional split to
$\operatorname{\mathbf{\Pi}}_{r}$ the following equalities hold for all
$\\{x,x^{\prime}\\}\in\operatorname{\mathbf{X}}$,
$\pi^{\prime}\in\operatorname{\mathbf{\Pi}}_{r}$ and
$j\in\operatorname{\mathbf{J}}$.

$\displaystyle\Pr(d|x,\pi_{l})$ $\displaystyle=\Pr(d|x,\pi_{r})$
$\displaystyle\Pr(x^{\prime},\pi^{\prime}|x,\pi_{l},j)$
$\displaystyle=\Pr(x^{\prime},\pi^{\prime}|x,\pi_{r},j)$
$\displaystyle\frac{\Pr(x,\pi_{l}|x^{\prime},\pi^{\prime},j)}{N(x,\pi_{l})}$
$\displaystyle=\frac{\Pr(x,\pi_{r}|x^{\prime},\pi^{\prime},j)}{N(x,\pi_{r})}$

which concludes that $\operatorname{\mathbf{\Pi}}_{r}$ is a perfect
discretization according to corollary 1. ∎

##  Appendix B Random discretization generator algorithm

This section explains the random discretization generator algorithm that we
used in the second simulation study. The intuition for this algorithm is very
similar to recursive partitioning – in each step, we randomly select one of
the partitions and split it into two partitions along one of the first 10
variables in $\operatorname{\mathbf{Q}}$. Please note that we only used the
first 10 variables in $\operatorname{\mathbf{Q}}$ for partitioning, and the
following 20 variables are added as irrelevant variables to show the
robustness of the algorithm to irrelevant variables. In addition, to prevent
very small or very large partitions, the random partition selection is
weighted by the size of the partition: big partitions are more likely to be
selected than smaller partitions. Formally, the random partitioning algorithm
is as the following.

  * •

Initialize $\operatorname{\mathbf{\Pi}}_{0}$ as one partition equal to the
full covariate space

  * •

Do the following for 15 rounds.

    * –

Randomly select a partition from $\operatorname{\mathbf{\Pi}}_{r}$ weighted by
$(\frac{1}{\text{partition's total splits}})^{2}$

    * –

Randomly select a variable from the first 10 variables

    * –

If possible, split the selected partition into two from the midpoint along the
selected variable. The total split for the resulting partitions is the total
split of their parents plus one. Repeat this round if the split is not
possible.

The total split for each partition is the total of times their parents have
selected to be split. If the total split for a partition is 5, it means that
it takes five splits from $\operatorname{\mathbf{Q}}$ to get to that
partition. Note that the total split of a partition is negatively correlated
with the size of the partition.

We also vary the replacement cost for each partition to add a decision
variation to the model that is not caused by state transition. The replacement
cost of a partition is calculated based on the range of its first 10 variables
as the following.

$\displaystyle
f_{dc}(\pi)=5-\frac{\sum_{i=1}^{10}(v_{i}^{min}(\pi)+v_{i}^{max}(\pi))}{10}$

where $v_{i}^{min}(\pi)$ and $v_{i}^{max}(\pi)$ are the minimum and maximum
range of $i^{th}$ variable in partition $\pi$. We choose this formulation for
replacement cost to create a good balance between decision variation caused by
state transition or replacement cost. Figure 5 depicts the distribution of
total split and replacement costs in the 1500 generated partitions across all
the 100 generated discretizations in the second simulation study.

Figure 5:  The histogram of generated partitions’ calculated replacement cost,
and number of splits in the 100 rounds of partitioning generation.