# HCTO: Optimality-Aware LiDAR Inertial Odometry with Hybrid Continuous Time
Optimization for Compact Wearable Mapping System

Jianping Li<EMAIL_ADDRESS>Shenghai Yuan<EMAIL_ADDRESS>Muqing
Cao<EMAIL_ADDRESS>Thien-Minh Nguyen<EMAIL_ADDRESS>Kun Cao
<EMAIL_ADDRESS>Lihua Xie<EMAIL_ADDRESS>School of Electrical and
Electronic Engineering, Nanyang Technological University, Singapore 639798

###### Abstract

Compact wearable mapping system (WMS) has gained significant attention due to
their convenience in various applications. Specifically, it provides an
efficient way to collect prior maps for 3D structure inspection and robot-
based “last-mile delivery” in complex environments. However, vibrations in
human motion and the uneven distribution of point cloud features in complex
environments often lead to rapid drift, which is a prevalent issue when
applying existing LiDAR Inertial Odometry (LIO) methods on low-cost WMS. To
address these limitations, we propose a novel LIO for WMSs based on Hybrid
Continuous Time Optimization (HCTO) considering the optimality of Lidar
correspondences. First, HCTO recognizes patterns in human motion (high-
frequency part, low-frequency part, and constant velocity part) by analyzing
raw IMU measurements. Second, HCTO constructs hybrid IMU factors according to
different motion states, which enables robust and accurate estimation against
vibration-induced noise in the IMU measurements. Third, the best point
correspondences are selected using optimal design to achieve real-time
performance and better odometry accuracy. We conduct experiments on head-
mounted WMS datasets to evaluate the performance of our system, demonstrating
significant advantages over state-of-the-art methods. Video recordings of
experiments can be found on the project page of HCTO:
https://github.com/kafeiyin00/HCTO.

###### keywords:

LiDAR inertial odometry , 3D mapping , SLAM , wearable sensing , point clouds

††journal: Journal of Photogrammetry and Remote Sensing

##  1 Introduction

Thanks to mobility, miniaturization, and portability, Wearable Mapping System
(WMS) has emerged as promising tools in the field of emergency rescue
(Kachurka et al., 2021), safe construction (Pirkl et al., 2016; Attard et al.,
2018) and Building Information Model (BIM) (Yiğit et al., 2023; Meyer et al.,
2023). In particular, wearable 3D mapping systems provide an efficient way to
collect 3D information in Global Navigation Satellite System (GNSS)-denied
complex environments, which may be difficult to access for common wheeled or
legged robots. The 3D information can be used as prior maps for robot
navigation (Wen et al., 2022).

Figure 1: Hardware configuration of the compact helmet-based wearable sensing
system. The main sensor is the Livox MID360.

Several commercial WMSs have been successfully applied in developing prior map
for GNSS-denied environments. Some notable examples are the backpack mapping
system, NavVis, which combines multiple scanners, and the hand-held mapping
system, GeoSLAM, which employs a rotation module. Zhou et al. (2023b) designed
a dual-LiDAR backpack mapping system, which utilizes multiple ground
constraints to restrict the SLAM drift in multi-level buildings. While the
multi-scanner configuration and the rotation module can increase the Field of
View (FoV), they also increase the weight and burden for the operator.
Moreover, existing WMSs usually employ high-grade IMU, which incurs high costs
that can be inhibitory for non-professional users. Thus, developing a compact
WMS is a hot topic in both academia and industry (Li et al., 2023). In
reducing cost, the limited FoV of single-LiDAR configurations and high noise
of low-cost IMUs (e.g., ICM40609 costs only 10 dollars) then pose a challenge
for WMS. Moreover, existing WMSs generally require that users move slowly to
avoid vibration and improve LiDAR Inertial Odometry (LIO) accuracy (Zhang et
al., 2019), which is inconvenient and also hard to guarantee in the most
practical scenarios. Although the effects of vibration are more obvious on a
helmet system compared to backpack or handheld ones (Beauregard, 2006), head-
mounted sensing has the benefit of “what you see is what you get” (Sidaoui et
al., 2019). In this paper, to boost the capability of compact WMS and satisfy
the prior map requirements of automated robot delivery in complex scenes, we
are concerned with three aspects of LIO on the compact helmet-based WMS: (1)
vibration-tolerant continuous-time optimization based on hybrid IMU factors,
(2) human motion-based factors, and (3) feature selection for low-latency and
low-drift in complex scenes.

Figure 2:  Prior map constructed by Hybrid Continuous Time Optimization (HCTO)
for robot navigation in different challenging scenes using wearable devices.
(a) The mapping results from Fast-Lio2 (Xu et al., 2022) are corrupted by
long-term drift, where the building walls are slanted and the ground plane
bends downwards. The robot goes to the wrong place using the map. (b) The
mapping results using the proposed HCTO maintain good accuracy, where the
building walls are vertical and the ground plane maintains levelness. The
robot goes to the right goal.

For trajectory formulation and optimization of LIO, existing methods can be
generally categorized into two types: Continuous Time Optimization (CTO) and
Discrete Time Optimization (DTO) (Cioffi et al., 2022). The traditional DTO
has been thoroughly studied (Cadena et al., 2016; Karam et al., 2021), which
is not the main target of this paper. Compared with DTO, CTO has several
intrinsic benefits and has been investigated for LIO in recent years (Nguyen
et al., 2023; Lv et al., 2021). First, CTO’s trajectory could be sampled at
any time, which makes it easy to fuse asynchronous data (LiDAR and IMU), and
especially human motion constraints in our WMS case. Second, we do not need to
add additional variables at the time instances corresponding to a measurement
or motion constraint, which keeps the number of variables manageable. However,
CTO assumes polynomial smoothness of the motion, which may lead to loss of
information and an ill-fitted model at high-variation segments, i.e., the
motion vibration caused by human motions in our WMS case. In this work, to
cope with the motion vibration of humans when using CTO, we divide the walking
motion into high-frequency parts, low-frequency parts, and constant-velocity
parts, then handle them separately. Instead of using a higher-order B-spline
trajectory or adding more knots to increase the model fitting ability (Cioffi
et al., 2022), our method constructs pre-integration constraints and raw
measurement constraints in high- and low-frequency parts, respectively. Thus,
we refer to the proposed method as hybrid optimization. Moreover, the constant
velocity factor is also constructed to limit the attitude drift.

In the aspect of motion-based constraints, zero-velocity detection and update
(ZUPT) is the most common human motion constraint (Wang et al., 2019).
However, ZUPT is only available for foot-mounted IMU. With the rapid
development of deep learning, a lot of data-driven motion-based methods are
proposed to predict IMU bias (Buchanan et al., 2022a) or velocity (Buchanan et
al., 2022b) to contain the low-cost IMU error accumulation. Moreover, the
motion-based constraints can be integrated into batch optimization to enhance
the system performance (Clark et al., 2017). The activities detected from the
IMU are also used for loop closure successfully (Zhou et al., 2023a). However,
in this paper, we do not focus on complex learning of the motion constraint,
instead, we provide a continuous fusion framework with a simple yet effective
constraint to correct attitude drift. In theory, the attitude (roll and pitch)
is observable for LIO and Visual Inertial Odometry (VIO), because of the
gravity vector (Qin et al., 2018). Unfortunately, in practice, due to the
uneven distribution or degeneration of feature correspondences (Pan et al.,
2021), the Iterative Closest Point (ICP)(Segal et al., 2009)-like the matching
process in LIO often converges to a local minimum, further resulting in wrong
IMU state estimation and the loss of attitude observability. The loss of
attitude observability is common when only a low-cost IMU is used, or the
system is moving under vibration according to our experience. In this work,
inspired by the complementary filters for orientation estimation (Mahony et
al., 2008), we analyze human constant-velocity motion patterns using the
sequential IMU and correct the attitude at each pedestrian step.

For a $k^{th}$ order B-spline trajectory, each constraint corresponds to $k$
knots, which means more Jacobian estimation than the DTO when performing the
batch optimization (Park et al., 2021). The maintenance of all feature points
in the LIO system may undermine losing the real-time performance of the LIO
system. Selecting fewer but more important feature points will boost the
performance of the B-spline trajectory estimation. Feature point selection has
been widely studied in the field of SLAM (Jiao et al., 2021; Zhao and Vela,
2020). LOAM (Zhang and Singh, 2014) selected features according to space
distribution. MULLS (Pan et al., 2021) selected features considering different
semantic types. M-LOAM (Jiao et al., 2021) selected features according to the
information matrix’s spectral attributes using a greedy manner. Different from
Jiao et al. (2021), we guarantee the optimality of the spline knots (other
than pose) considering the similarity between jacobians of LiDAR
correspondences. Additionally, we propose a group-based stochastic greedy
solver aimed at accelerating the feature selection process through an analysis
of feature similarity.

To sum up, applying existing LIO methods on compact WMS mainly faces two
challenges: (1) The human motion vibration cannot be modeled using low-order
spline (e.g., the commonly used $4^{th}$ order), while a higher-order B-spline
or more knots will dramatically increase the computational burden and
uncertainty of the system. (2) The uneven or degenerated LiDAR feature
correspondences cause the matching process to converge to a local minimum,
further resulting in a long-term drift. The above problems result in attitude
and Z-direction drift as shown in Fig.2 (a). The “bent” point cloud map,
though geometrically consistent at the local scale, cannot serve as the prior
map for the planning and navigation of delivery robots.

Aiming at these problems, we propose a novel LIO with Hybrid Continuous Time
Optimization (HCTO) to build an accurate point cloud map in real time. The
main contributions of our work are as follows:

  * 1.

HCTO recognizes the Low-Frequency Part (LFP) and High-Frequency Part (HFP)
from sequential IMU measurements. IMU constraints based on raw measurements
are used in LFP to better fit the observations. On the contrary, IMU pre-
integration constraints are used in HFP to suppress severe vibrations, which
cannot be fitted using a low-order spline trajectory model. Hence, HCTO
realizes efficient fusion of IMU without increasing variables and uncertainty
of a spline trajectory.

  * 2.

HCTO recognizes the Constant Velocity Part (CVP) from sequential IMU
measurements and corrects system attitude by constructing CVP constraints for
the spline trajectory, which restricts the attitude drift.

  * 3.

HCTO proposes a feature selection scheme using optimal design and a group-
based stochastic-greedy solver, which achieves real-time performance and
better odometry accuracy in degenerated scenes.

  * 4.

HCTO’s validation and effectiveness are demonstrated with extensive
experiments on public and in-house helmet-based datasets.

The rest of this paper is structured as follows. The preliminaries for our
system are provided in Section 2. A brief system overview including the
hardware and software structure is provided in Section 3. A detailed
description of our system is presented in Section 4. The experiments are
conducted on public and in-house helmet-based datasets in Section 5.
Conclusion and future work are drawn in Section 6.

##  2 Preliminary

###  2.1 Notation

In this paper, for a vector $\mathbf{p}\in\mathbb{R}^{3}$, the hat notation
$\hat{\mathbf{p}}$ denotes the global optimization-based estimation of
$\mathbf{p}$. The breve notation $\breve{\mathbf{p}}$ denotes the IMU-
propagated information. For a matrix $\mathbf{M}$, $\mathbf{M}_{r,c}$ denotes
the element at $r^{th}$ row and $c^{th}$ column of $\mathbf{M}$. The
orientation could be represented by rotation matrix $\mathbf{R}\in SO(3)$. We
reserve the right superscript and subscript for the coordinate transformation,
i.e., $\left[\mathbf{R}_{B}^{A},\mathbf{p}_{B}^{A}\right]$ transform a vector
from frame B to frame A.

###  2.2 State Estimation

We define the $i^{th}$ time segment spanning a period $[t_{i},t_{i+1})$ with
the constant duration $\Delta t$. The system state $\mathcalbf{X}(t)$ at time
$t\in[t_{i},t_{i+1})$ is defined as follows:

$\mathcalbf{X}\left(t\right)=\left[\mathbf{R}_{b}^{w}\left(t\right),\mathbf{p}_{b}^{w}\left(t\right),\mathbf{b}_{a}\left(t\right),\mathbf{b}_{g}\left(t\right)\right]\in
SO\left(3\right)\times\mathbb{R}^{9},$ (1)

where $\mathbf{R}_{b}^{w}(t),\mathbf{p}_{b}^{w}\left(t\right)$ are rotation
and translation of body frame defined in the world frame.
$\mathbf{q}_{b}^{w}(t)$ is the quaternion corresponding to
$\mathbf{R}_{b}^{w}(t)$.
$\mathbf{b}_{a}\left(t\right),\mathbf{b}_{g}\left(t\right)$ are IMU
accelerometer and gyroscope biases, which are modeled using the random-walk
model for low-cost IMU (Shin and El-Sheimy, 2004). It should be noted that the
velocity $\mathbf{v}_{b}^{w}\left(t\right)$ is not included in the system
states as we use the B-spline to formulate the trajectory.

###  2.3 IMU pre-integration

IMU pre-integration is commonly used for DTO by integrating the IMU
measurements in time duration to get relative spatial relationships. Given the
$i^{th}$ time segment $t\in[t_{i},t_{i+1})$, the state variables are
constrained by the IMU measurements (acceleration $\breve{\mathbf{a}}^{b}(t)$
and gyroscope $\breve{\mathbf{\omega}}^{b}(t)$) in this time segment with IMU
integration as follows:

$\displaystyle\mathbf{p}_{b}^{w}\left(t_{i+1}\right)$
$\displaystyle=\mathbf{p}_{b}^{w}\left(t_{i}\right)+\mathbf{v}_{b}^{w}\left(t_{i}\right)\Delta
t$ (2)
$\displaystyle+\iint_{t\in\left[t_{i},t_{i+1}\right)}{\left(\mathbf{R}_{b_{t}}^{w}\left(\breve{\mathbf{a}}^{b}(t)-\mathbf{b}_{a}\left(t_{i}\right)\right)-\mathbf{g}^{w}\right)}dt^{2},$
$\displaystyle\mathbf{v}_{b}^{w}\left(t_{i+1}\right)$
$\displaystyle=\mathbf{v}_{b}^{w}\left(t_{i}\right)$
$\displaystyle+\int_{t\in\left[t_{i},t_{i+1}\right)}{\left(\mathbf{R}_{b_{t}}^{w}\left(\breve{\mathbf{a}}^{b}(t)-\mathbf{b}_{a}\left(t_{i}\right)\right)-\mathbf{g}^{w}\right)dt},$
$\displaystyle\mathbf{q}_{b}^{w}\left(t_{i+1}\right)$
$\displaystyle=\mathbf{q}_{b}^{w}\left(t_{i}\right)$
$\displaystyle\otimes\int_{t\in\left[t_{i},t_{i+1}\right)}{0.5\mathbf{\Omega}\left(\breve{\mathbf{\omega}}^{b}(t)-\mathbf{b}_{g}\left(t_{i}\right)\right)\mathbf{q}_{b_{t}}^{w}dt},$

where

$\displaystyle\mathbf{\Omega}\left(\mathbf{\omega}\right)=\left[\begin{matrix}-\mathbf{\omega}_{\times}&\mathbf{\omega}\\\
-\mathbf{\omega}^{\top}&0\\\
\end{matrix}\right],\mathbf{\omega}_{\times}=\left[\begin{matrix}0&-\mathbf{\omega}_{2}&\mathbf{\omega}_{1}\\\
\mathbf{\omega}_{2}&0&-\mathbf{\omega}_{0}\\\
-\mathbf{\omega}_{1}&\mathbf{\omega}_{0}&0\\\ \end{matrix}\right],$ (3)

where $\mathbf{g}^{w}$ is the gravity vector in the world frame.
$\mathbf{\omega}_{0}$, $\mathbf{\omega}_{1}$, and $\mathbf{\omega}_{2}$ are
the three elements in vector $\mathbf{\omega}$. After changing the reference
frame of integration to $b_{i}$, the three IMU pre-integration factors,
$\Delta\mathbf{p}_{i+1}^{i},\Delta\mathbf{v}_{i+1}^{i}$ and
$\Delta\mathbf{q}_{i+1}^{i}$, are defined in Eq.(4) (Qin et al., 2018). These
three factors can also be used to restrict the relative motion of the spline
trajectory, which will be introduced in the following subsections.

$\displaystyle\Delta\mathbf{p}_{i+1}^{i}$
$\displaystyle=\iint_{t\in\left[t_{i},t_{i+1}\right)}{\mathbf{R}_{b_{t}}^{b_{i}}\left(\breve{\mathbf{a}}_{t}-\mathbf{b}_{a}\left(t_{i}\right)\right)}dt^{2},$
(4) $\displaystyle\Delta\mathbf{v}_{i+1}^{i}$
$\displaystyle=\int_{t\in\left[t_{i},t_{i+1}\right)}{\mathbf{R}_{b_{t}}^{b_{i}}\left(\breve{\mathbf{a}}_{t}-\mathbf{b}_{a}\left(t_{i}\right)\right)dt},$
$\displaystyle\Delta\mathbf{q}_{i+1}^{i}$
$\displaystyle=\int_{t\in\left[t_{i},t_{i+1}\right)}{0.5\mathbf{\Omega}\left(\breve{\mathbf{\omega}}_{t}-\mathbf{b}_{g}\left(t_{i}\right)\right)\Delta\mathbf{q}_{b_{t}}^{b_{i}}dt}.$

###  2.4 Continuous Time Optimization (CTO) using B-Spline

A $k^{th}$ order B-Spline with $N+1$ knots (or control points) could be
formulated using the De Boor-Cox recurrence relation as follows:

$\mathbf{p}\left(t\right)=\sum_{i=0}^{N}{B_{i,k}(t)\mathbf{p}_{i}},$ (5)

where $B_{i,k}(t)$ is the B-Spline basis function (Qin, 1998). We define
$u(t):=(t-t_{0})/\Delta t-i$ as normalized time elapsed in the window
$[t_{i},t_{i+1})$ and write it as $u$ for simplification. The value
$\textbf{p}(u)$ could be represented by matrix multiplication:

$\mathbf{p}\left(u\right)=\left[\mathbf{p}_{i},\mathbf{p}_{i+1},\cdots,\mathbf{p}_{i+k-1}\right]\mathbf{Mu},$
(6)

where $\mathbf{u}_{n}=u^{n}$. M is the $k^{th}$ order blending matrix (Qin,
1998):

$\begin{split}\mathbf{M}_{s,n}=\frac{C_{k-1}^{n}}{\left(k-1\right)!}\sum_{l=s}^{k-1}{\left(-1\right)^{l-s}\left(k-l-1\right)^{k-1-n}},\\\
s,n\in{0,...,k-1}.\end{split}$ (7)

$\textbf{p}(u)$ can also be transformed to a cumulative formulation (Sommer et
al., 2020):

$\begin{split}\mathbf{p}\left(u\right)=\mathbf{p}_{i}+\sum_{j=1}^{k-1}{\left(\mathbf{p}_{i+j}-\mathbf{p}_{i+j-1}\right)(\widetilde{\mathbf{M}}\mathbf{u}})_{j},\end{split}$
(8)

where
$\widetilde{\mathbf{M}}_{j,n}=\sum\nolimits_{s=j}^{k-1}{\mathbf{M}_{s,n}}$. As
our system state knot $\mathcalbf{X}_{i}$ is in a Lie group, the cumulative
B-Spline of the system with state knots
$\left\\{\mathcalbf{X}_{0},...\mathcalbf{X}_{i},...,\mathcalbf{X}_{N}\right\\}$
has the following form:

$\begin{split}\mathcalbf{X}\left(u\right)=\mathcalbf{X}_{i}\prod_{j=1}^{k-1}{\mathrm{Exp}\left(\mathrm{Log}\left(\mathcalbf{X}_{i+j-1}^{-1}\mathcalbf{X}_{i+j}\right)(\widetilde{\mathbf{M}}\mathbf{u})_{j}\right)}.\end{split}$
(9)

We can also obtain the velocity $\dot{\mathbf{p}}_{b}^{w}(u)$ , acceleration
$\ddot{\mathbf{p}}_{b}^{w}(u)$, and angular velocity
$\dot{\mathbf{R}}_{b}^{w}\left(u\right)$ at any time according to the
differential of $\mathcalbf{X}(u)$. Using the IMU measurements at these
specific times, we could restrict the spline trajectory by comparing the
differential results and raw IMU measurements, which is the standard way to
fuse the IMU information of the CTO (Cioffi et al., 2022).

###  2.5 Maximum-A-Priori (MAP) Optimization in a sliding window

Figure 3: Maximum-A-Priori (MAP) optimization in the active window taking the
$4^{th}$ order B-Spline as an example.

The core of the proposed LIO is solving the following MAP problem as
illustrated in Fig. 3 and written as Eq. 10:

$\displaystyle
f(\hat{\mathcalbf{X}})=\sum_{t\in\mathbf{\Phi}}{\left\|r_{t}^{\mathcal{H}\mathcal{F}}\right\|_{\Sigma_{\mathcal{HF}}}^{2}}+\sum_{t\in\mathbf{\Phi}}{\left\|r_{t}^{\mathcal{L}\mathcal{F}}\right\|_{\Sigma_{\mathcal{LF}}}^{2}}$
(10)
$\displaystyle+\sum_{t\in\mathbf{\Phi}}{\left\|r_{t}^{\mathcal{C}\mathcal{V}}\right\|^{2}_{\Sigma_{\mathcal{CV}}}}+\sum_{t\in\mathbf{\Phi}}{\left\|r_{t}^{\mathcal{L}}\right\|^{2}_{\Sigma_{\mathcal{L}}}}+\left\|r_{t}^{\mathcal{M}}\right\|^{2}_{\Sigma_{\mathcal{M}}}.$

Our system solves the above $k^{th}$ order B-Spline MAP problem every time
receiving a LiDAR frame. Assume $t_{i}$ and $t_{i+m}$, just cover the lower
bound and upper bound for the current LiDAR scan. According to the locality of
B-Spline, the trajectory segment of $(t_{i},t_{i+m})$ is affected by a set of
knots $[\mathcalbf{X}_{i},...,\mathcalbf{X}_{i+m+k-1}]$, defined as active
nodes (Lv et al., 2021). Only the active knots will be optimized in the
current sliding window. We define the active sensor data duration as
$\mathbf{\Phi}=[t_{i},t_{i+m}]$, and use the factors $r_{t}^{\mathcal{CV}}$
(CVP factor defined in Section 4.2.1), $r_{t}^{\mathcal{HF}}$ (HFP factor
defined in Section 4.2.2), $r_{t}^{\mathcal{LF}}$ (LFP factor defined in
Section 4.2.3), and $r_{t}^{\mathcal{L}}$ (LiDAR factor defined in Section
4.3) within the duration $\mathbf{\Phi}$ for the optimization. We also
marginalize the knot out of the window to build a prior constraint
$r_{t}^{\mathcal{M}}$ for the first $k-1$ knots in the active window to
achieve better accuracy while restricting the computation load (Lv et al.,
2023).
$\Sigma_{\mathcal{CV}},\Sigma_{\mathcal{HF}},\Sigma_{\mathcal{LF}},\Sigma_{\mathcal{L}}$,
and $\Sigma_{\mathcal{M}}$ are the covariance matrices corresponding to the
factors. $\Sigma_{\mathcal{CV}},\Sigma_{\mathcal{HF}},\Sigma_{\mathcal{LF}}$
are related to the raw measurements of IMU, which could be set according to
the measurement noises from the IMU specifications. $\Sigma_{\mathcal{L}}$ is
set according to the LiDAR measurement accuracy (3 cm for most Livox LiDAR).
$\Sigma_{\mathcal{M}}$ is calculated according to the marginalization (Qin et
al., 2018). It should be noticed that, like most LIO systems (Xu et al.,
2022), we propagated the IMU measurement to get the initial poses. While the
system states presented in knots of B-Spline is initialized with IMU-only
optimization (Lv et al., 2021).

##  3 System Overview

###  3.1 Hardware of the compact helmet-based sensing system

We integrated a compact helmet-based sensing system as illustrated in Fig.1.
The main sensor is the Livox MID360, which contains a built-in low-cost IMU
(ICM40609). The sensors are mounted on a standard Future Assault Shell
Technology (FAST) Helmet. The weight of the wearable sensing system is about
1.2 kg.

###  3.2 Main function blocks of diagram of the proposed helmet-based LIO

Figure 4: System overview of the proposed helmet-based LIO system. (a) LiDAR
Inertial Odometry (LIO) with Hybrid Continuous Time Optimization (HCTO). (b)
Diagram of HCTO.

Fig. 4 provides an overview of our helmet-based LIO system. Taking the low-
cost IMU and solid-state LiDAR data as input, the proposed system constructs
hybrid IMU constraints while removing the LiDAR motion distortion based on
HCTO (Section 4.1 and 4.2) first. Then, the point-to-plane correspondences are
extracted and selected using the optimal-aware point feature association block
(Section 4.3). The system states are optimized using hybrid IMU factors and
LiDAR factors in a sliding window when a new LiDAR frame comes. At last, the
current LiDAR frame is maintained in the keyframe database if it is selected
as a keyframe (Section 4.4). In the next section, we detail the numbered
blocks.

##  4 Methodology

###  4.1 Motion state recognition

Fig. 5 illustrates the accelerometer output on the helmet during human motion.
It can be easily observed that there is a repetitive motion pattern for each
step. As for the most commonly used cubic Spline ($4^{th}$ order), it only
guarantees the linear changes of acceleration, which results in ill-fitting
using the raw IMU measurements as residual during the vibration, as shown in
the zoomed view of Fig. 5. The LFP segment can be effectively modeled using a
linear model, whereas the HFP segment cannot. Accordingly, we partition the
trajectory into two parts, HFP and LFP, delineated by the Root Mean Squared
Error (RMSE) $\sigma_{motion}$ of acceleration measurements within the
B-Spline time interval $\Delta t$, as derived from linear fitting residuals.
If $\sigma_{motion}$ is bigger than three times the accelerometer measurement
error, this time segment is determined as an HFP following the three-sigma
rule. Otherwise, this time segment belongs to LPF because the acceleration
measurements can be fitted well using a linear model.

Moreover, we can obtain the initial guess of acceleration
$\mathbf{a}^{w}\left(t\right)$ in the world frame using pure IMU integration
at any time $t$. We detect the time
$\left\|\mathbf{a}^{w}\left(t\right)\right\|=0$, which is the moment called
the CVP. Details on how to use them to build hybrid IMU factors are in the
following section.

Figure 5: Illustration of different motion states in the repetitive motion
pattern.

###  4.2 Construction of Hybrid IMU factors

####  4.2.1 CVP

Inspired by the complementary filters for orientation estimation (Mahony et
al., 2008), we first investigate the force equation of the system as follows:

$\begin{split}\mathbf{R}_{b}^{w}\left(t\right)\left(\mathbf{a}^{b}(t)+\mathbf{b}_{a}\left(t\right)\right)+\mathbf{g}^{w}=\mathbf{a}^{w}\left(t\right).\end{split}$
(11)

As $\left\|\mathbf{a}^{w}\left(t\right)\right\|=0$, we have the following
relationship between the states and the gravity:

$\mathbf{R}_{b}^{w}\left(t\right)\left(\mathbf{a}^{b}(t)+\mathbf{b}_{a}\left(t\right)\right)=-\mathbf{g}^{w},\
\text{if }\left\|\mathbf{a}^{w}\left(t\right)\right\|=0.$ (12)

We use the condition $\left\lVert\breve{\mathbf{a}}^{w}(t)\right\rVert<0.3$ as
the trigger for adding a CVP factor to Eq. (11), where
$\breve{\mathbf{a}}^{w}(t)$ is the IMU-predicted acceleration in the world
frame Eq. (12).

We only use the CVP factor to correct the roll and pitch of the state
estimate. Assume $\delta\mathbf{\theta}(t)$ is the rotation correction
corresponding to initial guess $\breve{\mathbf{R}}_{b}^{w}(t)$, Eq. (12) can
be written as:

$\begin{split}-\breve{\mathbf{R}}_{b}^{w}\left(t\right)^{\top}\left(I-\left[\delta\hat{\mathbf{\theta}}\left(t\right)\right]_{\times}\right)\mathbf{g}^{w}=\breve{\mathbf{a}}^{b}(t)+\mathbf{b}_{a}(t).\end{split}$
(13)

Then the constant velocity factor $r_{t}^{\mathcal{CV}}$ at time t is written
as follow:

$\begin{split}r_{t}^{\mathcal{CV}}&=\left[\begin{matrix}0&-1\\\ 1&0\\\
\end{matrix}\right]\left[\begin{array}[]{c}\delta\mathbf{\hat{\theta}}\left(t\right)_{0}\\\
\delta\mathbf{\hat{\theta}}\left(t\right)_{1}\\\ \end{array}\right]\\\
+&\frac{1}{\left\|\mathbf{g}^{w}\right\|}\left[\left(\breve{\mathbf{R}}_{b}^{w}\left(t\right)^{\top}\breve{\mathbf{a}}^{b}(t)+\mathbf{b}_{a}\left(t\right)\right)+\breve{\mathbf{R}}_{b}^{w}\left(t\right)^{\top}\mathbf{g}^{w}\right]_{1:2}.\end{split}$
(14)

where $\delta\mathbf{\hat{\theta}}(t)_{0}$ and
$\delta\mathbf{\hat{\theta}}(t)_{1}$ are the roll and pitch corrections,
respectively.

####  4.2.2 HFP

As illustrated in Fig.5, the high-frequency part of motion can not be fitted
using $4^{th}$ order B-Spline, which only guarantees the linear changing of
acceleration, hence cannot represent the high-frequency motion. Consequently,
naively fitting this will lead to a large error and unstable results.
Increasing the order of B-Spline or decreasing the $\Delta t$, namely
increasing the parameter of the trajectory, could be one solution but will
increase the computation load and uncertainty of the system. Instead of
increasing the parameter to fit the vibrated motion, we integrate the raw IMU
measurements and construct HPF factors to constrain the relative motion
between the start and end of the high-frequency motion, which alleviates the
effect of the high-frequency part on sequential B-Spline segments. Assume the
period $[t_{i},t_{i+1})$ with duration $\Delta t$ belongs to HFP. We integrate
the IMU measurements in $[t_{i},t_{i+1})$ to get the position, velocity, and
rotation elements ($\Delta\mathbf{p}_{i+1}^{i},\Delta\mathbf{v}_{i+1}^{i}$ and
$\Delta\mathbf{q}_{i+1}^{i}$) of IMU pre-integration according to Eq. (4). The
HFP factor $r_{t}^{\mathcal{HF}}$ is written as follows:

$\begin{split}&r_{t}^{\mathcal{HF}}=\\\
&\left[\begin{array}[]{c}\Delta\mathbf{p}_{i+1}^{i}\\\
\Delta\mathbf{v}_{i+1}^{i}\\\ \Delta\mathbf{q}_{i+1}^{i}\\\
\end{array}\right]-\left[\begin{array}[]{c}\mathbf{R}_{b}^{w}\left(t_{i}\right)^{\top}\left(\begin{array}[]{c}\mathbf{p}_{b}^{w}\left(t_{i+1}\right)-\mathbf{p}_{b}^{w}\left(t_{i}\right)\\\
-0.5\mathbf{g}^{w}\Delta
t^{2}-\dot{\mathbf{p}}_{b}^{w}\left(t_{i}\right)\Delta t\\\
\end{array}\right)\\\
\mathbf{R}_{b}^{w}\left(t_{i}\right)^{\top}\left(\dot{\mathbf{p}}_{b}^{w}\left(t_{i+1}\right)-\mathbf{g}^{w}\Delta
t-\dot{\mathbf{p}}_{b}^{w}\left(t_{i}\right)\right)\\\
\mathbf{R}_{b}^{w}\left(t_{i}\right)^{\top}\mathbf{R}_{b}^{w}\left(t_{i+1}\right)\\\
\end{array}\right].\end{split}$ (15)

Remark. 1 It’s important to highlight that the system velocity is extracted
from the differential of the B-Spline trajectory, setting it apart from the
conventional DTO and ensuring local trajectory consistency. In the high-
frequency part, unlike the traditional CTO, the B-Spline trajectory doesn’t
attempt to fit the oscillated acceleration measurements, thereby facilitating
stable and swift convergence.

Remark. 2 Integration of the raw IMU measurements in the high-frequency part
can be regarded as a special low-pass filter (Tavares, 1966). This integration
effectively suppresses the peak values attributed to jitter. However, the HPF
factor introduced here differs from mere low-pass filtering applied to the
acceleration measurement in one axis. Instead, it incorporates the IMU
measurements according to the strap-down IMU integration principle, ensuring
the relative motion relationship remains consistent from the beginning to the
end of the high-frequency motion.

####  4.2.3 LFP

For the low-frequency portion of the motion, all IMU measurements are directly
employed in the B-Spline parameter estimation to fully utilize the available
information. If the period $[t_{i},t_{i+1})$ belongs to LFP, we construct the
following LFP factor $r_{t}^{\mathcal{LF}}$ for each raw IMU measurement:

$\begin{split}r_{t}^{\mathcal{LF}}=\left[\begin{array}[]{c}\ddot{\mathbf{p}}_{b}^{w}\left(t\right)\\\
\dot{\mathbf{R}}_{b}^{w}\left(t\right)\\\
\end{array}\right]-\left[\begin{array}[]{c}\mathbf{R}_{b}^{w}\left(t\right)\left(\breve{\mathbf{a}}^{b}(t)+\mathbf{b}_{a}\left(t\right)\right)+\mathbf{g}^{w}\\\
\,\,\breve{\mathbf{\omega}}^{b}(t)+\mathbf{b}_{g}\left(t\right)\\\
\end{array}\right].\end{split}$ (16)

Using only $r_{t}^{\mathcal{CV}}$, $r_{t}^{\mathcal{HF}}$, and
$r_{t}^{\mathcal{LF}}$, within a LiDAR frame duration (i.e., 0.1 s in most
scanning configuration), we conduct an IMU only optimization to get the
initial guess of system states $\breve{\mathcalbf{X}}\left(t\right)$, which is
used to correct the LiDAR motion distortion. The correction is important for
the following LiDAR constraints association.

###  4.3 Optimality-aware LiDAR factor association

Figure 6: Grouping the candidate correspondences in the current LiDAR frame.

According to the initial guess of system states $\breve{\mathcalbf{X}}$, we
project the points in the current frame into the map coordinate. For a LiDAR
measurement $\mathbf{f}_{t}$, we search its closest points from the map points
and calculate their average value $\mu$ and plane normal $\mathbf{n}$. The map
points are maintained using the ikdtree (Xu et al., 2022) and will be
introduced in Section 4.4. The point-to-plane factor $r_{t}^{\mathcal{L}}$ is
written as follows:

$\begin{split}r_{t}^{\mathcal{L}}=\mathbf{n}^{\top}(\mathbf{R}^{w}_{b}(t)\mathbf{f}_{t}+\mathbf{p}^{w}_{b}(t)-\mathcalbf{\mu}).\end{split}$
(17)

Given the set of point-to-plane factors in the current LiDAR frame
$S_{\mathcalbf{L}}=\\{r_{t}^{\mathcal{L}}\\}$, there are usually more than
several thousand LiDAR correspondences to be added to the MAP optimization in
the sliding window, which leads to heavy computation load for B-Spline
estimation and unforeseen degeneration caused by the uneven distribution of
the correspondences (Zhao and Vela, 2020; Zhang and Singh, 2014). Selecting
good correspondences for the state estimation not only benefits the time
efficiency but also odometry accuracy (Jiao et al., 2021). The selection of an
optimal correspondence subset $S^{O}_{\mathcalbf{L}}\subset S_{\mathcalbf{L}}$
for variable solving is also called an optimal design (Pukelsheim, 2006).

As our system states $\mathcalbf{X}$ are presented as $k^{th}$ order B-Spline,
hence there are $k$ knots related to the constant $\mathcalbf{L}_{t}$. As the
center knot has the most correlation with the current LiDAR frame according to
the B-Spline definition, we only consider the center knot located in the
current LiDAR frame time duration for the following optimal analysis to
guarantee real-time performance in practice. We linearize $\mathcalbf{L}_{t}$
and calculate its Jacobian w.r.t $\mathcalbf{X}$ as
$\textbf{J}_{t}=\partial\mathcalbf{L}_{t}/\partial\mathcalbf{X}|_{\mathcalbf{X}=\breve{\mathcalbf{X}}}$.
The information matrix $\varLambda$ for $S^{O}_{\mathcalbf{L}}$ is defined as
follows:

$\begin{split}\varLambda=\sum_{L_{t}\in
S^{O}_{\mathcalbf{L}}}{\textbf{J}_{t}^{\top}\textbf{J}_{t}}.\end{split}$ (18)

The information matrix $\varLambda$ reveals the uncertainties for the
variables to be estimated and also the optimality of the subset
$S^{O}_{\mathcalbf{L}}$ (Pukelsheim, 2006). Specifically, we utilize the
minimum eigenvalue $\lambda_{min}(\varLambda)$, a commonly used metric, to
qualify the optimality of information matrix $\varLambda$ (Jiao et al., 2021).
We select the minimum eigenvalue as the metric because it restricts the
possible degenerated direction. To achieve the most optimal design for the
knots estimation, we try to maximize the objective function
$g(S^{O}_{\mathcalbf{L}})$:

$\begin{split}arg\max\ &g(S^{O}_{\mathcalbf{L}})\ s.t.\
S^{O}_{\mathcalbf{L}}\subset S_{\mathcalbf{L}},|S^{O}_{\mathcalbf{L}}|\leq
N^{O},\\\
&g(S)=\lambda_{min}\left[\varLambda\left(S\right)\right],\end{split}$ (19)

where $N^{O}$ is the desirable size of correspondences remaining in the system
considering the computational load. The objective function in Eq. (19) is an
NP-hard problem and could be approximately solved by stochastic-greedy
heuristic (Khanna et al., 2017). The stochastic-greedy algorithm starts with
an empty set. Then, at each step, it selects one element from a random set
with a size of $|S_{\mathcalbf{L}}|/N^{O}\mathrm{log}(1/\epsilon)$ from the
remaining correspondence set, which gains most of the objective function
$g(S^{O}_{\mathcalbf{L}})$. $\epsilon$ is the decay factor. The iterative
process stops until it gets $N^{O}$ correspondences.

Theorem. 1 (Mirzasoleiman et al., 2015) The set function $g(S)$ is submodular
and monotone increasing w.r.t $S$.

Theorem. 2 (Nemhauser et al., 1978) Let $S^{O*}_{\mathcalbf{L}}$ and
$S^{O\\#}_{\mathcalbf{L}}$ be the optimal set and stochastic-greedy heuristic
result, respectively, then

$\begin{split}\frac{g(S^{O*}_{\mathcalbf{L}})-g(S^{O\\#}_{\mathcalbf{L}})}{g(S^{O*}_{\mathcalbf{L}})}\leq\left(\frac{N^{O}-1}{N^{O}}\right)^{N^{O}}+\epsilon\leq\frac{1}{e}+\epsilon.\end{split}$
(20)

Input: $S_{\mathcalbf{L}}$, ${\\{\mathbf{J}_{t}\\}}$, $\epsilon$, $\lambda$,
$N^{O},D_{v}$

Output: Selected LiDAR correspondences $S^{O}_{\mathcalbf{L}}$

1 Initialization of the set $S^{O}_{\mathcalbf{L}}\leftarrow\oslash$;

2 Calculate mean value $\\{\mu_{0},\mu_{1},...,\mu_{5}\\}$ and variance
$\\{\Sigma_{0},\Sigma_{1},...,\Sigma_{5}\\}$ at each dimension for
${\\{\mathbf{J}_{t}\\}}$;

3

4foreach  _$\mathbf{J}_{t}\in\\{\mathbf{J}_{t}\\},\
\bar{\mathbf{J}}_{t}\in\\{\bar{\mathbf{J}}_{t}\\}$_ do

5 $\bar{\mathbf{J}}_{t,0}=(\mathbf{J}_{t,0}-\mu_{0})/\Sigma_{0}$;
$\bar{\mathbf{J}}_{t,1}=(\mathbf{J}_{t,1}-\mu_{1})/\Sigma_{1}$;

6 $\bar{\mathbf{J}}_{t,2}=(\mathbf{J}_{t,2}-\mu_{2})/\Sigma_{2}$;
$\bar{\mathbf{J}}_{t,3}=(\mathbf{J}_{t,3}-\mu_{3})/\Sigma_{3}$;

7 $\bar{\mathbf{J}}_{t,4}=(\mathbf{J}_{t,4}-\mu_{4})/\Sigma_{4}$;
$\bar{\mathbf{J}}_{t,5}=(\mathbf{J}_{t,5}-\mu_{5})/\Sigma_{5}$;

8

9 end foreach

10

11Divide $\\{\mathbf{J}_{t}\\}$ into groups $\\{\mathbf{g}_{k}\\}$ using
$\\{\bar{\mathbf{J}}_{t}\\}$ as feature with voxel size of $D_{v}$;

12

13while  _ $S^{O}_{|\mathcalbf{L}}|<N^{O}$_ do

14 $\mathcalbf{G}\leftarrow$ subset obtained by random sampling
$\lambda\mathrm{log}(1/\epsilon)$ groups from $\\{\mathbf{g}_{k}\\}$;

15 $\mathcalbf{R}={\\{\mathbf{J}}_{j}\\}\leftarrow$ subset obtained the first
element in each sampled group $\mathcalbf{G}$ with
${\mathbf{J}}_{j}\in\mathbf{g}_{k}$;

16 foreach  _${\mathbf{J}}_{j}\in\mathcalbf{R}$_ do

17 Compute $\varLambda_{j}=\mathbf{J}_{j}^{\top}\mathbf{J}_{j}$;

18

19 end foreach

20
$j^{*}\leftarrow\mathrm{argmax}_{j\in\mathcalbf{R}}\lambda_{min}[\varLambda(S^{O}_{\mathcalbf{L}})+\varLambda_{j}]$;

21
$\varLambda(S^{O}_{\mathcalbf{L}})\leftarrow\varLambda(S^{O}_{\mathcalbf{L}})+\varLambda_{j}$;

22 $S^{O}_{\mathcalbf{L}}\leftarrow S^{O}_{\mathcalbf{L}}\bigcup
r_{j^{*}}^{\mathcal{L}}$;
$\mathbf{g}_{k}\leftarrow\mathbf{g}_{k}\backslash\mathbf{J}_{j^{*}}$.

23

24 end while

Algorithm 1 Group-Based Stochastic-Greedy for Feature Selection Considering
Optimality

Eq. (20) elicits a lower bound for the stochastic-greedy heuristic results. In
practice, the stochastic-greedy heuristics will achieve better results than
the lower bound.

Theorem. 3 (Nemhauser et al., 1978) The time complexity of the stochastic-
greedy heuristic is $O(|S_{\mathcalbf{L}}|\mathrm{log}(1/\epsilon))$, which is
related to $|S_{\mathcalbf{L}}|$ but independent of $N^{O}$.

Group-based stochastic-greedy solver: As the candidates with similar Jacobian
have similar contributions to the objective function Eq. (19), we can speed up
the stochastic-greedy search process by analyzing the similarity of Jacobians
and group the candidate correspondences to avoid sampling similar candidate
correspondences in each stochastic step as shown in Algorithm 1. We treat each
Jacobian vector $\textbf{J}_{t}$ as a 6-dimensional feature (3 for quaternion
and 3 for translation). Since the numerical range of the quaternion part and
translation part of $\textbf{J}_{t}$ differ significantly, we regularize each
dimension of $\textbf{J}_{t}$ to get $\bar{\textbf{J}}_{t}$ (Line 2-7 in
Algorithm 1). Second, we segment $S_{\mathcalbf{L}}$ into groups considering
the regularized Jacobian via a high-dimensional voxelization with a voxel size
$D_{v}$ to get $\lambda N^{O}$ groups as illustrated in Fig.6. The above
grouping processing’s complexity is $O(|S_{\mathcalbf{L}}|)$ by saving the
voxel indexes in the hash table. At last, in each greedy searching step, we
randomly get $\lambda\mathrm{log}(1/\epsilon)$ candidates from each group and
select one best candidate (other than
$|S_{\mathcalbf{L}}|/N^{O}\mathrm{log}(1/\epsilon)$ candidates in the original
stochastic-greedy search process). The iterative process stops until it gets
$N^{O}$ correspondences (Line 9-18 in Algorithm 1).

Lazy counting strategy: To determine the voxel size $D_{v}$, a lazy counting
strategy (LCS) is proposed. LCS first downsamples the current candidates with
farthest point sampling (Qi et al., 2017) to the size of $\lambda N^{O}$. Then
the mean distance between the remaining candidates is calculated to get
$D_{v}$. As the feature distribution in the environment does not dramatically
change every frame. LCS is only triggered to update $D_{v}$ when the size of
the grouping results differ a lot ($20\%$ in practice) from $\lambda N^{O}$.

###  4.4 LiDAR keyframe selection and maintenance

We select a keyframe when the system’s translation or orientation changes over
a certain threshold. Thus, the keyframe database consists of keyframes
distributed in time or space. The points in the keyframe are added to the
local map stored in ikdtree (Xu et al., 2022). Due to the length limitation,
this paper only focuses on LIO without loop closure.

##  5 Experimental Validation of the System

We demonstrate the performance of HCTO via the public wearable sensing dataset
(WHU-Helmet (Li et al., 2023)) and in-house wearable sensing datasets. We
compare our method with three other state-of-the-art (SOTA) methods. First is
Fast-Lio2 (Xu et al., 2022), which uses filter-based state estimation with the
ikdtree managing the whole map points in an incremental way. Second is DLIO
(Chen et al., 2023), which is a direct LIO using continuous-time motion
undistortion. Third is SLICT, which estimates the system states using CTO.
Fourth is CLINS (Lv et al., 2021), which enables fuse high-frequency and
asynchronous sensor data effectively. Video recordings of experiments could be
found on the project page of HCTO: https://github.com/kafeiyin00/HCTO.

###  5.1 Implementation

We implemented the proposed HCTO in C++ and Robot Operating System (ROS). The
order of the B-Spline is set to 4. The time segment length $\Delta t$ for the
B-Spline is set to 0.05. The decay factor $\epsilon$ for the stochastic-greedy
is set to 0.1. The size of the selected correspondences $N^{O}$ is set to 500.
Grouping factor $\lambda$ is set to 2 to get 1000 ($\lambda N^{O}$)
correspondence groups in each LiDAR frame.

###  5.2 Public WHU-Helmet Dataset

Figure 7:  Localization error analysis of the seq-subway-station in WHU-
Helmet. The first row is the trajectory of HCTO. The second row is the
localization errors in different axes of HCTO.  Figure 8:  The point cloud
maps constructed by the proposed HCTO in WHU-Helmet Datasets. (a) Car park
sequence; (b) Subway station sequence.

The WHU-Helmet dataset was also collected by the helmet-based wearable system.
However, the main difference in the hardware configuration between WHU-Helmet
and the in-house built one is the LiDAR scanner. The Livox Avia is used in
WHU-Helmet, which has a longer observation range but with a limited sight view
($70.4^{\circ}\times 77.2^{\circ}$) compared to the Livox Mid360 (with a sight
view of $360^{\circ}\times 59^{\circ}$) used in the proposed system. The
limited view also poses a challenge for LIO systems in indoor environments. We
selected two sequences, the car park and subway station scenes, from the WHU-
Helmet dataset (Li et al., 2023) to evaluate the performance of the proposed
method. Both car parks and subway stations are common scenes, which needed
prior maps for the robot-based delivery. We use Absolute Translation Error
(ATE) (Grupp, 2017) as the metric to evaluate the performance of different
methods and list the results in Table 1. As the two scenarios contain too many
degenerated scenes like the corridor and elevator passage, most of the
existing methods diverged. The localization error of seq-subway-station in
WHU-Helmet for the proposed HCTO is plotted in Fig.7. The proposed HCTO still
achieved good performance in these degenerated scenes.

The point cloud maps constructed by the proposed HCTO for the subway station
and car park are illustrated in Fig.8. From visual inspection, the point cloud
maps constructed by the proposed HCTO achieved high accuracy and did not
diverge in the narrow corridors and elevator passage. The experimental results
demonstrated the potential for the proposed method in the prior map
construction in car parks and subway stations.

Table 1: ATE of HCTO and other methods on public WHU-Helmet Dataset (Unit
[m]). The best results are in BOLD, second best results are underlined, x
denotes diverge.

Dataset | Fast-Lio2 | DLIO | SLICT | CLINS | HCTO (Ours)
---|---|---|---|---|---
seq-car-park ($\sim 0.6km$, W) | x | x | x | 1.16 | 0.504
seq-subway-station ($\sim 0.8km$, W) | 3.55 | x | x | 3.94 | 1.410

  * 1.

W for Walking

###  5.3 In-House NTU-Campus Dataset

Figure 9: The NTU-Campus Datasets. Site 1 is an open roadside environment.
Site 2 is a multi-level indoor environment. The intensity rendered point
clouds are overlapped on the satellite image of NTU.  Figure 10: Ground truth
collection using Terrestrial Laser Scanning (TLS) of site 2 in the NTU campus
dataset. (a) Setup of control points in the environment using optical prisms.
(b) The geodetic network for TLS data registration. (c) The obtained
centimeter-level ground truth map.  Figure 11:  Visualization of the point
clouds generated by HCTO in NTU campus. (a) Side view of a random LiDAR frame
in Site 1. (b) Side view of a random LiDAR frame Site 2. Figure 12:
Visualization of feature selection in NTU campus dataset. (a) Side view of a
random frame in Site 1. (b) Side view of a random frame in Site 2.

The NTU-Campus dataset, containing two study sites in Nanyang Technological
University as shown in Fig.9, was collected using the in-house built compact
helmet system. Site 1 is an open-road environment that covers an area of about
$500m\times 300m$. Site 2 is a multi-level indoor environment that covers an
area of $400m\times 100m$. The two study sites represent common environments
for robot delivery. The ground truth is constructed in a similar manner to the
Newer College Dataset (Ramezani et al., 2020). We utilized the centimeter-
level terrestrial laser scanner (Leica MS60) to obtain the prior map at the
study site. To guarantee the accuracy of the prior map, we first evenly set a
lot of optical prisms as the control points in the environment as shown in
Fig.10 (a). Then a standard geodetic network adjustment (Teunissen, 1985) is
applied for the terrestrial laser scanning registration as shown in Fig.10
(b). The ground truth centimeter-level point clouds map of Site 2 is shown in
Fig.10 (c). Each LiDAR scan from the wearable sensing system was registered to
the prior map to obtain the ground truth pose.

Figure 13: Localization error analysis of the seq-02 in NTU-Campus site 1. (a)
The trajectories from different methods. (b),(c) and (d) The localization
errors in different axes.

In site 1, the operator collected three data sequences (seq-01, seq-02, and
seq-03) under varying degrees of motion: walking (about $1.5m/s$), running
(about $2.5m/s$), and a combination of walking and running, respectively. The
trajectory length of each sequence is about $1500m$. When the operator was
running, the data sequence contained more vibration than the walking sequence.
Visualization of the point clouds generated by HCTO in Site 1 is shown in
Fig.11 (a). The feature selection result of a random LiDAR frame is visualized
in Fig.12 (a). It could be seen that 500 good features are kept from 4825
features, which guarantees real-time performance.

Similar to previous experiments, we used ATE as the metric to evaluate the
accuracy of different methods. The trajectories and localization errors from
different methods are plotted in Fig.13. The final results are listed in Table
2. As the operator moved more gently in the first sequence, most methods
achieved better results compared to the other two sequences. As the operator
ran to collect the seq-02, all the methods suffered from the vibration. Table
2 also illustrated that the proposed HCTO achieved the best performance for
all three sequences in site 1.

Figure 14: Localization error analysis of the seq-01 in NTU-Campus site 2. (a)
The trajectories from different methods. (b),(c) and (d) The localization
errors in different axes.

In site 2, the operator walked (about $1.5m/s$) to collect data sequence
(seq-01) in a very complicated indoor environment, the Nanyang Auditorium in
NTU, which contains multiple levels and degenerated corridor scenes.
Visualization of the point clouds generated by HCTO in Site 1 is shown in
Fig.11 (b). The feature selection result of a random LiDAR frame is visualized
in Fig.12 (b). The trajectories and localization errors from different methods
are plotted in Fig.14. From the results, it can be found that the proposed
HCTO achieved trajectory accuracy, especially in the Z direction. The APE of
the trajectory errors were also listed in Table 2. Due to the narrow corridors
in the environment, SLICT diverged and did not get a valid result.

In site 2, we also evaluated the quality of different methods by visual
inspection of the walls and ground constructed in the point map. We have
checked the ground and the walls in the real scenes were level and coincident
with the gravity direction, respectively. The visual comparison between point
clouds generated by different methods is illustrated in Fig.15. From the
visual inspection, only the point clouds generated by the proposed HCTO
achieved the leveling of the ground and straight-up walls, which is vital for
a prior map used for robot delivery. Otherwise, the robot can not achieve the
correct goal in a bent prior map. We also disabled the CVP factor and feature
selection model in HCTO and output the point clouds, which also have little
attitude drift from the visual inspection. The main reason for the bent map
constructed by the LIO methods is that the unevenly distributed
correspondences and degenerated scenes made the LIO systems converge to the
local minimal solution and lose the observation of attitude, especially under
the high vibration. However, the proposed HCTO used the CVP factor to maintain
the correct attitude and select good features to balance the unevenly
distributed correspondences, thus achieving the best performance. It also
should be noted that due to the field of view limitation of the MiD360 on the
helmet, the ground may not be observed in the LiDAR frame. Thus, the strategy
using ground points to restrict the drift (Shan and Englot, 2018) can be
difficult to be directly applied to the helmet system.

Figure 15: Visual comparison of the different methods in NTU Campus site 2.
The point clouds are rendered by height to have better visualization of the
drift in attitude and z direction.  Table 2: ATE of HCTO and other methods on
in-house NTU-Campus Site (Unit [m]). The best results are in BOLD, second best
results are underlined. X denotes diverge.

Dataset | Fast-Lio2 | DLIO | SLICT | CLINS | HCTO (Ours)
---|---|---|---|---|---
site1-seq-01 ($\sim 1.5km$, W) | 5.103 | 5.437 | 4.383 | 5.462 | 2.059
site1-seq-02 ($\sim 1.5km$, R) | 5.262 | 5.503 | 5.547 | 6.337 | 3.446
site1-seq-03 ($\sim 1.5km$, W&R) | 5.228 | 5.759 | 4.452 | 3.406 | 3.105
site2-seq-01 ($\sim 1.0km$, W) | 2.528 | 2.548 | x | 1.823 | 0.551

  * 1.

W for walking; R for running; W&R for a combination of walking and running

###  5.4 Degenerated Scenes of Multi-level Apartment

Figure 16: Prior map constructed using the proposed HCTO. (a) Outside view of
a four-floor apartment. The operator ran to collect data floor by floor going
through the staircase, which is a degenerated scene. (b) Inside view of the
apartment. The long corridor of each floor is also challenging for the LIO
system.

Apartments in hospitals and nursing homes are typical scenes that need robot
delivery. The apartments usually contain a lot of degenerated scenes like
staircases and long corridors, which pose challenges especially when the
platform is under shaking. We validated our system in a multi-level apartment
from outside to indoor as shown in Fig.16. The operator ran to collect data
from the first floor to the top floor through the staircase as shown in Fig.16
(a). As for the inside of the apartment, the operator walked through the long
corridor as shown in Fig.16 (b). Most existing LIO systems diverged when the
operator ran into the staircase or walked in the corridor. However, the
proposed HCTO still achieved pretty good results. It should be noted that we
only took about 15 minutes to construct the prior map for the multi-level
apartment using our wearable system, which is much more efficient and cost-
effective than the standard survey solution (Dong et al., 2020).

###  5.5 Time performance of HCTO

Figure 17: The time performance analysis in NTU-Campus site 2.

We analyze the computation load of HCTO on the NTU-Campus site 2 in Fig.17.
The experiment was run on a computer with an Intel Core I9-I2900 CPU. On
average, it took 72.16 ms ($\Delta t_{sum}$) to complete one LiDAR frame
procession, in which the pure IMU processing and LiDAR undistortion took 9.17
ms ($\Delta t_{IMU+undist}$), the LiDAR feature association took 7.34 ms
($\Delta t_{matching}$), the feature selection and state estimation took 55.65
ms ($\Delta t_{solve}$), which guaranteed the real-time performance (10 Hz) of
the proposed system. We recorded the memory usage and time performance for all
the sequences, which can be found on the project page of HCTO.

###  5.6 Ablation study of HCTO-based LIO

Table 3: ATE in different helmet-based sequences with ground truth (Unit [m]).
The results with increased ATE compared with HCTO are in GREEN. The results
with decreased ATE compared with HCTO are in RED. x denotes diverge.

Dataset | Sequence | HCTO (w/o HB) | HCTO (w/o CVP) | HCTO (w/o FS) | HCTO
---|---|---|---|---|---
WHU-Helmet | seq-car-park ($\sim 0.6km$, W) | x (+$\infty$) | 0.801 (+0.297) | 0.688 (+0.113) | 0.504
seq-subway-station ($\sim 0.8km$, W) | 1.832 (+0.422) | 1.644 (+0.234) | 2.751 (+1.341) | 1.410
NTU-Campus | site1-seq-01 ($\sim 1.5km$, W) | 5.575 (+3.516) | 2.819 (+0.760) | 2.013 (-0.046) | 2.059
site1-seq-02 ($\sim 1.5km$, R) | x (+$\infty$) | 3.580 (+0.134) | 3.312 (-0.134) | 3.446
site1-seq-03 ($\sim 1.5km$, W&R) | 4.603 (+1.498) | 3.315 (+0.210) | 2.893 (-0.212) | 3.105

  * 1.

W for walking; R for running; W&R for a combination of walking and running

  * 2.

HB for hybrid IMU factors; CVP for constant velocity factor; FS for feature
selection.

To investigate the contribution of each design feature in HCTO’s performance,
we set different configurations of HCTO and compare the results on the helmet-
based dataset with ground truth trajectories, and listed the results in
Table.3. We disabled the hybrid IMU factors and use the original IMU factors
(same as the LFP) for all motion states, whose results were listed in the
first column. We disabled the CVP factors and listed the results in the second
column. At last, we disabled the feature selection module and retained all the
correspondences for optimization without considering the time performance,
whose results are listed in the third column.

The first column of Table. 3 illustrates that using the hybrid IMU factors
could suppress the vibration effect. The high-frequency part can not be fitted
by the B-Spline trajectory, which may cause a large drift or even divergence.
The second column of Table. 3 illustrates that CVP factor could limit the
drift. From the results of NTU-Campus dataset, the CVP factor got higher
performance improvement when the operator was walking. Because the CVP period
may not exist when the operator was running. The third column of Table. 3
illustrated that the feature selection module achieved performance improvement
in degenerated scenes in the WHU-Helmet dataset. It should be noted that HCTO
(w/o FS) could not perform in real-time as it retained all the
correspondences.

In summary, we conclude that the hybrid IMU factor is effective in ensuring
robustness towards high vibration. The CVP factor can noticeably reduce the
drift, and the feature selection scheme can be very effective in degenerative
cases. In cases where feature selection reduces the accuracy, it is only a
minor decrease that can be justified by the significant improvement in real-
time performance.

##  6 Conclusion

This paper proposed HCTO, an optimality-aware LIO with hybrid continuous-time
optimization achieving significant accuracy and robustness for compact
wearable mapping systems under high vibration conditions and degenerated
environments. Extensive experiments on head-mounted WMS datasets were
conducted to evaluate the performance of our system. The results demonstrated
significant advantages of HCTO over state-of-the-art methods on a helmet-based
WMS. Compared with existing SOTA LIO methods, the gains of HCTO in robustness
and accuracy are due to constructing different types of IMU factors for
different human motion states and optimal design for the LiDAR
correspondences. The proposed compact wearable mapping system is used to
collect prior maps for robot-based ”last-mile” delivery in complex indoor
environments.

There remain many possibilities for the extension of HCTO. For one, the
motion-based factor for state estimation is simple and not fully explored.
More efficient motion-based factors and activity-based loop information can be
made by learning-based strategies (Zhou et al., 2023a; Wang et al., 2022).
Moreover, we will explore the possibility of integrating HCTO into other
platforms (e.g., UAV, robot dog) by utilizing motion dynamics (Bauersfeld et
al., 2021; Buchanan et al., 2022b).

##  7 Acknowledgment

This research is supported by the National Research Foundation, Singapore
under its Medium Sized Center for Advanced Robotics Technology Innovation.

##  8 Declaration of Interests

The authors declare that they have no known competing financial interests or
personal relationships that could have appeared to influence the work reported
in this paper.

## References

  * Attard et al. (2018) Attard, L., Debono, C.J., Valentino, G., Di Castro, M., 2018\.  Tunnel inspection using photogrammetric techniques and image processing: A review.  ISPRS journal of photogrammetry and remote sensing 144, 180–188.
  * Bauersfeld et al. (2021) Bauersfeld, L., Kaufmann, E., Foehn, P., Sun, S., Scaramuzza, D., 2021.  Neurobem: Hybrid aerodynamic quadrotor model.  arXiv preprint arXiv:2106.08015 .
  * Beauregard (2006) Beauregard, S., 2006.  A helmet-mounted pedestrian dead reckoning system, in: 3rd International Forum on Applied Wearable Computing 2006, VDE. pp. 1–11.
  * Buchanan et al. (2022a) Buchanan, R., Agrawal, V., Camurri, M., Dellaert, F., Fallon, M., 2022a.  Deep imu bias inference for robust visual-inertial odometry with factor graphs.  IEEE Robotics and Automation Letters 8, 41–48.
  * Buchanan et al. (2022b) Buchanan, R., Camurri, M., Dellaert, F., Fallon, M., 2022b.  Learning inertial odometry for dynamic legged robot state estimation, in: Conference on Robot Learning, PMLR. pp. 1575–1584.
  * Cadena et al. (2016) Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., Reid, I., Leonard, J.J., 2016\.  Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age.  IEEE Transactions on robotics 32, 1309–1332.
  * Chen et al. (2023) Chen, K., Nemiroff, R., Lopez, B.T., 2023.  Direct lidar-inertial odometry: Lightweight lio with continuous-time motion correction.  2023 IEEE International Conference on Robotics and Automation (ICRA) , 3983–3989doi:10.1109/ICRA48891.2023.10160508.
  * Cioffi et al. (2022) Cioffi, G., Cieslewski, T., Scaramuzza, D., 2022.  Continuous-time vs. discrete-time vision-based slam: A comparative study.  IEEE Robotics and Automation Letters 7, 2399–2406.
  * Clark et al. (2017) Clark, R., Wang, S., Wen, H., Markham, A., Trigoni, N., 2017\.  Vinet: Visual-inertial odometry as a sequence-to-sequence learning problem, in: Proceedings of the AAAI Conference on Artificial Intelligence.
  * Dong et al. (2020) Dong, Z., Liang, F., Yang, B., Xu, Y., Zang, Y., Li, J., Wang, Y., Dai, W., Fan, H., Hyyppä, J., et al., 2020\.  Registration of large-scale terrestrial laser scanner point clouds: A review and benchmark.  ISPRS Journal of Photogrammetry and Remote Sensing 163, 327–342.
  * Grupp (2017) Grupp, M., 2017.  evo: Python package for the evaluation of odometry and slam.  https://github.com/MichaelGrupp/evo.
  * Jiao et al. (2021) Jiao, J., Zhu, Y., Ye, H., Huang, H., Yun, P., Jiang, L., Wang, L., Liu, M., 2021.  Greedy-based feature selection for efficient lidar slam, in: 2021 IEEE International Conference on Robotics and Automation (ICRA), IEEE. pp. 5222–5228.
  * Kachurka et al. (2021) Kachurka, V., Rault, B., Muñoz, F.I.I., Roussel, D., Bonardi, F., Didier, J.Y., Hadj-Abdelkader, H., Bouchafa, S., Alliez, P., Robin, M., 2021\.  Weco-slam: Wearable cooperative slam system for real-time indoor localization under challenging conditions.  IEEE Sensors Journal 22, 5122–5132.
  * Karam et al. (2021) Karam, S., Lehtola, V., Vosselman, G., 2021.  Simple loop closing for continuous 6dof lidar&imu graph slam with planar features for indoor environments.  ISPRS journal of photogrammetry and remote sensing 181, 413–426.
  * Khanna et al. (2017) Khanna, R., Elenberg, E., Dimakis, A., Negahban, S., Ghosh, J., 2017.  Scalable greedy feature selection via weak submodularity, in: Artificial Intelligence and Statistics, PMLR. pp. 1560–1568.
  * Li et al. (2023) Li, J., Wu, W., Yang, B., Zou, X., Yang, Y., Zhao, X., Dong, Z., 2023.  Whu-helmet: A helmet-based multi-sensor slam dataset for the evaluation of real-time 3d mapping in large-scale gnss-denied environments.  IEEE Transactions on Geoscience and Remote Sensing .
  * Lv et al. (2021) Lv, J., Hu, K., Xu, J., Liu, Y., Ma, X., Zuo, X., 2021.  Clins: Continuous-time trajectory estimation for lidar-inertial system, in: 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE. pp. 6657–6663.
  * Lv et al. (2023) Lv, J., Lang, X., Xu, J., Wang, M., Liu, Y., Zuo, X., 2023.  Continuous-time fixed-lag smoothing for lidar-inertial-camera slam.  IEEE/ASME Transactions on Mechatronics .
  * Mahony et al. (2008) Mahony, R., Hamel, T., Pflimlin, J.M., 2008.  Nonlinear complementary filters on the special orthogonal group.  IEEE Transactions on automatic control 53, 1203–1218.
  * Meyer et al. (2023) Meyer, T., Brunn, A., Stilla, U., 2023.  Geometric bim verification of indoor construction sites by photogrammetric point clouds and evidence theory.  ISPRS Journal of Photogrammetry and Remote Sensing 195, 432–445.
  * Mirzasoleiman et al. (2015) Mirzasoleiman, B., Badanidiyuru, A., Karbasi, A., Vondrák, J., Krause, A., 2015.  Lazier than lazy greedy, in: Proceedings of the AAAI Conference on Artificial Intelligence.
  * Nemhauser et al. (1978) Nemhauser, G.L., Wolsey, L.A., Fisher, M.L., 1978.  An analysis of approximations for maximizing submodular set functions—i.  Mathematical programming 14, 265–294.
  * Nguyen et al. (2023) Nguyen, T.M., Duberg, D., Jensfelt, P., Yuan, S., Xie, L., 2023.  Slict: Multi-input multi-scale surfel-based lidar-inertial continuous-time odometry and mapping.  IEEE Robotics and Automation Letters 8, 2102–2109.
  * Pan et al. (2021) Pan, Y., Xiao, P., He, Y., Shao, Z., Li, Z., 2021\.  Mulls: Versatile lidar slam via multi-metric linear least square, in: 2021 IEEE International Conference on Robotics and Automation (ICRA), IEEE. pp. 11633–11640.
  * Park et al. (2021) Park, C., Moghadam, P., Williams, J.L., Kim, S., Sridharan, S., Fookes, C., 2021\.  Elasticity meets continuous-time: Map-centric dense 3d lidar slam.  IEEE Transactions on Robotics 38, 978–997.
  * Pirkl et al. (2016) Pirkl, G., Hevesi, P., Amarislanov, O., Lukowicz, P., 2016\.  Smart helmet for construction site documentation and work support, in: Proceedings of the 2016 ACM International Joint Conference on Pervasive and Ubiquitous Computing: Adjunct, pp. 349–352.
  * Pukelsheim (2006) Pukelsheim, F., 2006.  Optimal design of experiments.  SIAM.
  * Qi et al. (2017) Qi, C.R., Yi, L., Su, H., Guibas, L.J., 2017.  Pointnet++: Deep hierarchical feature learning on point sets in a metric space.  Advances in neural information processing systems 30\.
  * Qin (1998) Qin, K., 1998.  General matrix representations for b-splines, in: Proceedings Pacific Graphics’ 98. Sixth Pacific Conference on Computer Graphics and Applications (Cat. No. 98EX208), IEEE. pp. 37–43.
  * Qin et al. (2018) Qin, T., Li, P., Shen, S., 2018.  Vins-mono: A robust and versatile monocular visual-inertial state estimator.  IEEE Transactions on Robotics 34, 1004–1020.
  * Ramezani et al. (2020) Ramezani, M., Wang, Y., Camurri, M., Wisth, D., Mattamala, M., Fallon, M., 2020\.  The newer college dataset: Handheld lidar, inertial and vision with ground truth, in: 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE. pp. 4353–4360.
  * Segal et al. (2009) Segal, A., Haehnel, D., Thrun, S., 2009.  Generalized-icp., in: Robotics: science and systems, Seattle, WA. p. 435\.
  * Shan and Englot (2018) Shan, T., Englot, B., 2018.  Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain, in: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE. pp. 4758–4765.
  * Shin and El-Sheimy (2004) Shin, E.H., El-Sheimy, N., 2004\.  An unscented kalman filter for in-motion alignment of low-cost imus, in: PLANS 2004. Position Location and Navigation Symposium (IEEE Cat. No. 04CH37556), IEEE. pp. 273–279.
  * Sidaoui et al. (2019) Sidaoui, A., Elhajj, I.H., Asmar, D., 2019.  Collaborative human augmented slam, in: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE. pp. 2131–2138.
  * Sommer et al. (2020) Sommer, C., Usenko, V., Schubert, D., Demmel, N., Cremers, D., 2020.  Efficient derivative computation for cumulative b-splines on lie groups, in: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 11148–11156.
  * Tavares (1966) Tavares, S., 1966.  A comparison of integration and low-pass filtering.  IEEE Transactions on Instrumentation and Measurement 15, 33–38.
  * Teunissen (1985) Teunissen, P., 1985.  Quality control in geodetic networks, in: Optimization and design of geodetic networks. Springer, pp. 526–547.
  * Wang et al. (2022) Wang, Y., Cheng, H., Meng, M.Q.H., 2022.  A2dio: Attention-driven deep inertial odometry for pedestrian localization based on 6d imu, in: 2022 International Conference on Robotics and Automation (ICRA), IEEE. pp. 819–825.
  * Wang et al. (2019) Wang, Y., Chernyshoff, A., Shkel, A.M., 2019.  Study on estimation errors in zupt-aided pedestrian inertial navigation due to imu noises.  IEEE Transactions on Aerospace and Electronic Systems 56, 2280–2291.
  * Wen et al. (2022) Wen, M., Dai, Y., Chen, T., Zhao, C., Zhang, J., Wang, D., 2022.  A robust sidewalk navigation method for mobile robots based on sparse semantic point cloud, in: 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE. pp. 7841–7846.
  * Xu et al. (2022) Xu, W., Cai, Y., He, D., Lin, J., Zhang, F., 2022.  Fast-lio2: Fast direct lidar-inertial odometry.  IEEE Transactions on Robotics 38, 2053–2073.
  * Yiğit et al. (2023) Yiğit, A.Y., Hamal, S.N.G., Yakar, M., Ulvi, A., 2023\.  Investigation and implementation of new technology wearable mobile laser scanning (wmls) in transition to an intelligent geospatial cadastral information system.  Sustainability 15, 7159\.
  * Zhang et al. (2019) Zhang, D., Gong, Z., Chen, Y., Zelek, J., Li, J., 2019\.  Slam-based multi-sensor backpack lidar systems in gnss-denied environments, in: IGARSS 2019-2019 IEEE International Geoscience and Remote Sensing Symposium, IEEE. pp. 8984–8987.
  * Zhang and Singh (2014) Zhang, J., Singh, S., 2014.  Loam: Lidar odometry and mapping in real-time., in: Robotics: Science and systems, Berkeley, CA. pp. 1–9.
  * Zhao and Vela (2020) Zhao, Y., Vela, P.A., 2020.  Good feature matching: Toward accurate, robust vo/vslam with low latency.  IEEE Transactions on Robotics 36, 657–675.
  * Zhou et al. (2023a) Zhou, B., Li, C., Chen, S., Xie, D., Yu, M., Li, Q., 2023a.  Asl-slam: A lidar slam with activity semantics-based loop closure.  IEEE Sensors Journal .
  * Zhou et al. (2023b) Zhou, B., Mo, H., Tang, S., Zhang, X., Li, Q., 2023b.  Backpack lidar-based slam with multiple ground constraints for multistory indoor mapping.  IEEE Transactions on Geoscience and Remote Sensing 61, 1–16.