# Marker-based Visual SLAM leveraging Hierarchical Representations

Ali Tourani1, Hriday Bavle1, Jose Luis Sanchez-Lopez1, Rafael Muñoz Salinas2,
and Holger Voos1 1Authors are with the Automation and Robotics Research Group,
Interdisciplinary Centre for Security, Reliability, and Trust (SnT),
University of Luxembourg, Luxembourg. Holger Voos is also associated with the
Faculty of Science, Technology, and Medicine, University of Luxembourg,
Luxembourg. {ali.tourani, hriday.bavle, joseluis.sanchezlopez,
<EMAIL_ADDRESS>is with the Department of Computer Science and
Numerical Analysis, Rabanales Campus, University of Córdoba, Spain.
<EMAIL_ADDRESS>work was funded by the Institute of Advanced Studies
(IAS) of the University of Luxembourg (project TRANSCEND), the European
Commission Horizon2020 research and innovation program under the grant
agreement No 101017258 (SESAME), and the Luxembourg National Research Fund
(FNR) 5G-SKY project (ref. C19/IS/13713801).For the purpose of Open Access,
the author has applied a CC BY public copyright license to any Author Accepted
Manuscript version arising from this submission.The authors would like to
thank Deniz Işınsu Avşar and Jan P.F. Lagerwall, both from the Department of
Physics & Materials Science (DPHYMS) of the University of Luxembourg, for
their efforts and valuable remarks on employing invisible fiducial markers as
a futuristic scenario.

###### Abstract

Fiducial markers can encode rich information about the environment and can aid
Visual SLAM (VSLAM) approaches in reconstructing maps with practical semantic
information. Current marker-based VSLAM approaches mainly utilize markers for
improving feature detections in low-feature environments and/or for
incorporating loop closure constraints, generating only low-level geometric
maps of the environment prone to inaccuracies in complex environments. To
bridge this gap, this paper presents a VSLAM approach utilizing a monocular
camera along with fiducial markers to generate hierarchical representations of
the environment while improving the camera pose estimate. The proposed
approach detects semantic entities from the surroundings, including walls,
corridors, and rooms encoded within markers, and appropriately adds
topological constraints among them. Experimental results on a real-world
dataset collected with a robot demonstrate that the proposed approach
outperforms a traditional marker-based VSLAM baseline in terms of accuracy,
given the addition of new constraints while creating enhanced map
representations. Furthermore, it shows satisfactory results when comparing the
reconstructed map quality to the one reconstructed using a LiDAR SLAM
approach.

##  I Introduction

Visual SLAM (VSLAM) systems can employ a wide range of vision sensors, such as
monocular, stereo, Red Green Blue-Depth (RGB-D), omnidirectional, and event-
based cameras to estimate the environmental map while localizing the camera
[1]. The primary advantage of vision sensors is that they need low-cost
hardware to supply rich visual and semantic information from surroundings for
various tasks [2]. Semantic data, which refers to high-level information
acquired from the environment, make VSLAM tasks more robust and expand the
range of applications that can employ the reconstructed maps [1], [3]. For
instance, robots may need to identify objects in the scene for path planning
and dynamic object removal, which can be resolved using semantic data [4]. In
this regard, utilizing fiducial markers is one of the possible approaches to
encoding semantic information into the environment due to their rich and
better-defined features [5]. In addition, they can assist VSLAM frameworks by
providing accurate pose estimation due to their unique patterns, supplying
reliable features in low-texture environments, and enabling loop closure
detection based on marker identifiers. While most fiducial markers are visible
to humans and, thus, visually polluting the environment, we have recently
proposed novel invisible fiducial markers [6] that have the potential to be
seamlessly integrated into the environment. Nevertheless, for the sake of
simplicity, in this work, we use visible fiducial markers, leaving the use of
invisible fiducial markers as future works. Recent works such as [7] and [8]
propose VSLAM approaches using fiducial markers but do not encode them with
meaningful semantic information, creating purely geometric map representations
leading to inaccuracies in camera pose estimates and the generated
environmental map in the presence of complex environments.

Figure 1: The final reconstructed map of the environment using the proposed
method in a hierarchical representation. Accordingly, the primary map entities
are detected 3D points extracted from the environment and visited ArUco
markers. Various colors of the 3D points refer to distinct walls, and the line
connecting them to the cubes shows their belonging to rooms, including
corridors or four-wall rooms: a) the top view of the reconstructed map
represented in 2D, b) keypoints and robot trajectory records that lead to map
reconstruction.

To fully leverage the potential of fiducial markers in accurately identifying
both the semantic elements and their topological relationships while
generating accurate and meaningful maps, this paper proposes a VSLAM framework
for monocular cameras that utilizes the data encoded in fiducial markers for
enhanced map reconstruction, adding abstract semantic elements to the final
map along with their topological relationships. The system is built upon
UcoSLAM [7], which employs ArUco [9] markers and visual keypoints extracted
from natural landmarks reconstructing the geometric map of the environment
using keypoints and markers. Additionally, inspired by S-Graphs [10, 11],
leveraging high-level hierarchical representations for localization and
mapping, the proposed approach adds extraordinary information in the form of
walls, corridors, and rooms considering the data encoded in the ArUco markers
attached to the environment. A sample map reconstructed by the proposed
approach and its hierarchical representation is demonstrated in Fig. 1.

Herewith, the main contributions of the paper are summarized below:

  * •

An extension of marker-based VSLAM to reconstruct environmental maps with
high-level semantic features, including walls, corridors, and rooms, encoded
within the markers,

  * •

The design of novel geometric constraints, namely marker-to-wall and wall-to-
room, to improve the map quality and reduce camera localization errors,

  * •

Validation of the proposed method using a real-world indoor dataset showing
improved performance over marker-based VSLAM baseline.

The rest of the paper is organized as follows: Section II explores previous
efforts in the VSLAM domain, targeting the usage of semantic data for improved
reconstructed maps. Section III introduces the proposed approach and its
various modules in detail. In Section IV, the evaluation criteria and
discussions over the effectiveness of the proposed method are provided. The
paper finally concludes in Section V.

##  II Related Works

VSLAM approaches have matured over the past years and many researchers
contributed to this community by proposing efficient solutions to unresolved
challenges in this area. The authors of this paper also presented a
comprehensive survey on diverse VSLAM state-of-the-art works and studied their
trade-offs in [12]. Accordingly, many recent approaches focus on improving the
front-end thread of the VSLAM systems for better pose estimation and visual
odometry [13]. Authors in [7] proposed UcoSLAM that employs both natural and
artificial landmarks in the environment to reconstruct the map. It works in
keypoints-only, markers-only, and mixed modes and improves the global map
builder and loop closure detector modules in case ArUco markers are found in
the camera’s field of view. Another marker-based approach titled TagSLAM [8]
was proposed by Pfrommer and Daniilidis, which uses the capability of AprilTag
[14] fiducial markers for SLAM tasks. Although the proposed approaches perform
well in many scenarios, they cannot obtain any semantic information from the
environment for an improved map representation. Gomez-Ojeda et al. [15]
introduced PL-SLAM, which uses Oriented FAST and Rotated BRIEF (ORB) and Line
Segment Detector (LSD) algorithms to extract visual features of points and
lines in stereo vision cameras. However, this approach is computationally
intensive in feature tracking and extracting data from structural lines. Bruno
and Colombini [16] proposed LIFT-SLAM, which integrates deep learning-based
and conventional geometry-based feature descriptors. Despite the robust
feature detection and tracking performance, the method uses an un-optimized
Convolutional Neural Network (CNN), which leads to almost real-time execution.

Utilizing semantic data is another topic that attracted great attention in
recent years. Gonzalez et al. [17] presented S3LAM, a monocular approach based
on ORB-SLAM 2.0 [18] that uses semantic segmentation of generic objects and
structures. Their approach fits a plane using the triangulated 3D points for
each a priori planar cluster and uses a CNN-based panoptic segmentation
framework titled Detectron2 [19]. Likewise, YOLO-SLAM [20] is another CNN-
based approach that couples geometric constraints with the Darknet19-YOLOv3
object detector to generate semantic information. However, these solutions may
suffer from performance degradation in recognizing small or non-regular
objects and planes. QuadricSLAM [21] is another approach that models objects
found in the environment by employing quadric representations as object
descriptions and estimates quadric parameters using object detection bounding
boxes. This monocular VSLAM represents an object’s size, position, and
orientation for map reconstruction. The main challenge of QuadricSLAM is the
initialization stage, where a variety of viewing angles of the object is
required. Hosseinzadeh et al. [22] proposed another monocular VSLAM system
that inserts visited objects detected by a deep learning-based object detector
and the dominant structure of the scene using a planar landmark detector to
the map. Despite the robust performance, estimating the prior shape of the
objects leads to high computational complexity. Another semantic VSLAM
approach titled Blitz-SLAM [23] combines the original masks and depth
information of the objects and eliminates noise blocks formed by dynamic
objects. Authors in [24] proposed a parallel framework titled SOLO-SLAM, which
uses semantic attributes of map points and geometric constraints to filter
dynamic objects and reconstruct an improved map. The main challenge here is
losing a great deal of information in low-dynamic scenarios. MISD-SLAM [25] is
another semantic approach equipped with real-time instance segmentation based
on a pre-trained CNN and dynamic feature removal. However, the algorithm
removes objects that are static only in some frames.

In contrast with the mentioned works, the proposed approach creates a map of
the environment by utilizing semantic data encoded within makers instead of
employing any object detector. Accordingly, walls and room constraints are
estimated based on mathematical modeling obtained from keypoints and markers
data, which does not significantly affect the performance of the system.
Additionally, the accuracy of the final reconstructed map of the environment
is comparable with the one from Light Detection And Ranging (LiDAR)-based
approaches, demonstrating the proposed method’s potential.

##  III Proposed Method

Figure 2: The overall pipeline of the proposed approach. The main
contributions of the proposed method are adding the Semantic Analyzer module
and modifying the output of the Tracker to provide proper feed to it.
Moreover, the Global/Local Optimizer modules have also been modified for
better map reconstruction.

The pipeline of the proposed method is depicted in Fig. 2. Accordingly, the
frames captured by a monocular camera are passed to the system for processing.
The tracker module detects ORB visual features and ArUco markers as the
primary inputs of the system. Keyframes are selected and tracked over time,
and the output contains 3D points, detected markers, walls, and rooms visited
in the scene. The semantic analyzer module performs wall and room detection
tasks using the pose of the markers detected in the tracking stage.
Additionally, a mapper module handles adding new vertices and edges to the map
and improves the final map for an optimized reconstruction. Finally, a marker-
based loop detector module checks whether the markers in the current frame
have been seen before and triggers the global optimizer to enhance the
reconstructed map.

In order to develop a VSLAM framework with richer reconstructed maps, the
proposed approach utilizes UcoSLAM as the baseline and modifies its components
to be empowered with a semantic data analysis procedure. The mentioned
modifications enable the detection of walls and different types of rooms as
two semantic concepts in the environment using ArUco markers. Furthermore, as
these fiducial markers are used for camera pose estimation, they can store
additional encoded semantic data, which makes them an ideal alternative
according to their unique identification properties. This method also aspires
to employ visual data to represent the environment and the robot’s pose in a
single optimizable graph with an approach comparable to S-Graphs [26]. Thus,
instead of odometry readings and extracting planar surfaces using LiDAR scans
introduced by S-Graphs and S-Graphs+ [11], the proposed approach calculates
the location of walls and room variations using the fiducial markers attached
to them.

###  III-A Overview

The pipeline of the proposed method at time $t$ can be referred to four main
coordinate systems: the odometry frame of reference $O$, the camera coordinate
system $C_{t}$, the marker coordinate system $M_{t}$, and the global
coordinate system $G_{t}$. As the primary sensor of the system, a monocular
camera acquires a set $\mathbf{F}=\\{\mathbf{f}\\}$ of frames
$\mathbf{f}=\displaystyle\\{t,\mathbf{T},\mathbf{\delta}\\}$, where
$\mathbf{T}\in SE(3)$ is the camera’s pose obtained from the transformation of
$C_{t}$ to $G_{t}$, and $\delta$ is the set of camera intrinsic parameters. By
processing each camera frame using ORB keypoint detector and feature
extractor, a set of keypoints $\mathbf{g}=\\{l,u,\mathbf{d}\\}$ are extracted
in which $l$ is the subsampling level of the image, $u$ is the pixel
coordinates for upsampling w.r.t. the first level, and
$\mathbf{d}=(d_{1}...d_{n})|d_{i}\in[0,1]$ is the descriptor vector with
length $n$. Accordingly, the final constructed map of the environment
$\mathbf{E}$ will be represented as:

$\mathbf{E}=\\{\mathbf{K},\mathbf{P},\mathbf{M},\mathbf{W},\mathbf{R}\\}$ (1)

where $\mathbf{K}=\\{k\\}\subset\mathbf{F}$ is the set of keyframes and
$\mathbf{P}=\\{\mathbf{p}\\}$ represents the set of feature points
$\mathbf{p}=\\{\mathbf{x},\mathbf{v},\mathbf{\hat{d}}\\}$ extracted from the
environment with their corresponding 3D positions
$\mathbf{x}\in\mathbb{R}^{3}$, viewing direction
$\mathbf{v}\in\mathbb{R}^{3}$, and descriptor $\mathbf{\hat{d}}$.
Additionally, $\mathbf{M}=\\{\mathbf{m}\\}$ is the set of ArUco markers
detected in the environment, in which each marker
$\mathbf{m}=\\{s,\mathbf{p},\mathbf{cc}\\}$ holds marker size (i.e., length)
$s\in\mathbb{R}$, marker pose $\mathbf{p}\in SE(3)$ calculated from $M_{t}$ to
$G_{t}$, and corner coordinates
$\mathbf{cc}=(c_{1}...c_{4})|c_{i}\in\mathbb{R}^{3}$ values. The set of walls
detected from the environment is represented by $\mathbf{W}=\\{\mathbf{w}\\}$,
in which each wall $\mathbf{w}=\\{\mathbf{q},\mathbf{m_{w}}\\}$ holds the wall
equation $\mathbf{q}\in\mathbb{R}^{4}$ and the attached markers list
$\mathbf{m_{w}}\subset\mathbf{M}=(m_{1}...m_{n})|m_{i}\in\mathbb{N}$ where
$m_{i}$ represents ArUco marker-id. Similarly, $\mathbf{R}=\\{\mathbf{r}\\}$
refers to the set of rooms found in the environment, where each room
$\mathbf{r}=\\{\mathbf{r_{c}},\mathbf{r_{w}}\\}$ contains the room center
point $\mathbf{r_{c}}\in\mathbb{R}^{3}$ and the wall list
$\mathbf{r_{w}}\subset\mathbf{W}=(w_{1}...w_{n})|w_{i}\in\mathbb{N}$ that
comprise the room.

###  III-B Semantic Entities

The proposed approach adds two semantic entities to construct a richer map,
including walls and rooms. As detecting the mentioned entities is accomplished
differently, the procedures are discussed in detail in this section.

Walls. The procedure to find the walls as plane-shaped entities on which the
markers and their surrounding features are located is predominantly done using
the pose information provided by detected ArUco markers. This approach assumes
all fiducial markers are placed directly on the walls, not other elements such
as radiators or drawer units. Thus, the equations of the walls are obtained
based on the attached markers’ poses. The process of detecting walls in the
scene occurs whenever a marker is being visited in a recent keyframe, making
detecting and mapping the walls at a given time more efficient.

To add walls to the final map, each wall plane $\boldsymbol{w}_{i}$ is
extracted in the global coordinate system
${}^{G}\boldsymbol{w}_{i}=\begin{bmatrix}{}^{G}\boldsymbol{n}_{i}&{}^{G}d\end{bmatrix}$
with a normal vector
${}^{G}\boldsymbol{n}_{i}=\begin{bmatrix}n_{x}&n_{y}&n_{z}\end{bmatrix}^{T}$.
The vertex node of the wall is factored in the graph as
${{}^{G}}\boldsymbol{w_{i}}=[^{G}\phi,^{G}\theta,{{}^{G}}d]$, where
${}^{M}\phi$ and ${}^{M}\theta$ refer to the azimuth and elevation of the wall
in $G_{t}$. For each marker ${}^{G}\boldsymbol{m}_{i}$ attached to the wall
${}^{G}\boldsymbol{w}_{i}$ cost function can be defined as:

$c_{w_{i}}(^{G}\boldsymbol{w}_{i},^{G}\boldsymbol{m}_{i})=\|[^{M}\delta\phi_{w_{i_{m_{i}}}},^{M}\delta\theta_{w_{i_{m_{i}}}},^{M}d_{w_{i}}]^{T}\|^{2}_{\mathbf{\Lambda}_{\boldsymbol{\tilde{w}}_{i}}}$
(2)

where ${}^{M}\delta\phi_{w_{i_{m_{i}}}}$ difference between the azimuth angle
of the wall $w_{i}$ and its marker $m_{i}$ converted to its marker frame
$M_{i}$, ${}^{M}\delta\theta_{w_{i_{m_{i}}}}$ is the difference in the
elevation angles, while ${}^{M}d_{w_{i}}$ being the perpendicular distance
between the wall and the marker, which should be zero the given marker-wall
pair.

Rooms. Another semantic entity that has been considered in this work is the
room. Since perceiving a room can be difficult due to various configurations
and structures, the proposed approach employs the data encoded in ArUco
markers attached to the room’s walls to detect the mentioned semantic entity.
In this regard, a dictionary containing the rooms in the environment and the
fiducial markers attached to their walls are provided to feed the framework.
Note that the only information encoded in the dictionary is the marker-ids
corresponding to a room, and no additional pose information is required to be
encoded. The proposed approach presents a modified version of the room
segmentation process introduced in S-Graphs+ [11], where the markers play a
vital role. Hence, two room types titled ”two-wall room” and ”four-wall room”
have been considered in this work:

Two-wall Rooms (Corridors): In this case, only two parallel walls of a room
are labeled with fiducial markers. This scenario is proper for detecting
corridors or rooms with undetectable/unreachable walls in the scene.
Consequently, a room
${}^{G}\mathbf{r}_{x}=[{{}^{G}}{\mathbf{w}_{x_{a_{1}}}},{{}^{G}}{\mathbf{w}_{x_{b_{1}}}}]$
contains $x$-wall planes parallel to the $x$-axis while
${}^{G}\mathbf{r}_{y}=[{{}^{G}}{\mathbf{w}_{y_{a_{1}}}},{{}^{G}}{\mathbf{w}_{y_{b_{1}}}}]$
contains $y$-wall planes parallel planes to $y$-axis.

To compute the center of a two-wall room ${}^{G}\mathbf{r}_{x_{i}}$, the two
$x$-wall plane equations are utilized along with the center point
${{}^{G}}{\mathbf{c}_{i}}$ of the marker $\mathbf{m}_{i}$ as follows:

${{}^{G}}{\mathbf{k}_{x_{i}}}=\frac{1}{2}\big{[}\lvert{{{}^{G}}{d_{x_{a_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{x_{a_{1}}}}-{\lvert{{}^{G}}{d_{x_{b_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{x_{b_{1}}}}\big{]}+\lvert{{}^{G}}{{d_{x_{b_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{x_{b_{1}}}}$

$\displaystyle{{}^{G}}{\boldsymbol{\eta}_{x_{i}}}={{}^{G}}{\hat{\mathbf{k}}_{x_{i}}}+\big{[}{{}^{G}}{\mathbf{c}_{i}}-[\
{{}^{G}}{\mathbf{c}_{i}}\cdot{{}^{G}}{\hat{\mathbf{k}}_{x_{i}}}]\
\cdot\hat{{{}^{G}}{\mathbf{k}}_{x_{i}}}\big{]}$ (3)

where ${{}^{G}}{\boldsymbol{\eta}_{x_{i}}}$ is the center point of the two-
wall room ${}^{G}\mathbf{r}_{x_{i}}$ and ${{}^{G}}{\hat{\mathbf{k}}_{x_{i}}}$
is acquired from
${{}^{G}}{\hat{\mathbf{k}}_{x_{i}}}={{}^{G}}{\mathbf{k}_{x_{i}}}/\|{{}^{G}}{\mathbf{k}_{x_{i}}}\|$.
The center point ${{}^{G}}{\mathbf{c}_{i}}$ of the marker is obtained using
the marker pose in frame $G$. It should be noted that a two-wall room center
in the $y$ direction can be calculated analogously.

A two-wall room node is initialized using the room center, and the cost
function to minimize the two-wall room node and their corresponding wall
planes are defined below:

$c_{\boldsymbol{r}_{{x_{i}}}}({{}^{G}}{\boldsymbol{r}_{{x_{i}}}},\big{[}{{}^{G}}{\mathbf{w}_{x_{a_{1}}}},{{}^{G}}{\mathbf{w}_{x_{b_{1}}}},{{}^{G}}{\textbf{c}_{i}}\big{]})\\\
=\sum_{t=1,i=1}^{T,K}\|{{}^{G}}{\hat{\boldsymbol{\eta}}_{x_{i}}}-f({{}^{G}}{\tilde{\mathbf{w}}_{x_{a_{1}}}},{{}^{G}}{\tilde{\mathbf{w}}_{x_{b_{1}}}},{{}^{G}}{\textbf{c}_{i}})\|^{2}_{\boldsymbol{\Lambda}_{\boldsymbol{\tilde{\boldsymbol{r}}}_{i,t}}}$
(4)

where
$f({{}^{G}}{\tilde{\mathbf{w}}_{x_{a_{1}}}},{{}^{G}}{\tilde{\mathbf{w}}_{x_{b_{1}}}},{{}^{G}}{\textbf{c}_{i}})$
maps the wall planes to the center point of the room using Eq. 3.

Four-wall Rooms: This scenario refers to rooms with four walls labeled with
fiducial markers. In this regard, a four-wall room contains four wall planes
as
${}^{G}\mathbf{r}_{i}=[{{}^{G}}{\mathbf{w}_{x_{a_{1}}}}{{}^{G}}{\mathbf{w}_{x_{b_{1}}}}{{}^{G}}{\mathbf{w}_{y_{a_{1}}}}{{}^{G}}{\mathbf{w}_{y_{b_{1}}}}]$
forming the room. The center point of this variant of rooms can be computed
using the equation below:

${{}^{G}}{\mathbf{q}_{x_{i}}}=\frac{1}{2}\big{[}\lvert{{{}^{G}}{d_{x_{a_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{x_{a_{1}}}}-{\lvert{{}^{G}}{d_{x_{b_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{x_{b_{1}}}}\big{]}+\lvert{{{}^{G}}{d_{x_{b_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{x_{b_{1}}}}$

${{}^{G}}{\mathbf{q}_{y_{i}}}=\frac{1}{2}\big{[}\lvert{{{}^{G}}{d_{y_{a_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{y_{a_{1}}}}-{\lvert{{}^{G}}{d_{y_{b_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{y_{b_{1}}}}\big{]}+\lvert{{{}^{G}}{d_{y_{b_{1}}}}\rvert}\cdot{{}^{G}}{\mathbf{n}_{y_{b_{1}}}}$

$\displaystyle{{}^{G}}{\boldsymbol{\rho}_{i}}={{}^{G}}{\boldsymbol{q}_{x_{i}}}+{{}^{G}}{\boldsymbol{q}_{y_{i}}}$
(5)

where ${{}^{G}}{\boldsymbol{\rho}_{i}}$ is the center point of the four-wall
room ${}^{G}\mathbf{r}_{i}$. It should also be noted that Eq. 5 holds true
when $\lvert d_{x_{1}}\rvert>\lvert d_{x_{2}}\rvert$. The cost function to
minimize four-wall room nodes and their corresponding wall plane set is
similar to a two-wall room but with minor differences:

$c_{\boldsymbol{\rho}}({{}^{G}}{\boldsymbol{\rho}},\big{[}{{}^{G}}{\mathbf{w}_{x_{a_{i}}}},{{}^{G}}{\mathbf{w}_{x_{b_{i}}}},{{}^{G}}{\mathbf{w}_{y_{a_{i}}}},{{}^{G}}{\mathbf{w}_{y_{b_{i}}}}\big{]})\\\
=\sum_{t=1,i=1}^{T,S}\|{{}^{G}}{\hat{\boldsymbol{\rho}}_{i}}-{{f({{}^{G}}{\tilde{\mathbf{w}}_{x_{a_{i}}}},{{}^{G}}{\tilde{\mathbf{w}}_{x_{b_{i}}}},{{}^{G}}{\tilde{\mathbf{w}}_{y_{a_{i}}}},{{}^{G}}{\tilde{\mathbf{w}}_{y_{b_{i}}}})}}\|^{2}_{\mathbf{\Lambda}_{\boldsymbol{\tilde{\boldsymbol{\rho}}}_{i,t}}}$
(6)

where
$f({{}^{G}}{\tilde{\mathbf{w}}_{x_{a_{i}}}},{{}^{G}}{\tilde{\mathbf{w}}_{x_{b_{i}}}},{{}^{G}}{\tilde{\mathbf{w}}_{y_{a_{i}}}},{{}^{G}}{\tilde{\mathbf{w}}_{y_{b_{i}}}})$
maps the four estimated wall planes to the center point of the four-wall room
using Eq. 5. The above cost function maintains the structural consistency
among the four walls forming the room.

###  III-C Final Graph

Fig. 3 depicts the structure of the final semantic graph produced by the
proposed approach. Accordingly, the keyframes extracted by the system are the
primary sources of information that contain both visual feature points with
their corresponding 3D coordinates and visited ArUco markers in the scene. It
should be noted that the constraint defined among the keyframes, feature
points, and fiducial markers is utilized for computing the odometry and for
detecting loop closure. The wall-marker constraints are incorporated using Eq.
2. The topmost level of the graph retains rooms detected in the environment
using the marker-ids and the walls that hold those markers with constraints
obtained following Eq. 4 for two-wall rooms and Eq. 6 for four-wall rooms.

Figure 3: The graph representation of the hierarchical architecture of the
proposed approach. In this regard, the map employs new semantic constraints,
including walls and rooms, along with geometrical constraints for better map
reconstruction.

##  IV Evaluation

This section presents the proposed method’s evaluation procedure in terms of
accuracy and performance compared to the state-of-the-art approaches. In this
respect, various real-world scenario tests were performed using the proposed
method, UcoSLAM [7] as the baseline methodology, and S-Graph+ [11] as a LiDAR-
based approach for providing ground truth measurements. As LiDAR sensor has
less noise when compared with the monocular vision-based sensor, it can be
safely considered as ground truth.

###  IV-A Evaluation Setup

In order to evaluate the performance of the proposed approach in real-world
circumstances, we mounted a Intel® RealSense™ Depth Camera D435 as the
monocular sensor on a Boston Dynamics Spot® robot and collected data from an
indoor environment. The robot functioned in different office zones of two
different university buildings with various corridor and room setups, where
the walls were labeled with printed $17cm\times 17cm$ ArUco markers. Marker-
ids of the ArUco markers placed in the environment, along with the room unique
labels, were also stored in a database file and fed to the system. Fig. 4
demonstrates the robot employed for collecting data and some instances of the
collected dataset. Additionally, the characteristics of the dataset are
presented in Table I.

Figure 4: Collected dataset for evaluation of the method: a) the legged robot used for data collection, b) some instances of the dataset. TABLE I: The characteristics of the collected indoors dataset. Sequence∗ | Duration | #Markers |  Description
---|---|---|---
Seq-01 | 08m 53s | 35 |  Two corridors connected via a landing without robot rotation
Seq-02 | 09m 29s | 35 |  Two corridors connected via a landing with robot rotation
Seq-03 | 16m 05s | 26 |  One corridor and five rooms
Seq-04 | 10m 25s | 14 |  One corridor and a two-doors room
Seq-05 | 09m 37s | 20 |  Four aisles connected to a main corridor
Seq-06 | 23m 34s | 22 |  Three corridors and three rooms
∗data were stored as packages of rosbag files.

Evaluations have been conducted using a computer equipped with an 11th Gen.
Intel® Core™ i9 @2.60GHz processor and 32 GigaBytes of memory. Moreover, to
better visualize the detected objects in the environment, including 3D points,
ArUco markers, walls, and rooms, and provide a more accurate evaluation to
compare the proposed method with other state-of-the-art approaches, a set of
Robot Operating System (ROS) tools have been employed.

###  IV-B Experimental Results

In order to demonstrate the accuracy of the proposed method compared to its
baseline and ground truth, Absolute Trajectory Error (ATE) measurements have
been employed in this paper. Accordingly, the Root Mean Square Deviation
(RMSE) and Standard Deviation (STD) values of the proposed and baseline
approaches were compared to the ground truth, and the approach with less value
is assumed to perform more accurately.

According to the evaluation results presented in Table II, the proposed
approach works better than its baseline in most of the cases. The main reason
for such improvement is the ability of the proposed method to add new
constraints to the map and employ the association of semantic entities to
enhance the reconstruction of the final map. The mentioned improvement can
also be seen in Fig. 6. Moreover, the proposed approach utilizes data
association results and performs a local semantic loop closure in addition to
the marker-based loop closure detection when visiting a previously observed
wall. In this regard, if the equation of the currently visible wall was
previously measured, i.e., the wall was previously detected, a global
optimization runs again to improve the reconstructed map.

The above-mentioned impact is more obvious in cases such as Seq-04 where the
robot starts in a corridor, enters from one of the two doors of a room and
exits from the other door to continue in the same corridor, and as a result,
no loop closure using keypoints and markers is performed. While our method,
which creates wall and room constraints, correctly identifies the corridor and
its walls to provide better results. This improvement can be clearly seen in
Fig. 5, where our method is able to reconstruct a more accurate map of the
environment when compared with the baseline. Moreover, it is able to extract
meaningful semantic and topological information from the environment.

TABLE II: Evaluation results of the proposed method on the collected dataset using Root Mean Square Deviation (RMSE) error in meters and Standard Deviation (STD). The best results are boldfaced. | Method
---|---
| Proposed |  UcoSLAM [7]
Sequence | RMSE | STD | RMSE | STD
Seq-01 | 8.038 | 3.058 | 8.130 | 3.035
Seq-02 | 6.883 | 3.598 | 6.930 | 3.633
Seq-03 | 2.266 | 0.894 | 2.687 | 1.335
Seq-04 | 3.787 | 1.848 | 5.822 | 2.726
Seq-05 | 1.255 | 0.763 | 1.238 | 0.751
Seq-06 | 2.676 | 1.524 | 2.720 | 1.536

(a) S-Graphs+ on Seq-04

(b) UcoSLAM [7] on Seq-04

(c) Proposed method on Seq-04

(d) Proposed method on Seq-04 showing only wall 3D points

(e) S-Graphs+ on Seq-06

(f) UcoSLAM [7] on Seq-06

(g) Proposed method on Seq-06

(h) Proposed method on Seq-06 showing only wall 3D points

Figure 5: Reconstructed maps using the proposed method, the baseline, and the
ground truth. It can be seen that the proposed approach is able to generate
more precise maps similar to the ground truth in detailed and with high-level
semantic entities.

(a) Proposed

(b) UcoSLAM [7]

Figure 6:  Absolute Trajectory Error (ATE) w.r.t. translation part in meters
on Seq-04 using different approaches. It can be seen that the room created by
the proposed approach is more similar to the ground truth.

Limitations. The proposed method, when testing in an environment where marker-
based loop closure is triggered several times, can show similar results to
baseline UcoSLAM. Seq-06 in Fig. 5 shows an example of the final map
showcasing similar map quality results generated by the proposed method and
UcoSLAM for an environment where the robot revisits the places (thus the
markers) several times. Even with the given limitation for such scenarios, our
approach is able to extract more meaningful information from the environment
while maintaining similar accuracy with respect to the baseline.

##  V Conclusions

This paper presented a Visual SLAM approach that employs a monocular camera as
the sensor and the data encoded within fiducial markers placed in the
surroundings for accurate pose estimation and semantic segmentation. The
proposed method can detect three practical semantic entities in the
environment, including walls, corridors, and rooms, and provide a hierarchical
graph with high-level semantic data. Additionally, it can reconstruct a more
accurate map of the environment by adding more topological constraints and
employing the relations among mentioned semantic entities found in the scene.
Evaluations performed on a dataset collected by a legged robot in real-world
conditions demonstrated an accuracy improvement compared to the baseline and
satisfactory results compared to a LiDAR-based state-of-the-art techniques.

The approach proposed in this paper is part of a project which utilizes the
advantages of fiducial markers for various tasks while keeping these markers
invisible to human eyes but recognizable to robots. As for future works, the
authors plan to encode the environment with the mentioned invisible fiducial
markers introduced in [6] and equip the robot with a proper sensor setup to
interpret them, making the improved version of the proposed approach able to
accomplish Simultaneous Localization and Mapping tasks with the new type of
markers. Additionally, improving the performance of the proposed system to
work online and in real-time is another target for future work.

## References

  * [1] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” _IEEE Transactions on robotics_ , vol. 32, no. 6, pp. 1309–1332, 2016.
  * [2] M. Filipenko and I. Afanasyev, “Comparison of various slam systems for mobile robot in an indoor environment,” in _2018 International Conference on Intelligent Systems (IS)_. IEEE, 2018, pp. 400–407.
  * [3] S. Han and Z. Xi, “Dynamic scene semantics slam based on semantic segmentation,” _IEEE Access_ , vol. 8, pp. 43 563–43 570, 2020.
  * [4] S. Wen, P. Li, Y. Zhao, H. Zhang, F. Sun, and Z. Wang, “Semantic visual slam in dynamic environment,” _Autonomous Robots_ , vol. 45, no. 4, pp. 493–504, 2021.
  * [5] J. Zhao, Y. Huang, X. He, S. Zhang, C. Ye, T. Feng, and L. Xiong, “Visual semantic landmark-based robust mapping and localization for autonomous indoor parking,” _Sensors_ , vol. 19, no. 1, p. 161, 2019.
  * [6] H. Agha, Y. Geng, X. Ma, D. I. Avşar, R. Kizhakidathazhath, Y.-S. Zhang, A. Tourani, H. Bavle, J.-L. Sanchez-Lopez, H. Voos _et al._ , “Unclonable human-invisible machine vision markers leveraging the omnidirectional chiral bragg diffraction of cholesteric spherical reflectors,” _Light: Science & Applications_, vol. 11, no. 1, pp. 1–19, 2022.
  * [7] R. Muñoz-Salinas and R. Medina-Carnicer, “Ucoslam: Simultaneous localization and mapping by fusion of keypoints and squared planar markers,” _Pattern Recognition_ , vol. 101, p. 107193, 2020.
  * [8] B. Pfrommer and K. Daniilidis, “Tagslam: Robust slam with fiducial markers,” 2019\. [Online]. Available: https://arxiv.org/abs/1910.00679
  * [9] S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez, “Automatic generation and detection of highly reliable fiducial markers under occlusion,” _Pattern Recognition_ , vol. 47, no. 6, pp. 2280–2292, 2014.
  * [10] H. Bavle, J. L. Sanchez-Lopez, M. Shaheer, J. Civera, and H. Voos, “Situational graphs for robot navigation in structured indoor environments,” 2022. [Online]. Available: https://arxiv.org/abs/2202.12197
  * [11] ——, “S-graphs+: Real-time localization and mapping leveraging hierarchical representations,” _arXiv preprint arXiv:2212.11770_ , 2022.
  * [12] A. Tourani, H. Bavle, J. L. Sanchez-Lopez, and H. Voos, “Visual slam: What are the current trends and what to expect?” _Sensors_ , vol. 22, no. 23, p. 9297, 2022.
  * [13] I. A. Kazerouni, L. Fitzgerald, G. Dooly, and D. Toal, “A survey of state-of-the-art on visual slam,” _Expert Systems with Applications_ , p. 117734, 2022.
  * [14] E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in _2011 IEEE international conference on robotics and automation_. IEEE, 2011, pp. 3400–3407.
  * [15] R. Gomez-Ojeda, F.-A. Moreno, D. Zuniga-Noël, D. Scaramuzza, and J. Gonzalez-Jimenez, “Pl-slam: A stereo slam system through the combination of points and line segments,” _IEEE Transactions on Robotics_ , vol. 35, no. 3, pp. 734–746, 2019.
  * [16] H. M. S. Bruno and E. L. Colombini, “Lift-slam: A deep-learning feature-based monocular visual slam method,” _Neurocomputing_ , vol. 455, pp. 97–110, 2021\.
  * [17] M. Gonzalez, E. Marchand, A. Kacete, and J. Royan, “S3lam: Structured scene slam,” _arXiv preprint arXiv:2109.07339_ , 2021.
  * [18] R. Mur-Artal and J. D. Tardós, “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras,” _IEEE transactions on robotics_ , vol. 33, no. 5, pp. 1255–1262, 2017.
  * [19] Y. Wu, A. Kirillov, F. Massa, W.-Y. Lo, and R. Girshick, “Detectron2,” https://github.com/facebookresearch/detectron2, 2019.
  * [20] W. Wu, L. Guo, H. Gao, Z. You, Y. Liu, and Z. Chen, “Yolo-slam: A semantic slam system towards dynamic environment with geometric constraint,” _Neural Computing and Applications_ , vol. 34, no. 8, pp. 6011–6026, 2022\.
  * [21] L. Nicholson, M. Milford, and N. Sünderhauf, “Quadricslam: Dual quadrics from object detections as landmarks in object-oriented slam,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 1, pp. 1–8, 2018.
  * [22] M. Hosseinzadeh, K. Li, Y. Latif, and I. Reid, “Real-time monocular object-model aware sparse slam,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 7123–7129.
  * [23] Y. Fan, Q. Zhang, Y. Tang, S. Liu, and H. Han, “Blitz-slam: A semantic slam in dynamic environments,” _Pattern Recognition_ , vol. 121, p. 108225, 2022\.
  * [24] L. Sun, J. Wei, S. Su, and P. Wu, “Solo-slam: A parallel semantic slam algorithm for dynamic scenes,” _Sensors_ , vol. 22, no. 18, p. 6977, 2022\.
  * [25] Y. You, P. Wei, J. Cai, W. Huang, R. Kang, and H. Liu, “Misd-slam: multimodal semantic slam for dynamic environments,” _Wireless Communications and Mobile Computing_ , vol. 2022, 2022.
  * [26] H. Bavle, J. L. Sanchez-Lopez, M. Shaheer, J. Civera, and H. Voos, “Situational graphs for robot navigation in structured indoor environments,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9107–9114, 2022.

  *[VSLAM]: Visual SLAM
  *[LiDAR]: Light Detection And Ranging
  *[RGB-D]: Red Green Blue-Depth
  *[SLAM]: Simultaneous Localization and Mapping
  *[ORB]: Oriented FAST and Rotated BRIEF
  *[LSD]: Line Segment Detector
  *[CNN]: Convolutional Neural Network
  *[ROS]: Robot Operating System
  *[ATE]: Absolute Trajectory Error
  *[RMSE]: Root Mean Square Deviation
  *[STD]: Standard Deviation