# ReLoc-PDR: Visual Relocalization Enhanced Pedestrian Dead Reckoning via
Graph Optimization

Zongyang Chen  Xianfei Pan and Changhao Chen This work is supported by
National Natural Science Foundation of China (NSFC) under the Grant 62103427,
62073331, 62103430. (Zongyang Chen and Xianfei Pan are co-firstauthors.)
(Corresponding author: Changhao Chen.)The authors are with the College of
Intelligence Science and Technology, National University of Defense
Technology, Changsha 410073, China (e-mail<EMAIL_ADDRESS><EMAIL_ADDRESS>changhao.chen66@outlook.com).

###### Abstract

Accurately and reliably positioning pedestrians in satellite-denied conditions
remains a significant challenge. Pedestrian dead reckoning (PDR) is commonly
employed to estimate pedestrian location using low-cost inertial sensor.
However, PDR is susceptible to drift due to sensor noise, incorrect step
detection, and inaccurate stride length estimation. This work proposes ReLoc-
PDR, a fusion framework combining PDR and visual relocalization using graph
optimization. ReLoc-PDR leverages time-correlated visual observations and
learned descriptors to achieve robust positioning in visually-degraded
environments. A graph optimization-based fusion mechanism with the Tukey
kernel effectively corrects cumulative errors and mitigates the impact of
abnormal visual observations. Real-world experiments demonstrate that our
ReLoc-PDR surpasses representative methods in accuracy and robustness,
achieving accurte and robust pedestrian positioning results using only a
smartphone in challenging environments such as less-textured corridors and
dark nighttime scenarios.

Visual relocalization, pedestrian inertial navigation, factor graph,
incremental smoothing, sensor fusion, smartphone

##  1 Introduction

Robust and accurate indoor pedestrian navigation plays a crucial role in
enabling various location-based services (LBS). The determination of
pedestrian location in satellite-denied environments is a fundamental
requirement for numerous applications, including emergency rescue operations,
path guidance systems, and augmented reality experiences [1, 2]. The existing
indoor positioning solutions relying on the deployment of dedicated
infrastructures are susceptible to signal interference and non-line-of-sight
(NLOS) conditions, and their widespread deployment can be prohibitively
expensive.

In contrast to infrastructure-based positioning methods, pedestrian dead
reckoning (PDR) relies on only inertial data to estimate pedestrian location,
providing a relatively robust approach in various environments. However, due
to the inherent noise in inertial sensors, PDR is susceptible to trajectory
drift over long-term positioning. To mitigate this issue, researchers have
explored the combination of PDR with additional positioning methods, such as
Ultra-Wideband (UWB), WiFi, Bluetooth, among others [3, 4, 5]. These methods
have demonstrated impressive results in indoor localization. However, they
often necessitate the presence of extra infrastructure and require rigorous
calibration procedures in advance.

The pursuit of a low-cost, robust, and self-contained positioning system is of
great importance to flexible and resiliant pedestrian navigation. Visual
relocalization, which estimates the 6 degree-of-freedom (DoF) pose of a query
image against an existing 3D map model, holds potential for achieving drift-
free global localization using only a camera. Considering that smartphones
commonly are with built-in cameras, utilizing image-based localization methods
on mobile devices becomes a viable approach. However, the challenge lies in
the susceptibility of image-based relocalization to environmental factors such
as changes in lighting conditions and scene dynamics. Existing 3D structure-
based visual localization methods [6, 7, 8, 9] are computationally demanding
and fail to provide real-time pose output. Furthermore, the positioning
accuracy of visual relocalization significantly deteriorates in challenging
environments due to the scarcity of recognizable features.

Considering the continuity and autonomy advantages of PDR, combining visual
relocalization with PDR serves as a complementary approach. This integration
allows for the correction of accumulated errors in PDR using visual
relocalization results, while PDR improves the continuity and real-time
performance of trajectory estimation. Several recent studies have begun
exploring this direction [10, 11, 12, 13]. Existing methods primarily employ
dynamic weighting strategies to loosely integrate PDR with visual
relocalization. However, these approaches may lack robustness in challenging
environments and can result in significant trajectory inconsistencies due to
the interference caused by abnormal visual observations.

To tackle these challenges, this work presents ReLoc-PDR, a robust framework
for pedestrian inertial positioning aided by visual relocalization. ReLoc-PDR
leverages recent advancements in deep learning-based feature extraction [14]
and graph optimization [15] to ensure reliable visual feature matching and
robust localization. By integrating these techniques, our method effectively
mitigates the risk of visual relocalization failure, enhancing the system’s
robustness in visually degraded environments. To fuse the pose results from
PDR and visual relocalization effectively, we design a graph optimization-
based fusion mechanism using the Tukey kernel. This mechanism facilitates
cumulative error correction and eliminates the impact of abnormal visual
observations on positioning accuracy. As a result, the ReLoc-PDR system
exhibits stability and reliability. Real-world experiments were conducted to
evaluate the performance of the proposed method. The results demonstrate the
efficacy of our approach in various challenging environments, including
texture-less areas and nighttime outdoor scenarios.

Our contributions can be summarized as follows:

  * •

We propose ReLoc-PDR, a robust pedestrian positioning system that effectively
integrates Pedestrian Dead Reckoning (PDR) and visual relocalization to
mitigate positioning drifts.

  * •

We design a robust visual relocalization pipeline that leverages learned
global descriptors for image retrieval and learned local feature matching. It
enhances the robustness of the positioning system, particularly in visually
degraded scenes where traditional methods may struggle.

  * •

We introduce a pose fusion mechanism by incorporating the Tukey kernel into
graph optimization that facilitates cumulative error correction. It
effectively eliminates the impact of abnormal visual observations on
positioning accuracy, ensuring the stability and reliability of the ReLoc-PDR
system.

Figure 1: The system architecture of the proposed visual relocalization-aided
PDR method.

##  2 Related Work

###  2.1 Pedestrian Dead Reckoning

Pedestrian dead reckoning (PDR) relies on measurements obtained from the
built-in sensors of smartphones to detect human gait events and estimate step
length and heading angle for pedestrian positioning [1, 16]. However, PDR
alone is prone to cumulative error over time, resulting in inaccurate
positioning, particularly over long distances. To address this limitation,
previous studies have attempted to integrate PDR with other absolute
localization technologies such as Wi-Fi, Bluetooth, and Ultra-Wideband (UWB).
These integration approaches aim to periodically correct the accumulated error
of PDR by incorporating absolute position information [3, 4, 5]. Despite their
potential benefits, these methods are heavily reliant on infrastructure
availability and can be susceptible to changes in the physical environment.
Factors such as variations in signal strength, infrastructure coverage, and
environmental conditions may impact the accuracy and reliability of these
infrastructure-dependent approaches.

The integration of pedestrian dead reckoning (PDR) with visual relocalization
has emerged as a promising research direction for achieving self-contained and
highly accurate positioning using smartphones, without the need for additional
infrastructure. This research area has gained increasing attention in recent
years. Elloumi et al. [17] conducted a comparative study between inertial and
vision-based methods for indoor pedestrian localization using smartphones.
However, they did not explore the integration of these approaches to further
enhance the performance of the localization system. In another study, a
refined indoor localization framework was proposed in [10], which combines
image-based localization with PDR to improve pedestrian trajectory estimation.
Wang et al. [18] introduced a vision-aided PDR localization system that
integrates visual and PDR measurements into a unified graph model.
Furthermore, a novel indoor localization method called V-PDR was proposed in
[11], which integrates image retrieval and PDR using a weighted average
strategy. This approach successfully reduces the accumulated error of PDR. Shu
et al. [12] employed a dynamic fusion strategy that integrates PDR and image-
based localization (IBL) based on the number of inliers in the IBL process.
Their approach enables continuous and accurate 3D location estimation for
long-term tracking using smartphones. Additionally, in [13], a multimodal
fusion algorithm was proposed that loosely couples PDR with visual
localization to correct cumulative errors in PDR results. However, despite
these advancements, the robustness of these approaches remains limited, and
the positioning accuracy may significantly degrade in visually challenging
environments. Further improvements are necessary to ensure reliable and
accurate positioning in such scenarios.

###  2.2 Visual Relocalization

Visual relocalization, also known as image-based localization, refers to the
task of estimating the precise 6 DOF camera pose of a query image within a
known map. This task can be approached using retrieval-based and structure-
based methods. Retrieval-based approaches [19, 20, 21, 22] estimate the pose
of the query image by leveraging the geolocation of the most similar image
retrieved from an image database. However, these methods often fall short in
terms of localization accuracy. Structure-based methods [6, 7, 8, 9, 10], rely
on establishing correspondences between 2D features in the query image and 3D
points in a Structure from Motion (SFM) model using local descriptors. To
handle large-scale scenes, these methods typically employ image retrieval as
an initial step, restricting the 2D-3D matching to the visible portion of the
query image [8, 9]. However, the robustness of traditional localization
methods is limited due to the insufficient invariance of handcrafted local
features. In recent years, CNN-based local features [23, 14, 24] have
exhibited impressive robustness against illumination variations and viewpoint
changes. These features have been employed to enhance localization accuracy.
For example, [8] presents a comprehensive pipeline for structure-based visual
relocalization, incorporating global image retrieval, local feature matching,
and pose estimation. However, this method may encounter challenges during the
image retrieval stage in scenes with significant appearance variations, as the
representation power of global features is limited. Additionally, these
methods often yield a low-frequency global position estimation and require
high-performance computing platforms.

##  3 Visual ReLocalization Enhanced Pedestrian Dead Reckoning

###  3.1 System Design

####  3.1.1 System Overview

The framework of our ReLoc-PDR is depicted in Figure 1, comprising three
primary modules: inertial sensor-based Pedestrian Dead Reckoning (PDR), visual
relocalization, and graph optimization-based pose fusion. In this
architecture, the PDR algorithm is employed to compute the per-step pose using
the built-in inertial sensors of a commercially available smartphone. The
visual relocalization pipeline aims to accurately and robustly estimate the
pose of the triggered image captured by the smartphone camera. This estimation
is performed in relation to a pre-built 3D feature map, enabling global visual
observations that periodically correct the accumulated error in the PDR.
Finally, the pose fusion module integrates the pose results from PDR and
visual relocalization using graph optimization with a robust Tukey kernel.
This integration enables the continuous and smooth estimation of the
pedestrian’s position and trajectory during long-term walking.

####  3.1.2 Pedestrian Dead Reckoning

The PDR algorithm utilizes inertial data to estimate the pedestrian’s position
based on human motion characteristics. It consists of four main steps: step
detection, step length estimation, heading angle estimation, and position
update. Step detection relies on the repetitive pattern observed in the
accelerometer measurements during human walking. In our work, we employ a
multi-threshold peak detection algorithm to identify pedestrian gait:

$\left\\{\begin{array}[]{ll}\delta_{min}<|a_{m}-g|<\delta_{max}\\\ \Delta
T>\delta_{t}\end{array}\right.$ (1)

Here, $a_{m}$ represents the magnitude of acceleration, $\delta_{min}$ and
$\delta_{max}$ denote the minimum and maximum acceleration values within one
step, $g$ represents the gravity value, $\Delta T$ denotes the time interval
between adjacent peaks, and $\delta_{t}$ represents the minimum duration
threshold. Considering the complexity of pedestrian movement and the presence
of inevitable noises in low-cost MEMS inertial sensors, we employ a fourth-
order low-pass filter to preprocess the acceleration signal. This
preprocessing step enhances the quality of the gait characteristics obtained.
Furthermore, to eliminate false peaks caused by external interference, we
evaluate whether each peak is a local maximum value within a specific sliding
window size. This additional criterion helps ensure the accuracy of the
detected gait peaks.

The step length estimation component aims to calculate the distance covered by
a pedestrian in a single step, which is influenced by the pedestrian’s motion
states. In our approach, we employ the Weinberg model [25] to estimate the
pedestrian step length via:

$S_{k}=K\cdot\sqrt[4]{a_{z,max}-a_{z,min}}$ (2)

Here, $K$ represents the calibrated step-length coefficient, while $a_{z,max}$
and $a_{z,min}$ denote the maximum and minimum values of vertical acceleration
during step $k$, respectively.

Heading estimation is utilized to determine the walking direction of the
pedestrian. We leverage the gyroscope data from the smartphone’s built-in
Inertial Measurement Unit (IMU) to estimate the pedestrian’s heading angle.
This is achieved through an attitude update equation based on the median
integration method. Finally, the pedestrian’s position is updated based on
their previous position, incorporating the estimated step length and heading
angle. The updated position is as follows:

$\left[\begin{matrix}x_{k}\
y_{k}\end{matrix}\right]=\left[\begin{matrix}x_{k-1}\
y_{k-1}\end{matrix}\right]+\left[\begin{matrix}S_{k}\cos(\psi_{k})\
S_{k}\sin(\psi_{k})\end{matrix}\right]$ (3)

Here, $\psi_{k}$ represents the heading angle, while $x_{k}$ and $y_{k}$
indicate the pedestrian’s horizontal position at step $k$.

(a) Feature extraction

(b) 2D-3D correspondence

Figure 2: Local feature matching with learned feature via SuperPoint network.
(a) KeyPoints and descriptors extraction for query image and each reference
image in the scene cluster through SuperPoint network. (b) Establishing 2D-3D
correspondence between 2D features detected in the query image and 3D points
contained in the scene cluster depending on learned local feature matching.

####  3.1.3 Data Flow

As depicted in Figure 1, the ReLoc-PDR system takes in acceleration and
gyroscope data from the built-in inertial sensor of the smartphone to execute
the PDR algorithm on the mobile device, incrementally calculating the
pedestrian’s position $\mathbf{P}_{k}^{\text{PDR}}$ based on human motion
characteristics.

Once a step is successfully detected, a query image is triggered by capturing
a photo using the smartphone’s camera. Simultaneously, the captured query
image, along with the current pedestrian pose, is transmitted to the visual
relocalization module. Relocalization is achieved by matching the current
image against a pre-constructed 3D sparse feature map and estimating the 6-DOF
pose of the query image. Upon completing the visual relocalization pipeline,
the ReLocalization module returns the pose of the query image
$\mathbf{P}_{k}^{\text{ReLoc}}$ and the number of matched inliers $M$ to the
pose fusion module. Finally, the fusion module processes the results from the
reLocalization and PDR module, and performs pose graph optimization in a
separate thread.

###  3.2 Visual Relocalization with Learned Feature

Given that pedestrian navigation spans diverse indoor and outdoor
environments, the visual relocalization method needs to exhibit robustness in
handling various viewpoint and lighting conditions, including illumination,
weather, seasonal changes, and consistent performance in both indoor and
outdoor settings. The robustness of traditional retrieval-based [19, 20] or
structure-based relocalization [7, 6, 10, 13] methods is limited due to the
insufficient invariance of handcrafted designed features [26, 27], resulting
in reduced stability performance under conditions with low texture or poor
lighting. Recent advancements in deep neural network-based methods, such as
NetVLAD [21] and SuperPoint [14], have demonstrated superior capabilities in
image feature extraction, keypoint detection, and matching. These deep
learning-based approaches surpass traditional baselines like bag-of-words
[19], VLAD [20], and SIFT [26] in terms of robustness. Motivated by these
developments, we incorporate the advancements in learned global descriptors
and learned local features into the visual relocalization pipeline. This
integration enhances the robustness of the pedestrian positioning system in
visually degraded scenarios.

The flowchart illustrating the visual relocalization process with learned
features is presented in Figure 1. Given a pre-built 3D sparse feature map
database comprising images with known poses
$\\{\mathbf{I}^{r}_{i},\mathbf{T}^{r}_{i}\\}$, 3D point clouds
$\\{\mathbf{P}_{j}\\}$, and global feature descriptors
$\\{f(\mathbf{I}^{r}_{i})\\}$, the objective of visual relocalization is to
estimate the 6-DoF pose $\mathbf{T}^{q}$ of the query image $\mathbf{I}^{q}$
requested by the mobile device. To accomplish this, we propose a hierarchical
three-stage visual localization pipeline. Firstly, to identify similar
reference images in the database, we employ a learned global descriptor [21]
and exploit co-visible information encoded in the sparse feature model to
retrieve the top $N$ scene clusters $\mathcal{S}_{covis}$. Next, we establish
2D-3D correspondences between the query image $\mathbf{I}^{q}$ and the 3D
features ${\mathbf{P}_{j}}$ by leveraging 2D-2D matching with the learned
Superpoint feature [14]. Finally, we solve for the camera pose
$\mathbf{T}^{q}$ using the PnP [28] algorithm with a geometric consistency
check within a RANSAC [29] loop.

Figure 3: An illustration of the inertial-centric pose graph for fusing PDR
and visual relocalization results. The pose graph include two types of edges,
where the PDR edge connect the every two adjacent step nodes and is
continuously added into graph with pedestrian steps. However, not every state
node has the ReLoc edge, due to visual relocalization may fail or invalid in
challenging scenarios.

####  3.2.1 Learned Global Descriptor based Image Retrieval

Image retrieval aims to identify a subset of database images that share a
covisibility relationship with the query image. For the query image
$\mathbf{I}^{q}$, we initially extract learned global descriptors
$f(\mathbf{I}^{q})$ using the NetVLAD model (Remarkable CNN-based descriptor)
[21], which has demonstrated superior performance compared to non-learned
image representations [19, 20] in visual place recognition benchmarks.
Subsequently, we retrieve the top $k$ reference images
$\mathcal{\tilde{S}}={\tilde{\mathbf{I}}^{r}_{1},\tilde{\mathbf{I}}^{r}_{2},\cdots,\tilde{\mathbf{I}}^{r}_{k}}$
from the sparse features model based on the distance metric
$d(f(\mathbf{I}^{q}),f(\mathbf{I}^{r}))$. Rather than directly clustering
these retrieved reference images based on co-observed 3D points or performing
feature matching between the query image and retrieved reference images (as
done in HF-Net) [8], we expand the retrieval results by leveraging the co-
visible information embedded in the sparse features model. This approach helps
to search for additional potential 3D points and enlarge the view region.
Specifically, for each retrieved image $\tilde{\mathbf{I}}^{r}_{i}$, we search
for the $n$ database images in the Structure from Motion (SFM) model that
share the same observed 3D map points. We then merge these images to form a
scene cluster $\mathcal{S}_{i}$, with the size of each scene cluster limited
to 20 images to reduce computational costs. If a retrieved image
$\tilde{\mathbf{I}}^{r}_{i}$ already exists in a previous cluster, it is
disregarded to avoid redundant expansion of the retrieval results. Finally, we
obtain $N$ scene clusters
$\mathcal{S}_{covis}={\mathcal{S}_{1},\cdots,\mathcal{S}_{N}}$ and sort them
according to the size of the cluster set.

####  3.2.2 Learned Local Feature Matching

After obtaining the co-visible candidates $\mathcal{S}_{covis}$ for the query
image $\mathbf{I}^{q}$, our focus shifts to establishing 2D-3D correspondences
between the query image and 3D points based on 2D-2D matching utilizing the
SuperPoint learned feature [14]. SuperPoint is a self-supervised framework
that extracts both interest points and descriptors. It has been demonstrated
to generate denser and more accurate matching results compared to traditional
handcrafted detectors like SIFT [26] and ORB [27]. Moreover, SuperPoint
exhibits excellent robustness against illumination changes. For each scene
cluster $\mathcal{S}_{i}\in\mathcal{S}_{covis}$, we begin by extracting the
set of ketpoint positions $\mathbf{p}^{q}$ and associated local descriptors
$\mathbf{d}^{q}$ of the query image using the SuperPoint network, as
illustrated in Figure 2(a), jointing them
$\left(\mathbf{p}^{q},\mathbf{d}^{q}\right)$ as local features
$\mathbf{F}^{q}$. Meanwhile, the 2D features of reference images, represented
by $\mathbf{F}^{r}_{i}=\left(\mathbf{p}^{r}_{i},\mathbf{d}^{r}_{i}\right)$,
within the cluster set $\mathcal{S}_{i}$.

During the process of local feature matching, we employ nearest neighbor (NN)
matcher to establish 2D-2D matches between the query features $\mathbf{F}^{q}$
and each reference feature $\mathbf{F}^{r}_{i}$ through visual descriptors
similarity, followed by a Lowe’s ratio test [30] to filter out ambiguous
matched pairs. Finally, the 2D-3D correspondences are established by
retrieving the 3D points corresponding to the keypoints detected in the
database image based on the previous 2D-2D match results (Figure 2(b)). These
correspondences are crucial for solving the camera pose estimation.

####  3.2.3 Robust Pose Estimation

For the query image $\mathbf{I}^{q}$, we effectively establish the 2D-3D
correspondence between the 2D keypoints detected in the query image and the 3D
points within the scene cluster $\mathcal{S}_{i}$. Upon building this
correspondence, we proceed to solve the pose of the query image using the PnP
algorithm [28] within a RANSAC loop [29]. During the iterative solving
process, if the number of inliers in the estimated pose surpasses a predefined
threshold, the program will terminate early. This early termination condition
ensures efficiency and expedites the pose estimation procedure.

###  3.3 Integrating PDR and Visual Relocalization via Factor Graph
Optimization

In the process of visual and inertial pose fusion, ensuring the reliability of
observation information is crucial for maintaining the stability of the multi-
sensors positioning system. However, we observed that evaluating the quality
of visual relocalization solely based on the number of inliers [12, 13] is not
robust enough in visually degraded scenes. Examples of such scenes include
texture-less walls, areas with similar structures, and dark roadways. In these
scenarios, abnormal visual observations can still be introduced into the
positioning system, leading to a significant degradation in accuracy.

To address this challenge, we propose a robust pose fusion algorithm that
integrates Pedestrian Dead Reckoning (PDR) with visual relocalization using
graph optimization [15] and the Tukey robust kernel [31]. By incorporating the
Tukey kernel function into the pose graph, we can adaptively assess the impact
of current visual relocalization results on the system’s states. This adaptive
assessment dynamically determines the weight of the visual observation,
effectively mitigating the risk of visual relocalization failures.
Furthermore, unlike existing visual-inertial fusion methods [32], we employ an
inertial-centric data processing scheme. This scheme enables the dynamic
integration of visual relocalization observations into the graph. In the pose
graph, as illustrated in Figure 3, each step node serves as a vertex,
connecting to other vertices through two types of edges.

(a) Indoor 3D feature map

(b) Outdoor 3D feature map

Figure 4: The reconstruction results of 3D feature map with SuperPoint
features. (a) Indoor experimental environment map. (b) Outdoor experimental
environment map.

####  3.3.1 PDR Factor

As discussed in Section 3.1, the Pedestrian Dead Reckoning (PDR) algorithm
offers the advantages of autonomy and continuity, enabling high-accuracy
positioning within a short period. Upon successful detection of a step during
pedestrian walking, a PDR factor is established to connect it with the
previous step. This PDR factor represents the relative change in the
pedestrian’s position and is obtained directly from the PDR algorithm. Given
our inertial-sensor-centric pose graph, it continuously expands as pedestrian
steps are taken, rendering it relatively robust to environmental variations.
For step $k$ and its previous step $k-1$, the residual of the PDR factor is
formulated as follows:

$\displaystyle\mathbf{r}_{k,k-1}(\mathbf{z}_{k}^{\text{PDR}},\mathbf{x}_{k-1},\mathbf{x}_{k})$
$\displaystyle=\mathbf{z}_{k}^{\text{PDR}}-f\left(\mathbf{x}_{k-1},\mathbf{x}_{k}\right)$
(4)
$\displaystyle=\left[\begin{array}[]{c}S_{k}\cos(\psi_{k})-(x_{k}-x_{k-1})\\\
S_{k}\sin(\psi_{k})-(y_{k}-y_{k-1})\end{array}\right]$

Here, $\mathbf{r}_{k,k-1}(\cdot)$ represents the residual of the PDR factor.
$\mathbf{z}_{k}^{\text{PDR}}$ denotes the PDR observation, which corresponds
to the position increment from step $k$ to $k-1$. The state estimations at
steps $k-1$ and $k$ are denoted as $\mathbf{x}_{k-1}$ and $\mathbf{x}_{k}$,
respectively, after optimization.

####  3.3.2 Relocalization Factor

Before incorporating relocalization edges into the pose graph, a reliability
assessment is performed to mitigate the impact of visual relocalization
failures on positioning accuracy. In our approach, we utilize the number of
inliers as the criterion to determine the success of visual relocalization
results. If the number of inliers exceeds 25, we consider the relocalization
results reliable. In such cases, we add a ReLoc edge to the current state node
and subsequently perform incremental smoothing optimization. However, if the
number of inliers is below the threshold, we skip the pose graph optimization
step and rely solely on the previously optimized state and PDR estimation to
determine the current pedestrian position.

Assuming that the visual relocalization result is reliable at step $k$, the
residual of the relocalization factor is calculated using equation (5):

$\displaystyle\mathbf{r}_{k}(\mathbf{z}_{k}^{\text{ReLoc}},\mathbf{x}_{k})$
$\displaystyle=\mathbf{z}_{k}^{\text{ReLoc}}-\mathbf{x}_{k}$ (5)
$\displaystyle=[\mathbf{T}_{k}^{\text{q}}]_{\mathbf{p}_{xy}}-\mathbf{x}_{k}$

Here, $\mathbf{r}_{k}(\cdot)$ represents the residual of the relocalization
factor. $\mathbf{z}_{k}^{\text{ReLoc}}$ denotes the visual relocalization
observation, specifically the horizontal position $[\cdot]{\mathbf{p}_{xy}}$
of the camera pose. Additionally, the prior factor information
$\mathbf{r}_{0}$ is also provided by the visual relocalization result, which
aids in determining the initial position of the pedestrian before walking.

####  3.3.3 Incremental Smoothing Optimization for Poses Fusion

The set of states $\mathcal{X}$ comprises the state variables from the first
step to the $k$th step, represented as
$\mathcal{X}=\\{\mathbf{x}_{0},\mathbf{x}_{1},\cdots,\mathbf{x}_{k}\\}$. The
entire pose graph, consisting of PDR factors and ReLoc factors, is optimized
by minimizing the following cost function:

${\mathcal{X}^{*}}=\mathop{\arg\min}\limits_{\mathcal{X}}\left\\{{\sum_{k\in\mathcal{B}}{\left\|{\mathbf{r}_{k-1,k}}\right\|_{\mathbf{\Omega}_{k}^{\text{PDR}}}^{2}+\sum_{i\in\mathcal{L}}\rho\left(\left\|{\mathbf{r}_{i}}\right\|_{\mathbf{\Omega}_{i}^{\text{ReLoc}}}^{2}\right)}}\right\\}$
(6)
$\rho(x)=\left\\{\begin{array}[]{ll}\frac{1}{6}{\delta}^{2}\left(1-\left[1-(x/{\delta})^{2}\right]^{3}\right),&\text{if}\left|{x}\right|\leq\delta\\\
\frac{1}{6}{\delta}^{2},&\text{otherwise}\end{array}\right.$ (7)

Here, $\mathcal{B}$ is the set of all PDR factors, $\mathcal{L}$ is the set of
all ReLoc factors, and $\mathbf{\Omega}_{k}^{\text{PDR}}$ and
$\mathbf{\Omega}_{i}^{\text{ReLoc}}$ denote the corresponding covariance
matrices. To account for variations in the quality of relocalization results
under different visual conditions, we dynamically adjust the weight of the
ReLoc edges based on the number of inliers. Although the reliability
evaluation strategy based on the number of inliers has helped eliminate
incorrect visual relocalization results, we further incorporate the Tukey
robust kernel $\rho(\cdot)$ [31] to mitigate the potential impact of any
erroneous visual observations. The loss function of the Tukey robust kernel is
defined in equation (7). In contrast, we do not employ any kernel function for
the PDR edge, as PDR can achieve high-accuracy positioning within a short
period.

To achieve real-time pose optimization, we utilize an adaptive-lag smoother
called Incremental Smoothing and Mapping (iSAM2) [15]. Unlike batch optimizers
that repeatedly compute and update all historical states, iSAM2 dynamically
determines which historical states are affected by the current observations
and selectively optimizes and updates only those affected states. This
adaptive approach significantly reduces unnecessary computations, resulting in
near-optimal results comparable to batch graph optimization but at a lower
computational cost. For implementation, we employ the open-source GTSAM
library [33] to construct the factor graph and perform incremental smoothing
optimization. The use of GTSAM enables efficient construction and manipulation
of the factor graph, facilitating the real-time pose optimization process.

##  4 Experiments

In this section, we first present the experimental setup, encompassing the
necessary equipment and intricate details of the experiment. Subsequently, we
conduct comprehensive experiments to assess the robustness and accuracy of the
fusion positioning system proposed in this study. We evaluate our method
across three distinct environmental conditions, i.e. a textureless corridor,
overcast weather, and a dark roadway, each presenting unique visual
challenges.

###  4.1 Experimental Setup

In the experiment, a Xiaomi 10 smartphone was employed for both offline map
construction and online testing purposes. To reconstruct the 3D map model,
video sequences of the scene were captured using the smartphone at a frequency
of 30 Hz and a resolution of 1920x1080. These sequences were then downsampled
to obtain discrete database images. In this study, COLMAP [34], a structure-
from-motion (SFM) tool, was utilized for generating sparse SFM models. We made
certain modifications to adapt it to pedestrian navigation. Specifically, we
employed the NetVLAD [21] feature to retrieve the top 50 image matching pairs
for each database image, which were subsequently inputted into the COLMAP
pipeline to aid the image matching process. Additionally, we introduced a
scale estimation module into COLMAP to convert the 3D point cloud into real-
world scale, allowing for an understanding of the environment’s dimensions.
This approach involved using pre-placed artificial markers [35] with known
lengths to restore the 3D point cloud to a real-world scale. Finally, a new 3D
SFM model was constructed using keypoints detected by SuperPoint [14], based
on the hloc toolbox [8]. The resulting 3D SFM model, depicting both indoor and
outdoor environments, can be observed in Fig. 4.

During the testing phase, our system records IMU data at a rate of 100Hz and
captures image frames at 30Hz. However, the query image is triggered
specifically on the node where gait is detected during pedestrian walking. The
resolution of all query images is standardized to 600x800 pixels. To assess
the localization performance and robustness of the proposed method, we
conducted experiments in three distinct environments characterized by varying
visual challenges. These experiments involved holding a smartphone in each of
these environments.

###  4.2 Indoor Experiment in Textureless Corridor

As illustrated in Fig. 5, the initial experiment took place in an indoor
corridor and office environment, known for its visually challenging
characteristics such as walls with limited texture and the presence of moving
pedestrians. During this experiment, the participant navigated the corridor
while holding the smartphone and encountered multiple sharp turns along a pre-
determined path. These sharp turns have the potential to induce significant
heading drift. The experiment had a duration of approximately 355 seconds,
covering a total distance of approximately 240 meters.

Figure 5: The process of indoor experiment. The volunteer walked along a
preset route (red line) with holding a smartphone (Xiaomi 10).

To highlight the advantages of our method in indoor positioning environments,
we conducted a comparative analysis with three other approaches. The first
method examined was a pure inertial-based pedestrian navigation (PDR)
approach. The second approach involved VINS-Mono [32], which is a state-of-
the-art Visual-Inertial SLAM method known for its exceptional tracking
performance and competitive positioning accuracy. The third approach combined
PDR with visual localization using a dynamic weighting strategy [12, 13],
referred to as DW-PDR/vision.

Figure 6: Comparison of the trajectory estimated by different methods in textureless corridor. Table 1: Horizontal positioning error statistics in indoor experiment |  RMSE(m) |  Max Error(m) |  Loop Error(m)
---|---|---|---
PDR |  5.5951 |  8.4082 |  4.4419
VINS-Mono |  10.9209 |  30.7579 |  30.6570
DW-PDR/vision |  2.5575 |  12.4825 |  0.2274
Ours |  0.2079 |  0.4435 |  0.1955

Figure 6 illustrates the trajectory comparison among the different methods in
an indoor environment. The PDR algorithm demonstrates relatively smooth
overall trajectory; however, it suffers from trajectory drift due to
accumulated errors in heading estimation. The positioning accuracy of VINS-
Mono is severely compromised in indoor environments due to the presence of
less-textured features and the utilization of low-quality sensors in mobile
devices, leading to inferior performance compared to the pure inertial-based
PDR. Leveraging stronger geometric constraints from a prior 3D map and the
robustness of learned features, the visual relocalization method achieves
accurate positioning in most cases. By combining PDR with the visual
relocalization results, the cumulative errors in PDR can be effectively
corrected using the visual measurements. However, it is evident that the
trajectory of DW-PDR/vision lacks robustness and smoothness, often
experiencing significant discontinuities due to interference from abnormal
visual relocalization observations in visually similar scenarios. In contrast,
our proposed method exhibits robustness against abnormal visual relocalization
observations. It dynamically assesses the reliability of visual relocalization
results using the Tukey robust kernel, enabling adaptive decision-making
regarding the reliance on either PDR or global visual observations.
Additionally, our method leverages the incremental smoothing iSAM2 algorithm
to provide a smoother and more continuous trajectory compared to other
approaches, as depicted in the locally enlarged region in Figure 6.

Table 1 provides the statistical analysis of horizontal positioning errors for
the different methods. As obtaining the ground truth of pedestrian trajectory
is not feasible, we adopt artificial marker points with known positions as a
reference benchmark. The results presented in Table 1 indicate that our
proposed method achieves superior positioning accuracy in complex indoor
environments, effectively reducing the root mean square error (RMSE) of the
pure inertial-based PDR by 96.3%. VINS-Mono exhibits the lowest accuracy due
to its degraded tracking performance in indoor environments characterized by
less-textured conditions. The DW-PDR/vision method experiences a maximum error
of 12.4825 m, attributed to the influence of abnormal visual observations. In
comparison, our method surpasses the performance of DW-PDR/vision by improving
the positioning accuracy by 91.9% in terms of RMSE and reducing the maximum
error to 0.4435 m. These results effectively demonstrate the robustness of our
method in challenging indoor environments.

Figure 7: The handheld experimental equipment, integrating the smartphone (mi
10) and solid-state LiDAR (livox AVIA) on one platform. Figure 8: The
trajectories estimated by different methods in outdoor overcast environments.

###  4.3 Outdoor Experiment in Overcast Weather Condition

To evaluate the positioning performance of our proposed method in challenging
outdoor environments, a second experiment was conducted along a route
encircling a hill. This test encompassed dynamic vehicle movements, variations
in cloudy weather, and changes in scene structure, all of which have the
potential to impede visual tracking. The experiment had a walking duration of
approximately 230 seconds, covering a total path length of approximately 225
meters. As the satellite signal was obstructed by tall trees and buildings, it
was not feasible to obtain a reference positioning trajectory from the RTK
recorder. Instead, we employed the trajectory estimation of FAST-LIO2 [36],
one of the state-of-the-art LiDAR-inertial odometers, as a reference value. To
synchronize the timestamps between the smartphone-based results and the LiDAR-
based output, the volunteer held the experimental device (Fig. 7) in an upward
position to stimulate the accelerometer and produce a spike before walking. By
aligning the first peak of the smartphone acceleration data with the first
peak of the LiDAR’s built-in IMU acceleration data, we obtained the time
difference between them, achieving synchronization.

Figure 9: Distribution of horizontal position error comparison with the change of detected steps. Table 2: Comparison of positioning accuracy in outdoor overcast weather condition |  RMSE(m) |  Max Error(m) |  Loop Error(m)
---|---|---|---
PDR |  4.2875 |  5.5949 |  4.9342
VINS-Mono |  3.2201 |  5.0276 |  0.2182
DW-PDR/vision |  0.6605 |  4.7965 |  0.2036
Ours |  0.5618 |  1.0664 |  0.0984

Figure 8 displays the trajectory results of various algorithms in outdoor
cloudy environments. It is evident that our proposed method closely aligns
with the reference trajectory and exhibits a smooth trajectory without any
sudden jumps. The PDR algorithm, due to the inherent noise of inertial
sensors, significantly deviates from the ground truth. While VINS-Mono
performs reasonably well in outdoor environments, its positioning accuracy is
limited by the lower quality of mobile phone built-in sensors. Compared to
visual-inertial SLAM methods, the visual relocalization aided PDR methods
achieve superior trajectory estimation. However, the DW-PDR/vision approach
based on dynamic weighting strategy, despite achieving remarkable positioning
results, demonstrates noticeable trajectory jumps under abnormal visual
relocalization observations, a phenomenon not observed in our method. The
proposed optimization-based fusion positioning method effectively mitigates
the impact of erroneous visual observations on positioning accuracy through
the use of a robust kernel function, resulting in smoother and more robust
positioning outcomes. Furthermore, the distribution of horizontal positioning
errors with pedestrian steps is depicted in Figure 9. These results highlight
the superior performance of our method compared to other algorithms,
consistently providing accurate positioning results with errors of less than 1
m. By incorporating the visual relocalization results, our method
significantly reduces cumulative errors in PDR. In contrast, the positioning
accuracy of the DW-PDR/vision method degrades significantly due to the
interference of abnormal visual relocalization observations, underscoring the
robustness of our method in challenging environments.

Table 2 presents the positioning error statistics for different algorithms.
Our method exhibits the highest positioning accuracy and significantly reduces
the cumulative errors of PDR by 86.9%. While VINS-Mono achieves impressive
loop error reduction through loop-closing, its positioning accuracy does not
match the competitiveness of our method. In comparison to DW-PDR/vision, our
method outperforms it by demonstrating a 14.9% improvement in terms of the
RMSE metric. Moreover, our method exhibits a maximum error of only 1.0604 m,
whereas DW-PDR/vision shows a maximum error of 4.7975 m. These results
effectively illustrate the robustness of our proposed method against abnormal
disturbances.

Figure 10: Example query images for outdoor nighttime experiment. Figure 11:
The number of inliers comparison between SuperPoint feature and SIFT features
in nighttime environment.

###  4.4 Outdoor Experiment in Low-Light Condition

To further assess the robustness of our method in visually challenging
environments, we conducted a third experiment under outdoor nighttime
conditions. Figure 10 showcases an example of the nighttime images captured
using a mobile phone. The low lighting conditions during nighttime result in
poor image quality, presenting a significant challenge for visual tracking
methods. Traditional handcrafted feature descriptors exhibit limited
robustness in such challenging environments due to their poor invariance. In
this study, we introduced learned features [14] into our image-based pipeline.
These learned features generate denser and more accurate matches compared to
traditional methods like SIFT, as depicted in Figure 11. This integration
effectively enhances the reliability and continuity of conventional visual
methods.

Figure 12: The trajectory comparison of different methods in outdoor nighttime
environment. Figure 13: Distribution of horizontal position error comparison
with the change of detected steps. Figure 14: Cumulative distribution of
horizontal positioning errors across different methods.

Figure 12 displays the trajectory results of different approaches in an
outdoor nighttime environment. It is evident that our method consistently
provides continuous and accurate trajectory estimations, showcasing robustness
against severely degraded environments. The pure inertial-based PDR approach
yields acceptable results as inertial sensors are independent of visual cues.
However, its positioning error accumulates over time. In the nighttime
environment, VINS-Mono’s positioning results deviate significantly from the
reference trajectory due to the lack of sufficient features for accurate match
tracking. Although the incorporation of learned features improves the
robustness of positioning for nighttime query images to some extent, visual
relocalization still encounters numerous failures due to the limited
representation of local features. Figure 13 illustrates the horizontal
positioning errors with significant fluctuations observed in the DW-PDR/vision
method. This is attributed to relying solely on the number of inliers as a
criterion for evaluating reliability, which can introduce abnormal visual
results. The dynamic weighting fusion strategy fails to eliminate the impact
of incorrect observations, resulting in estimated trajectories deviating
significantly from the reference path. In contrast, our method effectively
addresses the risk of visual relocalization failures and mitigates the impact
of abnormal observations on positioning accuracy by employing an optimization-
based fusion strategy. Additionally, our method achieves globally smooth and
drift-free trajectory estimations through incremental smoothing optimization.

Figure 14 displays the cumulative distribution of horizontal positioning
errors, demonstrating the superior performance of our method compared to other
algorithms. Table 3 provides statistical analysis results of horizontal
positioning errors in outdoor nighttime conditions. Our method improves the
positioning accuracy of PDR by 77.0% and reduces the maximum error from 4.3309
m to 1.3854 m. Remarkably, even in the dark lighting environment, our method
outperforms VINS-Mono in terms of positioning accuracy, which is surprising
given the latter’s reliance on visual information. The positioning accuracy of
DW-PDR/vision is significantly affected by abnormal visual observations.
Conversely, our method consistently achieves accurate and remarkable
positioning results, with a loop error of only 0.5152 m, underscoring its
robustness against visually challenging environments.

Table 3: Comparison of positioning accuracy in outdoor nighttime experiment |  RMSE(m) |  Max Error(m) |  Loop Error(m)
---|---|---|---
PDR |  2.4589 |  4.3309 |  2.8320
VINS-Mono |  5.9169 |  9.6373 |  9.5584
DW-PDR/vision |  7.6334 |  55.1899 |  48.5443
Ours |  0.5655 |  1.3854 |  0.5152

##  5 Conclusion

To achieve self-reliant and robust pedestrian navigation using a smartphone in
visually challenging environments, our work proposes, ReLoc-PDR, a robust
pedestrian positioning framework that integrates Pedestrian Dead Reckoning
(PDR) and visual relocalization based on incremental smoothing optimization.
Considering the visual degradation problem in environments with weak textures
and varying illumination, we introduce a visual relocalization pipeline using
the learned features from deep neural network instead of traditional
handcrafted features. This effectively establishes 2D-3D correspondences with
higher inlier rates, enhancing the robustness of the pedestrian localization
system. Furthermore, we propose an optimization-based fusion strategy that
couples the PDR and visual relocalization poses into a graph model. This
fusion strategy is accompanied by the use of the Tukey robust kernel, which
helps eliminate the risk of abnormal visual observations. Experimental results
demonstrate the effectiveness of our ReLoc-PDR in various challenging
environments, including corridors with limited texture, overcast weather
conditions, and dark nighttime scenarios. The proposed ReLoc-PDR method
achieves accurate and smooth trajectory estimation, continuously providing
pedestrian positions at a decimeter-level accuracy and a high frequency.

## References

  * [1] Qu Wang, Meixia Fu, Jianquan Wang, Haiyong Luo, Lei Sun, Zhangchao Ma, Wei Li, Chaoyi Zhang, Rong Huang, Xianda Li, Zhuqing Jiang, and Qilian Liang.  Recent advances in pedestrian inertial navigation based on smartphone: A review.  IEEE Sensors Journal, 22(23):22319–22343, 2022.
  * [2] Francesca De Cillis, Luca Faramondi, Federica Inderst, Stefano Marsella, Marcello Marzoli, Federica Pascucci, and Roberto Setola.  Hybrid indoor positioning system for first responders.  IEEE Transactions on Systems, Man, and Cybernetics: Systems, 50(2):468–479, 2020.
  * [3] Shuli Guo, Yitong Zhang, Xinzhe Gui, and Lina Han.  An improved pdr/uwb integrated system for indoor navigation applications.  IEEE Sensors Journal, 20(14):8046–8061, 2020.
  * [4] Yuan Zhuang and Naser El-Sheimy.  Tightly-coupled integration of wifi and mems sensors on handheld devices for indoor pedestrian navigation.  IEEE Sensors Journal, 16(1):224–234, 2016.
  * [5] Thai-Mai Thi Dinh, Ngoc-Son Duong, and Kumbesan Sandrasegaran.  Smartphone-based indoor positioning using ble ibeacon and reliable lightweight fingerprint map.  IEEE Sensors Journal, 20(17):10283–10294, 2020.
  * [6] Torsten Sattler, Bastian Leibe, and Leif Kobbelt.  Efficient & effective prioritized matching for large-scale image-based localization.  IEEE Transactions on Pattern Analysis and Machine Intelligence, 39(9):1744–1756, 2017.
  * [7] Linus Svärm, Olof Enqvist, Fredrik Kahl, and Magnus Oskarsson.  City-scale localization for cameras with known vertical direction.  IEEE Transactions on Pattern Analysis and Machine Intelligence, 39(7):1455–1461, 2017.
  * [8] Paul-Edouard Sarlin, Cesar Cadena, Roland Siegwart, and Marcin Dymczyk.  From coarse to fine: Robust hierarchical localization at large scale.  In 2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), pages 12708–12717, 2019.
  * [9] Hajime Taira, Masatoshi Okutomi, Torsten Sattler, Mircea Cimpoi, Marc Pollefeys, Josef Sivic, Tomas Pajdla, and Akihiko Torii.  Inloc: Indoor visual localization with dense matching and view synthesis.  IEEE Transactions on Pattern Analysis and Machine Intelligence, 43(4):1293–1307, 2021.
  * [10] Yan Zhou, Xianwei Zheng, Ruizhi Chen, Hanjiang Xiong, and Sheng Guo.  Image-based localization aided indoor pedestrian trajectory estimation using smartphones.  Sensors, 18(1), 2018.
  * [11] Jiuchao Qian, Yuhao Cheng, Rendong Ying, and Peilin Liu.  A novel indoor localization method based on image retrieval and dead reckoning.  Applied Sciences, 10(11), 2020.
  * [12] Mingcong Shu, Guoliang Chen, Zhenghua Zhang, and Lei Xu.  Accurate indoor 3d location based on mems/vision by using a smartphone.  In 2022 IEEE 12th International Conference on Indoor Positioning and Indoor Navigation (IPIN), pages 1–8, 2022.
  * [13] Mingcong Shu, Guoliang Chen, and Zhenghua Zhang.  Efficient image-based indoor localization with mems aid on the mobile device.  ISPRS Journal of Photogrammetry and Remote Sensing, 185:85–110, 2022.
  * [14] Daniel DeTone, Tomasz Malisiewicz, and Andrew Rabinovich.  Superpoint: Self-supervised interest point detection and description.  In 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition Workshops (CVPRW), pages 337–33712, 2018.
  * [15] Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert.  isam2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering.  In 2011 IEEE International Conference on Robotics and Automation, pages 3281–3288, 2011.
  * [16] Yingbiao Yao, Lei Pan, Wei Fen, Xiaorong Xu, Xuesong Liang, and Xin Xu.  A robust step detection and stride length estimation for pedestrian dead reckoning using a smartphone.  IEEE Sensors Journal, 20(17):9685–9697, 2020.
  * [17] Wael Elloumi, Abdelhakim Latoui, Raphaël Canals, Aladine Chetouani, and Sylvie Treuillet.  Indoor pedestrian localization with a smartphone: A comparison of inertial and vision-based methods.  IEEE Sensors Journal, 16(13):5376–5388, 2016.
  * [18] Zhouyang Wang, Erwu Liu, and Rui Wang.  A vision-aided pdr localization system.  In 2020 Information Communication Technologies Conference (ICTC), pages 98–102, 2020.
  * [19] Dorian Gálvez-López and J. D. Tardós.  Bags of binary words for fast place recognition in image sequences.  IEEE Transactions on Robotics, 28(5):1188–1197, October 2012.
  * [20] Hervé Jégou, Matthijs Douze, Cordelia Schmid, and Patrick Pérez.  Aggregating local descriptors into a compact image representation.  In 2010 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, pages 3304–3311, 2010.
  * [21] Relja Arandjelović, Petr Gronat, Akihiko Torii, Tomas Pajdla, and Josef Sivic.  Netvlad: Cnn architecture for weakly supervised place recognition.  IEEE Transactions on Pattern Analysis and Machine Intelligence, 40(6):1437–1451, 2018.
  * [22] Xiwu Zhang, Lei Wang, and Yan Su.  Visual place recognition: A survey from deep learning perspective.  Pattern Recognition, 113:107760, 2021.
  * [23] Yuki Ono, Eduard Trulls, Pascal Fua, and Kwang Moo Yi.  Lf-net: Learning local features from images.  In Proceedings of the 32nd International Conference on Neural Information Processing Systems, NIPS’18, page 6237–6247, Red Hook, NY, USA, 2018. Curran Associates Inc.
  * [24] Mihai Dusmanu, Ignacio Rocco, Tomas Pajdla, Marc Pollefeys, Josef Sivic, Akihiko Torii, and Torsten Sattler.  D2-Net: A Trainable CNN for Joint Detection and Description of Local Features.  In Proceedings of the 2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2019.
  * [25] Harvey Weinberg.  Using the adxl202 in pedometer and personal navigation applications.  Analog Devices AN-602 application note, 2(2):1–6, 2002.
  * [26] David G Lowe.  Distinctive image features from scale-invariant keypoints.  International journal of computer vision, 60:91–110, 2004.
  * [27] Ethan Rublee, Vincent Rabaud, Kurt Konolige, and Gary Bradski.  Orb: An efficient alternative to sift or surf.  In 2011 International Conference on Computer Vision, pages 2564–2571, 2011.
  * [28] Laurent Kneip, Davide Scaramuzza, and Roland Siegwart.  A novel parametrization of the perspective-three-point problem for a direct computation of absolute camera position and orientation.  In CVPR 2011, pages 2969–2976, 2011.
  * [29] Martin A. Fischler and Robert C. Bolles.  Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography.  Commun. ACM, 24(6):381–395, jun 1981.
  * [30] David G Lowe.  Distinctive image features from scale-invariant keypoints.  International journal of computer vision, 60:91–110, 2004.
  * [31] Zhengyou Zhang.  Parameter estimation techniques: A tutorial with application to conic fitting.  Image and Vision Computing, pages 59–76, 1997.
  * [32] Tong Qin, Peiliang Li, and Shaojie Shen.  Vins-mono: A robust and versatile monocular visual-inertial state estimator.  IEEE Transactions on Robotics, 34(4):1004–1020, 2018.
  * [33] Frank Dellaert and GTSAM Contributors.  borglab/gtsam, May 2022.
  * [34] Johannes L. Schönberger and Jan-Michael Frahm.  Structure-from-motion revisited.  In 2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR), pages 4104–4113, 2016.
  * [35] John Wang and Edwin Olson.  Apriltag 2: Efficient and robust fiducial detection.  In 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 4193–4198, 2016.
  * [36] Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, and Fu Zhang.  Fast-lio2: Fast direct lidar-inertial odometry.  IEEE Transactions on Robotics, 38(4):2053–2073, 2022.

[]Zongyang Chen received the B.S. degree from Fuzhou University, Fuzhou,
China, in 2021. He is currently pursuing the master’s degree with the National
University of Defense Technology, Changsha, China. His research interests
include pedestrian inertial navigation and visual–inertial integrated
navigation.

[]Xianfei Pan received the Ph.D. degree in control science and engineering
from the National University of Defense Technology, Changsha, China, in 2008.
Currently, he is a professor of the College of Intelligence Science and
Technology, National University of Defense Technology. His current research
interests include Inertial navigation system and indoor navigation system.

[]Changhao Chen obtained his Ph.D. degree at University of Oxford (UK), M.Eng.
degree at National University of Defense Technology (China), and B.Eng. degree
at Tongji University (China). Now he is a Lecturer at College of Intelligence
Science and Technology, National University of Defense Technology (China). His
research interest lies in robotics, computer vision and cyberphysical systems.