# SRVIO: Super Robust Visual Inertial Odometry for Dynamic Environments and
Challenging Loop-closure Conditions

Ali Samadzadeh  Computer Engineering Department
Amirkabir University of Technology
Tehran, Iran
Email<EMAIL_ADDRESS>Ahmad Nickabadi  Computer Engineering Department
Amirkabir University of Technology
Tehran, Iran
Email<EMAIL_ADDRESS>
###### Abstract

There has been extensive research on visual localization and odometry for
autonomous robots and virtual reality during the past decades. Traditionally,
this problem has been solved with the help of expensive sensors, such as
lidars. Nowadays, the focus of the leading research in this field is on robust
localization using more economic sensors, such as cameras and IMUs.
Consequently, geometric visual localization methods have become more accurate
in time. However, these methods still suffer from significant loss and
divergence in challenging environments, such as a room full of moving people.
Scientists started using deep neural networks (DNNs) to mitigate this problem.
The main idea behind using DNNs is to better understand challenging aspects of
the data and overcome complex conditions such as the movement of a dynamic
object in front of the camera that covers the full view of the camera, extreme
lighting conditions, and high speed of the camera. Prior end-to-end DNN
methods have overcome some of these challenges. However, no general and robust
framework is available to overcome all challenges together. In this paper, we
have combined geometric and DNN-based methods to have the generality and speed
of geometric SLAM frameworks and overcome most of these challenging conditions
with the help of DNNs and deliver the most robust framework so far. To do so,
we have designed a framework based on Vins-Mono, and show that it is able to
achieve state-of-the-art results on TUM-Dynamic, TUM-VI, ADVIO, and EuRoC
datasets compared to geometric and end-to-end DNN based SLAMs. Our proposed
framework could also achieve outstanding results on extreme simulated cases
resembling the aforementioned challenges.

##  I Introduction

Visual inertial odometry (VIO) is the process of determining the location and
orientation of an entity with the help of visual (camera) and inertial (IMU)
sensors. Today, visual odometry is being used in a wide variety of
applications, including autonomous driving [1, 2], controlling and navigating
aerial micro autonomous vehicles and nano-drones [3, 4, 5], augmented reality
and enabling navigation on mobile devices [6]. In this paper, it is assumed
that the target entity only has a single RGB camera and a single IMU sensor.

The general pipeline of a modern VIO framework consists of three major
parts[7]: preprocessing, optimization, loop closure and pose graph
optimization. The first component (preprocessing) typically consists of visual
feature tracking and IMU preintegration between frames. There are direct [8,
9], keypoint based [7, 10] and deep-learning-based [11, 12] visual feature
tracking methods.

Figure 1: An example of SRVIO framework’s loop-closure performed on day and
night images of the same scene. The loop is successfully discovered by SRVIO
based on the matched keypoints.

The optimization component typically starts with an initialization stage in
which, algorithms like five-point [13] and triangulation are exploited to
create an initial 3D point cloud and reconstruct the initial poses. The
process is also known as visual Structure-from-Motion (SfM) [14]. After
initial SfM, the extracted poses and 3D point cloud locations are fine-tuned
using bundle adjustment and then the visual data are matched with inertial
data by estimating the scale and initial gravity vector. Finally, the
optimization process starts based on visual and inertial residuals. These
residuals are prominent in state-of-art VIO frameworks [9, 7, 15].

Loop closure and pose optimization is the last standard component of VIO
frameworks. This component’s goal is to globally optimize the poses using the
information provided by loop closure and map reuse mechanisms. The loop
closure mechanism is a tool for reducing the long-term error and improving
robustness of the models and is claimed to be a crucial part of modern SLAM
frameworks [10]. The three aforementioned components of a VIO system may be
executed in parallel. There should only be inter-process communications for
transferring data streams between these components.

Despite the success of the current state-of-the-art three-component VIO
models, they fail dramatically in some challenging situations. For example,
the presence of dynamic objects in the field of view of camera violates the
scene rigidity assumption of the base VIO frameworks and degrades the visual
features tracking quality. The moving objects make it difficult to build a
stable and accurate 3D point cloud and poses. There have been many attempts,
such as DynaSlam [16], to extend monocular and stereo VIO to dynamic
scenarios.

Also, VIO methods often struggle to keep track of visual features when the
entity being tracked is moving quickly, resulting in blurry or completely new
frames. Another major challenge is the existence of external objects that
cover most of the camera’s field of view and generate false visual cues of
movement. In such cases, the visual odometry either provides wrong estimates
or resorts to movements priors like assuming zero- or constant- speed
movement.

Stable long-term localization is another challenge of VIO as there are issues
with inertial sensors and visual tracking. Regarding the inertial sensors,
integration over time diverges due to the accumulation of errors in
measurements. On the other hand, the loop-closure, which plays a vital role in
stabilizing VIO systems in long time usages, fails to match photos of the same
scene taken at different times (e.g., day and night) or viewing angles (e.g.,
entrance and exit directions of an area). These situations are almost
impossible for classical K-Nearest Neighbor (KNN) feature match and
Perspective-n-Point to handle. While there are other deficiencies and
limitations, we focus on the aforementioned issues in this paper.

In this paper, we propose a novel VIO-framework to address the previously
mentioned shortages of current state-of-the-art VIO methods. In this
framework, a hybrid visual preprocessing step is designed to overcome the
problem of the dynamic objects at real-time speed. Our proposed optimization
component is capable of switching between different choices of useful data
sources (including visual, inertial or both of them) to stay stable and
provide accurate estimations under any circumstance. Our model’s inertial
preprocessing component solves the integration output divergence problem while
it provides real-time output. Finally, we propose a novel transformer based
loop-closure mechanism for robust localization in day-night and enter-exit
scenarios. The proposed framework is evaluated against all of the state-of-
the-art methods using well-known benchmarks and other challenging scenarios
described in Section IV, and the results indicate the efficiency of the
proposed solution.

The rest of this paper is structured as follows. The next section reviews the
related works. Section III gives a full description of the proposed SRVIO
model. Section IV reports the results of the experiments. Finally, Section V)
concludes the paper.

##  II Related works

This section will discuss the literature on VIO frameworks and the methods
addressing the challenges reported in the previous section. First, in Section
II-A, the most recent VIO frameworks utilizing a single camera and a single
IMU are reviewed in detail. In Section II-B, methods that deal with moving
objects and dynamic environments will be discussed. In Section II-C, the
problem of inertial data integration and odometry divergence is explained.
Also, the frameworks that have tackled this problem are described. Lastly, the
old problem of robust relocalization in challenging scenarios and the
solutions provided for these situations are discussed in Section II-D.

###  II-A Single-camera single-IMU VIO frameworks

There are many robust and successful frameworks that have been designed for
single-camera single-IMU visual-inertial odometry, which can be divided into
two categories: geometric and neural network-based methods. The former relies
on traditional visual feature extraction methods, while the latter utilizes
deep neural networks for feature extraction. One of the most famous geometric
VIO frameworks is VINS-Mono[7]. VINS-Mono’s preprocessing steps are well-known
algorithms; keypoints are extracted using the good-features-to-track algorithm
[17] and are tracked in video frames using Lucas-Kanade [18] method. The
optimization and initialization components of VINS-Mono are specially designed
for this framework and play an essential role in the success of this
framework. Despite the speed and accuracy of VINS-Mono in usual scenarios, it
is not very stable in some challenging environments.

ORB-SLAM3 is another state-of-the-art geometric VI-SLAM framework that is more
robust than VINS-Mono, addressing camera blur and visual feature loss and IMU-
camera translation matrices error. However, the algorithm does not provide
reliable outputs when dynamic objects appear in the scene despite its
robustness. The loop-closure of ORB-SLAM is similar to the classic
perspective-n-point and descriptor check method, which has its downsides when
the loop-closure is required in day-night and enter-exit scenarios.

VI-DSO [8] is another well-known geometric VI-SLAM algorithm that is based on
sparse and direct visual feature tracking between frames and does not extract
keypoints. This framework is also fast and accurate but has the same
shortcomings as other geometric SLAM frameworks. Generally, geometric SLAM
algorithms are fast, hand-engineered and robust to standard conditions, but
they fail to provide a robust localization in extreme conditions stated in the
previous section.

Recently, there has been an increasing interest in developing DNN-based SLAM
frameworks. UnDeepVO [19] was the first visual odometry framework based on
DNNs. The first attempts only proved the concept; they did not have the same
accuracy and robustness as the geometric SLAMs. However, more recent
frameworks such as VINet [11] and D3VO [12] show promising results. VINet is a
VIO model that formulates motion estimation as a sequence-to-sequence problem
and uses an end-to-end network to learn it. The visual preprocessing is done
by a FlowNetC NN in which the optical flow between frames is extracted, and
the inertial preprocessing for integrating IMU data is accomplished via a LSTM
network. VINet has achieved state-of-the-art performance but still suffers
from robustness issues in the challenging environments mentioned in Section I.

D3VO is a visual-only method that is more stable than VINet. The stability of
this approach comes from a NN that estimates the depth of each frame and
outputs the uncertainty coefficient of each pixel. This depth estimation NN
uses the Bayesian filtering method introduced in [20] to solve the problem of
image depth estimation in occluded and dynamic environments. The model shows
outstanding performance and accuracy over various datasets. D3VO is not VIO-
based and does not have a loop closure mechanism. Therefore, it diverges in
long-term scenarios and could fail in other challenging situations like
objects covering camera view or extreme motion blur.

###  II-B Dynamic robust SLAM frameworks

Today, almost all SLAM and VIO models are equipped with mechanisms to deal
with non-rigid environments. One of the most straightforward methods is to use
RANSAC in order to detect outliers[21, 15]. Another reasonably simple method
is to use a robust error function [22]. Unfortunately, these approaches fail
when the majority of keypoints belong to moving objects.

The more complex models tackle dynamic objects by identifying and dismissing
dynamic keypoints or dynamic objects entirely. To identify moving objects, the
common approach is to use semantic segmentation to remove the areas belonging
to dynamic objectt classes. Mask-SLAM [23] uses a mask produced by a semantic
segmentation algorithm to exclude undesirable feature points. DS-SLAM [24] is
equipped with a real-time semantic segmentation network running in an
independent thread which coupled with moving consistency checking methods,
enables it to filter out the dynamic portion of the scene. Similarly, PSPNET-
SLAM [25] uses a semantic segmentation network (PSPNET [26]) combined with
ORB-SLAM2 to achieve the same goal. RGB-D SLAM [27] filters out dynamic
keypoints instead of objects by only using depth edge points for visual
odometry. These depth edges have static weights that indicate each point’s
probability of belonging to a static object.

Dyna-SLAM [16] applies pixel-wise segmentation using a CNN to detect dynamic
objects for the monocular and stereo cases. In the RGBD case, multi-view
geometry and deep learning are used to remove dynamic objects and in-paint the
background occluded by these objects. Finally, Dynamic-SLAM [28] consists of
an SSD (Single Shot Detector) object detector for detecting dynamic objects,
and a missed detection compensation algorithm to improve the recall rate of
the SSD component, and a feature-based visual SLAM that correctly handles the
feature point of the dynamic objects.

Most of the methods mentioned above achieve real-time dynamic object
filtering. Nevertheless, when the dynamic object covers the whole field of
view in extreme cases, the frameworks’ outputs diverge or show motion prior.
This paper shows that using another information source like the inertial
sensor’s data can help to correct motion estimation errors in these extreme
situations.

###  II-C Inertial Odometry

Integrating inertial data (linear acceleration and rotation speed) provided by
the IMU sensor is an excellent odometry method for short periods. However, in
long term, the output diverges dramatically, as discussed and demonstrated in
[29]. Some methods, such as [30], try to limit IMU integration error over
time, but they are not very useful because they are designed for particular
applications such as pedestrian movements and are not very accurate as well.
With the advances in deep learning methods in processing time-series data, new
methods emerged to solve this problem known as inertial odometry.

One of the first successful algorithms which used DNNs to cure the curse of
drift in inertial odometry was IONet [31]. This method, with proper
initialization, performed close to industrial VIO frameworks such as Google
Tango. IONet uses an LSTM-based architecture and polar coordinates vectors to
estimate the pose at each step. It outperforms the previous classical attempts
by a large margin.

The other idea was the use of denoising CNNs. For example, [32] tries to
remove the noise that causes the integration to drift. Such methods require
calibration data from a stationary entity with the IMU sensor. Almost all new
emerging VIO frameworks have a performance gap against modern VIO and VO
frameworks, but fusing them with geometric SLAM’s preprocessing units can show
promising results over challenging scenarios. In this paper, we have designed
a denoising CNN to do the aforementioned task and have combined it with a
geometric SLAM (details provided in section III).

Figure 2: SRVIO framework architecture

###  II-D Robust Relocalization

Robust relocalization over extreme scenarios such as day-night, rainy or snowy
weather, seasonal changes, extreme viewpoint changes, and similar scenarios is
a big challenge in VIO. So far, many methods have tried to overcome some of
these challenges. The most interesting recent methods are X-view [33] and
Superglue [34]. The X-view method uses semantic segmentation over frames to
build a graph of objects over frames; then, relocalization is done via sub-
graph matching. The method is interesting because it can solve almost all
relocalization challenges, especially the extreme viewpoint changes. The
method can even relocalize the same place seen on the ground from aerial
images. However, this method is not accurate and gives a rough estimation of
the location and not a transformation matrix.

The Superglue provides exact transformation between two frames of a video.
This method is based on Superpoint [38] visual keypoint extractor and
descriptor proposed by the same team. It uses graph transformers to find
appropriate matches between the corresponding keypoints in frames and shows
robustness over day-night conditions and rainy weather, but it does not show
robustness over seasonal changes. Despite the success of Superglue in accurate
estimation of transformation between images with robustness over extreme
changes, it has never been used in a classical loop closure mechanism inside
geometric SLAM. In this paper, we use the Superglue and Superpoint inside a
novel loop closure mechanism to improve the relocalization robustness of our
model.

##  III SRVIO framework

The proposed SRVIO framework is explained in detail in this section. As
demonstrated in Fig. 2, the SRVIO consists of three main components:
preprocessing, optimization and loop closure. The input to this framework is
the image stream of a single RGB camera and IMU sensor data. The final output
is the pose of the camera in each key frame and the corresponding 3D point
cloud. In addition to the poses and locations, each frame’s keypoints are
reported in the form of pixel locations and their descriptors. In the
following subsections, each component of the model is discussed in detail.

###  III-A Preprocessing

The preprocessing module of SRVIO is made up of two visual preprocessing and
inertial preprocessing blocks, as seen in Fig. 2.

####  III-A1 Visual preprocessing

The visual preprocessing block of our SLAM system takes in a sequence of
frames ($I_{i}$) and outputs corresponding keypoints matrices
${\mathcal{P}}^{\mathcal{C}_{i}}$ between each pair of consecutive frames, as
well as a weight for each frame which represents the quality of the keypoints
in that frame. To do this, it first employs a keypoint extractor, good-
features-to-track [17], to track previous frame’s keypoints and extract new
keypoints from the regions of frame $i$ in which the old keypoints of frame
$i-1$ do not exist. The result is the set of initial keypoints of current
frame as
${\mathcal{P}}^{{\mathcal{C}_{i}}}_{initial}=GoodFeaturesToTrack(I_{i},{\mathcal{P}}^{\mathcal{C}_{i-1}})$.

If keypoints extracted from the regions occupied by non-stationary objects are
used, they will provide misleading clues for motion estimation. Hence, the
initial keypoint set is then matched against the output of a semantic
segmentation neural network (e.g., HRNet [35]) to decide whether they are
possibly dynamic (like keypoints on humans, cars, and trains) or static (like
keypoints on buildings or streets). This sub-step is known as the semantic
filter in our model. The semantic segmentation network segments the input
image into non-overlapping regions labeled as different items of a predefined
set of objects. The output of the semantic segmentation algorithm is then
converted to a binary mask indicating static and dynamic regions of the input
frame. The dynamic region is then dilated to contain the borderline pixels,
preventing the model from defining keypoints on these lines.

The semantic segmentation network can be implemented inside a SLAM framework
by various approaches, as in other methods like [36] and [16]. The output of
this network is used for filtering out the undesired (dynamic) keypoints.
However, it is also possible to use the semantic segmentation output before
the keypoint extraction step to restrict the search for keypoints to the
stationary regions of the input frame. However, this may cause the model to be
unstable as there might be many potentially dynamic objects (e.g., humans)
that are already static in the scene and rejecting all of the keypoints on
these objects remains very few keypoints for proper tracking and optimization.

The keypoint selection mechanism of SRVIO is fast and accurate. It is fast
since the semantic segmentation neural network is employed only when new
keypoints are required due to objects getting out of the scene or being
covered. It is also accurate since it allows the use of the keypoints wrongly
labeled as dynamic, e.g., points on steady cars. To do this, SRVIO employs a
three-step algorithm. First, RANSAC [37] is applied only to the keypoints
labeled as static by the semantic segmentation network to find initial
fundamental matrix transformation ($F^{i,i-1}_{inital}$) between frame $i$ and
$i-1$. Next, a test is performed to check the consistency of all keypoints,
including those labeled as dynamic objects by the segmentation network, with
this initial fundamental matrix and only the truly moving keypoints are
removed. Finally, the RANSAC is again applied to the new set of keypoints.
These three steps can be formulated as follows:

$\displaystyle\left[{\mathcal{P}}^{\mathcal{C}_{i,S1}},F^{i,i-1}_{inital}\right]$
$\displaystyle=RANSAC\Big{(}{\mathcal{P}}^{\mathcal{C}_{i}}_{initial},{\mathcal{P}}^{\mathcal{C}_{i-1}}\Big{)}$
(1)
$\displaystyle{\mathcal{P}}^{\mathcal{C}_{i,S2}}=\Bigg{\\{}{\mathcal{P}}^{\mathcal{C}_{i}}_{l,initial}$
$\displaystyle:\
\left\lvert{\mathcal{P}}^{\mathcal{C}_{i}}_{l,initial}.F^{i,i-1}_{inital}.{\mathcal{P}}^{\mathcal{C}_{i-1}}_{l}\right\lvert<\epsilon\Bigg{\\}}$
(2)
$\displaystyle\left[{\mathcal{P}}^{\mathcal{C}_{i}},F^{i,i-1}_{accurate}\right]$
$\displaystyle=RANSAC\Big{(}{\mathcal{P}}^{\mathcal{C}_{i,S2}},{\mathcal{P}}^{\mathcal{C}_{i-1}}\Big{)}$
(3)

where ${\mathcal{P}}^{\mathcal{C}_{i}}_{initial}$ is the initial keypoint
extracted from good-features-to-track algorithm. $F^{i,i-1}_{inital}$ and
${\mathcal{P}}^{\mathcal{C}_{i,S1}}$ are the fundamental matrix and the
keypoint matrix corresponding to the initial static keypoints. The Equation 2
checks the fundamental matrix test with the corresponding keypoints of
$i^{th}$ frame and static keypoints of ${(i-1)}^{th}$ frame. The output
(${\mathcal{P}}^{\mathcal{C}_{i,S2}}$) is the new set of keypoints including
the keypoints on static objects initially mislabeled as dynamic. Finally, in
order to accurately obtain non-dynamic keypoints and remove the outliers and
dynamic keypoints, the RANSAC algorithm is performed again in Equation 3. The
${\mathcal{P}}^{\mathcal{C}_{i}}$ is the final matrix of keypoints. Fig. 3
shows three examples of keypoint extraction and filtering in SRVIO along with
the corresponding static/dynamic masks. The green, blue and red dots on the
frames are filtered, tracked and not-tracked keypoints, respectively. The
white (black) regions of the mask show static (dynamic) objects. As shown in
this figure, the keypoints on the steady cars in the left picture are not
filtered even though the semantic mask labels them as dynamic, while the
keypoints on the train in the right picture are not used as expected.

Figure 3: Examples of the effect of semantic segmentation on keypoint
selection. Top: three frames from seq22 of the Advio dataset on which the
keypoints detected on the dynamic objects are shown in green. Bottom: dynamic
semantic mask extracted for each frame. The figures show that the keypoints on
steady cars are not removed in the proposed method.

After eliminating the moving keypoints, a weight parameter ($\Psi_{c_{i}}$) is
computed for each frame that determines the quality of the keypoints of that
frame. This frame weight is used in the optimization component and is defined
as follows:

$\displaystyle\Psi_{c_{i}}$
$\displaystyle=\dfrac{\left|{\mathcal{P}}^{\mathcal{C}_{i}}\right|}{F_{max}}$
(4)

where $|\,.\,|$ denotes the size of the set of all remained keypoints inside
$i^{th}$ frame. The $F_{max}$ is a constant indicating the maximum number of
keypoints that can be present in a frame (experimentally set to 250 in this
paper). Accordingly, the frame weight is defined as the number of selected
static points in proportion to the maximum points extracted by the keypoint
extractor algorithm for a frame. This weight helps to decrease attention to
the visual error where the visual clues are unreliable because the dynamic
objects are covering the camera’s view field or the keypoint extractor is
unable to extract a proper number of keypoints due to motion blur or similar
challenges inside frames.

####  III-A2 Inertial preprocessing

In the inertial preprocessing component, a denoising convolutional neural
network (DCNN) is used to help eliminate unwanted IMU noise and mitigate the
divergence issue. As shown in Fig. 4, the proposed DCNN consists of two clone
networks: one for gyro and the other for the accelerometer data denoising. The
gyro DCNN gets $N$ sequential IMU data from time steps $i-N$ to $i$ and
outputs gyro correction ($\tilde{\omega}_{i}$) for the $i^{th}$ IMU
measurement. The accelerometer DCNN is the same as the gyro DCNN except that
it is trained to estimate the accelerometer correction ($\tilde{a}_{i}$) for
the $i^{th}$ IMU data. The correction fixes the IMU bias and random walk noise
via separating the noise part of the noisy IMU data. The correction is
performed as follows:

$\displaystyle\hat{C}_{(.)}$ $\displaystyle=\hat{S}_{(.)}\hat{M}_{(.)}$
$\displaystyle\hat{\omega}_{i}$
$\displaystyle=\hat{\mathbf{C}}_{\omega}\omega_{i}^{IMU}+\tilde{\omega}_{i}$
(5) $\displaystyle\hat{a}_{i}$
$\displaystyle=\hat{C}_{a}a_{i}^{IMU}+\tilde{a}_{i}$ $\displaystyle\zeta_{i}$
$\displaystyle=\zeta^{\omega}_{i}+\zeta^{a}_{i}$

where $\hat{M}_{(.)}$ are axis misalignment matrices and $\hat{S}_{(.)}$ are
scale factors; therefore, $\hat{C}_{(.)}$ is the correction matrix of the raw
input data. These parameters are unique for each IMU and are learned for each
dataset. The $(.)^{IMU}_{i}$, $\tilde{(.)}_{i}$ and $\hat{(.)}_{i}$ correspond
to IMU raw measurements, denoising correction values of the corresponding
DCNN, and the final corrected values, respectively. $\zeta^{a}_{i}$ and
$\zeta^{\omega}_{i}$ are the accelerometer and gyro data quality scores for
$i^{th}$ IMU data. This score improves the framework’s robustness over faulty
IMU sensors and similar challenges.

Figure 4: IMU denoising CNN architecture.

The denoising networks of our model (Fig. 4) are trained in the following
manner. First, the convolution layers are trained using GELU activation
function and cosine schedulers using the labeled IMU calibration data. Then,
the convolutional layers are frozen and the linear layers are trained with
augmented IMU data contaminated with different levels of random noise while
the labels are the noise percentage. Consequently, the network is trained to
predict the IMU data quality or the noise level in this stage. Moreover, since
ground truth is not available with high frequency, the corrected IMU (gyro and
accelerometer) data is integrated through time to reach the timestep interval
of the ground truth data. The training loss is defined as follows:

$\displaystyle\delta\hat{R}_{i,i+j}$
$\displaystyle=\prod_{k=i}^{i+j-1}\exp\left(\hat{\omega}_{k}\right)$ (6)
$\displaystyle\delta\hat{P}_{i,i+j}$
$\displaystyle=\int\int_{t\in[i,i+j]}\exp\left(\hat{\omega}_{t}\right)\
\hat{a}_{t}\ dt^{2}$ (7) $\displaystyle\mathcal{L}^{\omega}_{j}$
$\displaystyle=\sum_{i}\rho\left(\log\left(\delta
R_{i,i+j}\delta\hat{R}_{i,i+j}^{T}\right)\right)$ (8)
$\displaystyle\mathcal{L}^{a}_{j}$ $\displaystyle=\sum_{i}\rho\left(\delta
P_{i,i+j}-\delta\hat{P}_{i,i+j}\right)$ (9)
$\displaystyle\mathcal{L}^{\omega}$
$\displaystyle=\mathcal{L}^{\omega}_{16}+\mathcal{L}^{\omega}_{32}$ (10)
$\displaystyle\mathcal{L}^{a}$
$\displaystyle=\mathcal{L}^{a}_{16}+\mathcal{L}^{a}_{32}$ (11)

where $\mathcal{L}^{a}$ and $\mathcal{L}^{\omega}$ are the main loss functions
for training accelerometer and gyro denoising networks, respectively. The
$\hat{(.)}$ symbols represent corrected IMU measurements and $\delta
P_{i,i+j}$ and $\delta R_{i,i+j}$ represent ground truth values of position
and rotation in the time period of $i$ to $i+j$. $(.)^{T}$ is used to show the
transpose of a matrix. Since ground truth of rotations is not available with
high frequency for training accelerometer denoising network and calculation of
$\delta\hat{P}_{i,i+j}$, this network is trained after the gyro denoising
network training in order to have an accurate rotation matrix per data
($\exp\left(\hat{\omega}_{t}\right)$). The $\rho(.)$ is the Huber-norm defined
in Equation 12. The final loss of each network is defined as the sum of the
losses for 16 and 32 time steps.

$\rho(s)=\begin{cases}1&s\geq 1,\\\ 2\sqrt{s}-1&s<1.\end{cases}$ (12)

Hereafter in SRVIO, corrected values of accelerometer and gyro are regarded as
the IMU measurements on which the pre-integrations and integrations are
performed with the assumption that bias and noise standard deviation are near
zero. Using the IMU data validity score ($\zeta_{i}$), defined in Equation
III-A2, the IMU attention weight is defined as follows:

$\displaystyle\Psi_{b_{i}}=\frac{\sum_{k\in M_{l,j}}\zeta_{k}}{m}$ (13)

where $M_{l,j}$ is the set of IMU data inputs between $l^{th}$ frame and
$j^{th}$ frame, and $m$ is the size of this set. This weight parameter may be
set to 1 if the user is confident about the IMU data quality.

###  III-B Optimization

The location and pose estimation of SRVIO starts with an estimator
initialization step and continues with an optimization-based VIO procedure
combined with loop-closure detection. The initialization component of our
model is similar to those of the previous frameworks like VINS-Mono whose
details are given in [7]. Thus, in this section, we only focus on the details
of our proposed optimization module. In the following, we first define the
residual terms for visual and IMU measurements and then introduce the final
error function.

The formulas of this section use the following notations: $(.)^{w}$,
$(.)^{b}$, $(.)^{c}$ represent the world frame, the body frame (i.e. the IMU
frame) and the camera frame, respectively. $R$ and $q$ are used as matrix and
quaternion of rotation, respectively. The subscript for $R$ or $q$ indicates
the original frame while the superscript denotes the destination frame. $p$ is
used to show translation. $b_{k}$ and $\mathcal{C}_{k}$ are used to show body
frame and camera frame while taking the $k^{th}$ image. $\bigotimes$ is used
to denote multiplication operation between two quaternions.
$g^{w}=[0,0,g]^{T}$ is the gravity vector. Finally $\hat{(.)}$ denotes a noisy
measurement.

####  III-B1 IMU measurement residuals

Let us denote accelerator and gyroscope denoised values by $\hat{a}$ and
$\hat{\omega}$. The IMU pre-integration terms are then defined as

$\displaystyle\alpha^{b_{k}}_{b_{k+1}}=\int$
$\displaystyle\int_{t\in[t_{k},t_{k+1}]}R^{b_{k}}_{t}(\hat{a}_{t})dt^{2}$
$\displaystyle\beta^{b_{k}}_{b_{k+1}}=$
$\displaystyle\int_{t\in[t_{k},t_{k+1}]}R^{b_{k}}_{t}(\hat{a}_{t})dt$ (14)
$\displaystyle\gamma^{b_{k}}_{b_{k+1}}=$
$\displaystyle\int_{t\in[t_{k},t_{k+1}]}\dfrac{1}{2}\Omega(\hat{\omega})\gamma_{t}^{b_{k}}dt$

where $\Omega(.)$ is:

$\displaystyle\Omega(\omega)$
$\displaystyle=\begin{bmatrix}-\big{\lfloor}\omega\big{\rfloor}_{\times}&&\omega\\\
-\omega^{T}&&0\\\ \end{bmatrix},$
$\displaystyle-\big{\lfloor}\omega\big{\rfloor}_{\times}$
$\displaystyle=\begin{bmatrix}0&&-\omega_{z}&&\omega_{y}\\\
\omega_{z}&&0&&-\omega_{x}\\\ -\omega_{y}&&\omega_{x}&&0\end{bmatrix}.$ (15)

The residual for two IMU measurements in consecutive frames at body (IMU)
coordinates $b_{k}$ and $b_{k+1}$ in the sliding window can be written as:

$\displaystyle
r_{\mathcal{B}}(\hat{z}_{b_{k+1}}^{b_{k}},\chi)=\begin{bmatrix}\delta\alpha^{b_{k}}_{b_{k+1}}\\\
\delta\beta^{b_{k}}_{b_{k+1}}\\\ \delta\theta^{b_{k}}_{b_{k+1}}\end{bmatrix}$
$\displaystyle=\begin{bmatrix}R_{w}^{b_{k}}\big{(}p_{b_{k+1}}^{w}-p_{b_{k}}^{w}+\dfrac{1}{2}g^{w}\Delta
t^{2}_{k}-v_{b_{k}}^{w}\Delta t_{k}\big{)}-\hat{\alpha}^{b_{k}}_{b_{k+1}}\\\
R_{w}^{b_{k}}\big{(}v_{b_{k+1}}^{w}+g^{w}\Delta
t_{k}-v_{b_{k}}^{w}\big{)}-\hat{\beta}^{b_{k}}_{b_{k+1}}\\\
2\big{[}{q^{w}_{b_{k}}}^{-1}\bigotimes
q^{w}_{b_{k+1}}\bigotimes(\hat{\gamma}^{b_{k}}_{b_{k+1}})^{-1}\big{]}_{xyz}\end{bmatrix},$
(16)

where $\Delta t$ is time interval, $v$ denotes velocity, $[\,.\,]_{xyz}$ is an
operator extracting the vector part of quaternion $q$ for error state
representation and $\delta\theta^{b_{k}}_{b_{k+1}}$ is the three dimensional
error state representation of the quaternion. When the DCNN is enabled, the
IMU biases are not optimized and bias terms will be considered constant and
zero.

####  III-B2 visual measurement residual

The camera model defines camera measurement residuals on a generalized unit
sphere in contrast to the traditional pinhole camera model. Considering the
$l^{th}$ feature which appears at the $i^{th}$ image, the residual for
observing that feature at the $j^{th}$ image is written as:

$\displaystyle
r_{c}(\hat{z}_{l}^{c_{j}},\chi)=[b_{1}b_{2}]^{T}.(\mathcal{P}^{c_{j}}_{l}-\dfrac{\mathcal{P}^{c_{i}}_{l}}{\|\mathcal{P}^{c_{i}}_{l}\|})$
$\displaystyle\mathcal{P}^{c_{j}}_{l}=\pi_{c}^{-1}(\begin{bmatrix}u_{l}^{c_{j}}\\\
v_{l}^{c_{j}}\end{bmatrix})$ (17)
$\displaystyle\mathcal{P}^{c_{i}}_{l}=R^{c}_{b}(R^{b_{j}}_{w}(R^{w}_{b_{i}}(R^{b}_{c}\dfrac{1}{\lambda_{l}}\pi_{c}^{-1}(\begin{bmatrix}u_{l}^{c_{i}}\\\
v_{l}^{c_{i}}\end{bmatrix})+p^{b}_{c})$
$\displaystyle+p^{w}_{b_{i}}-p^{w}_{b_{j}})-p^{b}_{c}),$

where $\begin{bmatrix}u_{l}^{c_{i}}\\\ v_{l}^{c_{i}}\end{bmatrix}$ denotes the
first observation of $l^{th}$ feature in the $i^{th}$ image while
$\begin{bmatrix}u_{l}^{c_{j}}\\\ v_{l}^{c_{j}}\end{bmatrix}$ is the
observation of that feature in the $j^{th}$ image. $\pi_{c}^{-1}$ is the back
projection function which given a pixel location yields the corresponding unit
vector using camera intrinsic parameters. The residual vector is projected
onto the tangent plane since vision residual has two degrees of freedom.
$b_{1}$, $b_{2}$ are bases spanning the tangent plane
$\mathcal{P}^{c_{j}}_{l}$.

####  III-B3 full error function

The full state of window denoted by $\chi$ can be written as:

$\displaystyle\chi$
$\displaystyle=[\mathbf{x}_{0},\mathbf{x}_{1},\dots\mathbf{x}_{n},\mathbf{x}_{c}^{b},\lambda_{1},\ldots\lambda_{m}],$
(18) $\displaystyle\mathbf{x_{k}}$
$\displaystyle=[p_{b_{k}}^{w},v_{b_{k}}^{w},q_{b_{k}}^{w}],\in[0,n],$ (19)
$\displaystyle\mathbf{x_{c}^{b}}$ $\displaystyle=[p_{c}^{b},q_{c}^{b}],$ (20)

where $\mathbf{x_{k}}$ is the IMU state when the $k^{th}$ image is captured.
$p_{b_{k}}^{w},v_{b_{k}}^{w}\text{ and }q_{b_{k}}^{w}$ denote position,
velocity and the orientation of the body in the world frame at the time $k$,
respectively. $n$ indicates the number of key frames in the sliding window
while $m$ is the number of features in that window. Finally $\lambda_{l}$ is
the inverse depth of $l^{th}$ feature in the sliding window from its first
observation.

The base error function used in VINS-Mono [7] can then be written as:

$\displaystyle\mathcal{R}=\|r_{p}-H_{p}\chi\|^{2}$
$\displaystyle+\sum_{k\in\mathcal{B}}\|r_{\mathcal{B}}(\hat{z}_{b_{k+1}}^{b_{k}},\chi)\|_{P_{b_{k+1}}^{b_{k}}}^{2}$
$\displaystyle+\sum_{l,j\in\mathcal{C}}\rho(\|r_{\mathcal{C}}(\hat{z}_{l}^{\mathcal{C}_{j}},\chi)\|_{P_{l}^{\mathcal{C}_{j}}}^{2}),$
(21)

where $r_{\mathcal{B}}(\hat{z}_{b_{k+1}}^{b_{k}},\chi)$ and
$r_{\mathcal{C}}(\hat{z}_{l}^{\mathcal{C}_{j}},\chi)$ are residuals for visual
and IMU measurements, respectively, and $\mathcal{C}$ is the set of features
which have been observed at least twice in the current sliding window. $r_{p}$
and $H_{p}$ are prior information from marginalization and $\rho(.)$ is the
Huber norm (defined in Eqn. 12).

The Huber function $\rho$ penalizes residuals contributed by outlier features,
but it assigns equal importance to both camera and IMU residuals. However,
camera measurements usually yield a more satisfying result when sufficient
keypoints are present in the scene. IMUs are usually vulnerable to shifts over
long periods, but they exhibit precise performance during short periods. The
goal of the optimization component of SRVIO is to utilize the potential of
each of the measurements to compensate for the deficiencies of the other
measurement source in specific scenarios. To achieve this goal a novel
weighted error-function is proposed as follows:

$\displaystyle\mathcal{R}_{2}=\|r_{p}-H_{p}\chi\|^{2}$
$\displaystyle+\sum_{k\in\mathcal{B}}\Psi_{b_{k}}\
\|r_{\mathcal{B}}(\hat{z}_{b_{k+1}}^{b_{k}},\chi)\|_{P_{b_{k+1}}^{b_{k}}}^{2}$
$\displaystyle+\sum_{l,j\in\mathcal{C}}\Psi_{c_{k}}\
\rho(\|r_{\mathcal{C}}(\hat{z}_{l}^{\mathcal{C}_{j}},\chi)\|_{P_{l}^{\mathcal{C}_{j}}}^{2})$
(22)

This error function assigns a weight to each of the visual and inertial terms
according to the number of keypoints present in the current window (Eqn. 22).
The camera residual weight increases in proportion to the number of visual
keypoints in the frame window and causes the model to rely on the visual clues
in windows with large number of keypoints. On the other hand, the error
function mainly depends on IMU residual in frame windows with few keypoints.

###  III-C Loop closure

The loop closure component is essential for the long-term accuracy of
trajectory estimation as it corrects the large drifts that may occur over
time. The algorithm needs to memorize the previous scenes it has visited to
detect the loops. However, raw frames are not saved; instead, a bag-of-words
representations of key frames is stored in a visual database to decrease
memory usage. To do this, a set of keypoints are detected in each keyframe.
Each keypoint is then described as a visual word using the Superpoints
keypoint descriptor [38]. Once a loop -i.e. the correspondence between the
input frame and a frame in the keyframe database- is detected, the estimated
path is optimized to match the detected loop.

As shown in Fig. 2, the loop closure component consists of two blocks: loop
closure detection block and pose graph optimization block which closes the
loops and enables map reuse. The loop closure component performs a yaw and
translation optimization defined as below:

$\displaystyle\min_{p,\psi}$ $\displaystyle\Bigg{\\{}\sum_{(i,j)\in
E}\|\mathbf{r}_{i,j}\|^{2}\Bigg{\\}}$ (23) $\displaystyle\bar{P}_{i,j}$
$\displaystyle=\bar{R}^{w^{-1}}_{i}\left(\bar{P}^{w}_{j}-\bar{P}^{w}_{i}\right)$
$\displaystyle\bar{\psi}_{i,j}$ $\displaystyle=\bar{\psi}_{j}-\bar{\psi}_{i}$
(24) $\displaystyle\mathbf{r}_{i,j}(P^{w}_{i},\psi_{i},P^{w}_{j},\psi_{j})$
$\displaystyle=\begin{bmatrix}R^{w^{-1}}_{i}\left(P^{w}_{j}-P^{w}_{i}\right)-\bar{P}_{i,j}\\\
\psi_{j}-\psi_{i}-\bar{\psi}_{i,j}\end{bmatrix}$

where $E$ is the set of all pose graph edges and $(\,\bar{.}\,)$ indicates a
constant value. Additionally, $\bar{\psi}_{i,j}$ is the constant previous yaw
difference between two graph nodes.

The VINS-Mono algorithm divides the equation III-C into loop closed edges and
sequential edges to reduce the risk of wrong loop closure. However, the
Superglue method is very robust, and most of the correspondences result in
correct relocalization, and there is no need for such division.

The second block in this component is the loop closure detection block. The
loop closure detection block is crucial for maintaining long-term trajectory
estimation accuracy. This block should be robust to challenges such as light
source, day/night, and viewpoint changes. Previous methods (e.g., VINS-
Mono[7]) use some simple keypoint extractors and KNN mapping methods for this
purpose which are useless in facing the aforementioned challenges. SRVIO, on
the contrary, introduces a modern hybrid and much more robust loop detection
mechanism. In this method, the initial keypoints are first extracted using the
Superpoint model [38]. The keypoints are then filtered using the semantic
segmentation neural network of the visual preprocessing component. In this
stage, there is no need for checking the fundamental matrix test to add semi-
dynamic objects’ keypoints because they may not stay steady in the long-term.

Subsequent to this stage, the descriptors of keypoints of each keyframe are
stored in a DBoW2 [39] database to detect new loops in the future. Whenever a
new keyframe comes in, its $K$ closest keyframes from the DBoW2 database are
sought using a weak KNN algorithm. If the number of matched descriptors
(keypoints) between the input frame and any of these keyframes is more than a
predefined threshold ($T_{\textit{KNN}}$), the corresponding pair of keyframes
are passed to the Superglue[34] matching algorithm. Finally, there is a PnP
and pose matching stage to determine the exact transformation between two
keyframes. This transformation and the corresponding keyframes’ indices are
passed to pose graph optimization block to close the detected loop. The
Superglue correspondence detection algorithm allows SRVIO to match significant
viewpoint changes, occluded scenes, day/night scenarios and many other
challenging conditions as demonstrated in [34].

The keyframe database contains the following data:

$\displaystyle\left[i,\hat{\mathbf{p}}_{i}^{w},\hat{\mathbf{q}}_{i}^{w},j,\hat{\mathbf{p}}_{i,j}^{i},\hat{\psi}_{i,j},\mathbf{D}(u,v,des)\right]$
(25)

where $\hat{\mathbf{p}}_{i}^{w}$ and $\hat{\mathbf{q}}_{i}^{w}$ are estimated
position and orientation of the $i^{th}$ keyframe, respectively. $j$ is the
possible loop-closed keyframe index. $\hat{\mathbf{p}}_{i,j}$ and
$\hat{\psi}_{i,j}$ are the estimated relative position and yaw angle between
these two frames ($i$ and $j$), respectively. $\mathbf{D}(u,v,des)$ is the
keypoint matrix of $i^{th}$ keyframe containing pixel locations $(u,v)$ and
$des$ descriptors. This data matrix may be considered as the output of the
proposed VIO framework.

Figure 5: A simulated sequence based on ADVIO Sequence 22 in which a bus gets
close to the camera and covers the whole camera view for a few seconds and
then goes away. This simulation is designed to mimic the situation when a car
is moving behind a bus in traffic.

Figure 6: The results of the proposed SRVIO model and its sub-models over the
simulated ADVIO sequence 22. The VIO corresponds to a basic VIO framework with
no semantic segmentation and denoising networks. In this experiment, the loop-
closure mechanism is turned off.

Figure 7: The simulated outdoor7 sequence of the TUM-VI dataset has a human
blocking most of the camera’s view. The human appears at second 10 and leaves
at second 28.

Figure 8: The results of the proposed SRVIO model and its sub-models over the simulated TUM-VI outdoor7 sequence.In this experiment the loop-closure mechanism is turned off. Model | Sim1 ATE | Sim1 RPE | Sim2 ATE | Sim2 RPE
---|---|---|---|---
VIO | 22.691 | 0.207 | 5.150 | 2.578
VO+SS | 12.826 | 0.706 | 1.223 | 0.709
IO | 19.712 | 0.094 | 5.084 | 0.363
VIO+SS | 9.457 | 0.061 | 1.663 | 0.336
VIO+SS+W | 7.548 | 0.026 | 3.709 | 0.348
SRVIO | 2.201 | 0.024 | 0.959 | 0.180
TABLE I: The results of the proposed SRVIO model and its sub-models over ADVIO22-sim (Sim1) and TUM-VI outdoor7-sim (Sim2). The error metrics are RMS of ATE and RPE (m). The errors are not divided by path length of 29.8m to have a better distiction with limited floating point numbers. Model | s_half | w_xyz | w_rpy
---|---|---|---
VINS-Mono [7] | 0.043 | 0.478 | 0.601
ORB-SLAM3 [10] | 0.019 | 0.202 | 0.515
dynamic-SLAM [28] | 0.017 | 0.465 | 0.540
DS-SLAM [24] | 0.016 | 0.021 | 0.330
DynaSLAM [16] | 0.017 | 0.015 | 0.035
SLAMANTIC [36] | 0.016 | 0.016 | 0.043
SRVIO | 0.014 | 0.016 | 0.028
TABLE II: Frameworks evaluated on TUM-RGBD dynamic dataset. Error metric is RMS of ATE / path length. Model | V103 | V202 | V203 | M03 | M05
---|---|---|---|---|---
VINS-Mono [7] | 0.130 | 0.088 | 0.214 | 0.074 | 0.147
VI-DSO [8] | 0.096 | 0.062 | 0.174 | 0.117 | 0.121
ORB-SLAM3 [10] | 0.025 | 0.015 | 0.037 | 0.033 | 0.071
UnDeepVO [19] | 1.000 | 1.240 | 0.780 | 1.800 | 0.880
D3VO [12] | 0.110 | 0.050 | 0.190 | 0.080 | 0.090
SRVIO no LC | 0.104 | 0.027 | 0.093 | 0.082 | 0.109
SRVIO | 0.021 | 0.017 | 0.042 | 0.032 | 0.062
TABLE III: Frameworks evaluated on EuRoC dataset. Error metric is RMS of ATE / path length. Model | Room3 | Room5 | Outdoor6 | Outdoor7
---|---|---|---|---
VINS-Mono | 0.11 | 0.20 | 130.6 | 21.90
VI-DSO | 0.13 | 0.13 | 65.6 | 4.07
ORB-SLAM3 | 0.04 | 0.02 | 16.84 | 7.59
SRVIO | 0.06 | 0.08 | 13.19 | 3.87
SRVIO (no SS) | 0.05 | 0.06 | 13.73 | 3.90
TABLE IV: Frameworks evaluated on TUM-VI dataset. Error metric is RMS of ATE /
path length.

##  IV Experiments and results

In this section, multiple experiments are performed to showcase the
superiority of SRVIO in the aforementioned challenging conditions over the
current state-of-the-art VIO models.

###  IV-A Datasets and Parameter setting

We have selected several datasets to evaluate SRVIO against competitors. The
ADVIO [40] sequence 22 is edited to include highly dynamic objects in order to
compare the outcomes of the models in such situations. TUM-RGBD dynamic [41]
is a standard visual-only dataset. Here, TUM-RGBD is used with some estimators
to have IMU data. This modified dataset is then used to evaluate the
performance of dynamic SLAM models. The Simulated TUM-VI [42] outdoor7
sequence is used to evaluate the outcomes of SLAM frameworks in a case in
which a dynamic object completely covers the camera view. To do so, blocking
objects are added to the corresponding video. Lastly, SRVIO is evaluated on
multiple sequences of EuRoC [43] and TUM-VI datasets to demonstrate the
SRVIO’s performance in normal conditions.

The experiments are performed on a system consisting of an i5-6500 CPU and two
GTX-1080ti and GTX-1080 GPUs with a total RAM of 24 GBs and a Samsung 850 SSD
drive. At each experiment, if a framework processes less than 10 frames per
second, it is assumed that the framework have failed at that experiment. We
have also used Ubuntu 16.04 LTS operating system with CUDA toolkit 10.1 and
Nvidia driver 450.80, ROS Kinetic, Pytorch 1.4.0, Torchvision 0.5.0 and OpenCV
3.4.2 in our experiments.

The SRVIO’s parameters are set as follows. The max feature count and threshold
are set to 300 and 150, respectivly. Tracking frequency is chosen to be 20
fps. Max solver time for Ceres solver is set to 12 loops of 0.06 secs. As for
the neural networks, in order to give ADVIO 1280x720 images to the Superpoint
NN for retrieving keypoints, we first resize the images to 512x360 and then
add zero padding to make it 512x512. The Superpoint and Superglue are fine
tuned on some unused sequences of TUM-VI and EoRoC. The HRNet semantic
segmentation model is pretrained on Cityscapes dataset and there are no
training on TUM-VI or EuRoC or TUM-RGBD dynamic. The architecture of
Superpoint and Superglue and HRNet are not altered. As for the Denoising CNNs,
we use 7 layers of dilated CNNs for each of gyro data denoising and
accelerometer data denoising. The activation functions are GELU to reduce
overfitting. The learning scheduler is Cosine-sceduler. The training optimizer
is Adam. To train this NN for TUM-VI and TUM-RGBD datasets, we use the
calibration IMU data (almost 111 hrs) provided by dataset owners to carefully
calibrate the denoising CNNs over staionary conditions. As for the ADVIO and
EoRoC, we use the unused sequences as calibration data to train the network
with them.

Lastly, the evaluations are performed with both absolute trajectory error
(ATE) and relative pose error (RPE) using our custom EVO evaluator algorithm.

###  IV-B Ablation study

Goal of this experiment is to demonstrate the task of each block of SRVIO
framework in facing extreme dynamic environments. To do so, we study the
results of a number of sub-models of SRVIO. In the rest of this section, VIO
stands for a vanilla VIO framework (similar to VINS-Mono) with no semantic
segmentation, adaptive weighting, IMU denoising networks, or loop-closure. IO,
resembling Inertial Odometry, is the same VIO framework with visual residual
deleted from the optimization. Similarly, VO is another version of VIO with no
inertial residual inside the optimization formula. SS stands for semantic
segmentation module, and if used, it means that the framework uses the visual
preprocessing of the SRVIO. W stands for dynamic weighting and if used, the W
framework uses the optimization block of SRVIO with inertial weights fixed to
unity. Sim1 and sim2 refer to simulated ADVIO sequence 22 and TUM-VI sequence
outdoor7, respectively.

For the experiments of this section, we have used two sequences whose
trajectories are plotted in Fig 5 and Fig. 7. The first sequence (Fig. 5) is
designed to simulate driving behind a bus or walking behind a large object.
The second sequence (Fig. 7) is designed to evaluate the robustness of the
models against complete coverage of the camera view. In this sequence, he
camera view is blocked from second 10 to second 28 by a human.

The results of SRVIO and its sub-models over the above two sequences are
reported in Fig. 6, Fig. 8 and table I. These results indicate that vanilla
VIO could never solve the impossible task of odometry on ADVIO22-sim or TUM-
VI-sim. Furthermore, the IO configuration diverges after a few seconds at both
ADVIO22-sim (sim1) and TUM-VI outdoor7-sim (sim2), and therefore the loss is
very high compared to other configs (see Table I). However, the RPE loss is
relatively smaller than expected. This small RPE error is because the IO
problem is the build-up error of noise inside inertial integration, and
everything seems to be ok in the short term. The VIO configuration also
diverges because the semantic segmentation is not used and visual residual
becomes large and inertial residual cannot help rescue the framework from
divergence. The visual-only model combined with the semantic segmentation
network (VO+SS) cannot converge at the sim1 sequence when the bus comes near
and covers the image. The same thing happens in the sim2 sequence when the
human completely covers the camera. This is because the VO+SS framework does
not have enough static keypoints when the bus is near, and the odometry
trajectory estimation diverges. The results of the VIO equipped with the
semantic segmentation (VIO + SS) are slightly different. This configuration’s
odometry estimation does not diverge when the bus covers the camera in sim1
sequence. This convergence is because inertial data assists the optimization.
However, it diverges after a while because it does not have enough keypoints
to track and optimize correctly. In the sim2 sequence, the loss of keypoints
and small inertial residual causes the algorithm to maintain movement bias
(fixed pose). When the human goes away, the trajectory continues to be
accurate.

The VIO combined with semantic segmentation and dynamic weighting inside the
optimization block (VIO+SS+W) seems to perform great at sim1 sequence.
Nevertheless, at sim2 sequence, it increase the weight of the inertial
residual and hence the divergence of the inertial integration happens. The
need for the denoising neural network becomes evident in this experiment. The
(VIO+SS+W) configuration shows the importance of having dynamic weighting. It
also explains the vulnerability of solely using the weighting mechanism and
placing too much attention on inertial data in the optimization step. The
complete SRVIO (without loop-closure) is the most successful model in both
sim1 and sim2 sequences. It shows how IMU denoising can result in robustness
over harsh camera blocking situations and help the VIO framework work more
accurately when visual features are not good.

To summarize, it can be concluded that using only semantic segmentation is not
enough and methods similar to SLAMANTIC, DynaSLAM2 would not output accurate
trajectories in the extreme dynamic conditions. Also, Other state-of-the-art
methods like ORB-SLAM3 and VINS-Mono will not be as accurate as SRVIO in full
or partial visual feature loss situations.

###  IV-C Comparison with the state-of-the-art models

In this section, SRVIO’s performance is compared with state-of-the-art VIO
methods in two main experiments. In the first experiment, the accuracy of the
SRVIO on standard dynamic datasets is evaluated and compared with state-of-
the-art methods. The IMU data of the vision-only dataset are simulated using
the ground truth and the additional SLAMANTIC output. The results, reported in
Table II, indicate that when dynamic objects and some motion blur are present
in the input sequences, conventional methods like ORB-SLAM3 and VINS-Mono are
useless. Also, the results suggest that our SRVIO method is superior to the
SLAMANTIC method even when the IMU data is simulated.

In the TUM-RGBD dynamic dataset, the walking sequences contain humans walking
in front of the camera, and the camera moves rapidly. Also, there are some
camera blurs and extreme brightness changes. As the results show (Table II),
the challenge is relatively high; therefore, the ORB-SLAM3 and VINS-Mono
frameworks diverge and give unacceptable results. However, Other dynamic
methods use semantic segmentation and thus have much better performance.
Although, the other methods have problems in camera blur and extreme light
changes, the SRVIO framework compensates the lack of proper visual data using
IMU data and the adaptive weighting mechanism. Therefore, the SRVIO is the
best choice for indoor datasets with dynamic objects and challenging
environments similar to this dataset. Despite having simulated the IMU data,
the results at Table II show that our method outperforms all previous dynamic
methods on this standard dataset.

In the last experiment, SRVIO is compared in normal conditions against state-
of-the-art geometric and deep NN SLAM frameworks on EuRoC and TUM-VI datasets.
The results are depicted in Table III and Table IV. The results show that pure
deep NN SLAM methods like D3VO has comparable results against geometric
frameworks with disabled loop closure such as VINS-Mono and VI-DSO. Also, the
results show that our SRVIO framework is great under normal conditions as well
and its performance does not drop with semantic segmentation as reported by
SLAMANTIC method. The reason behind this is the IMU denoised data which helps
to reduce the false semantic segmentation errors on trajectory estimation.
Moreover, The ORB-SLAM3 method is excellent on both datasets, and our SRVIO
framework has comparable results with this model in the general conditions.
Nevertheless, the TUM-VI outdoor sequences show the superiority of our SRVIO
framework’s loop closure mechanism over the ORB-SLAM3’s loop closure block. To
summarize, Our SRVIO framework outperformed all state-of-the-art methods in
the dynamic conditions and general conditions. Also, SRVIO in common sequences
with easy or no loop closure conditions had comparable results with the state-
of-the-art ORB-SLAM3 framework.

##  V Conclusions

As mentioned, there are three main unsolved challenges in existing state-of-
the-art VIO/VO SLAM frameworks: dynamic objects especially camera blocking
objects, the divergence of IMU data odometry, and robust long-term loop-
closure in challenging conditions such as day/night and multi-view situations.
Our proposed SRVIO framework is demonstrated to solve all of the problems at
once and become the best choice in those challenging conditions. Moreover, our
proposed framework is able to run on any dataset with framerates higher than
10fps with the mentioned computational power.

Our experiments on simulated data also show the need for a dynamic and
challenging dataset. There is a simulated dataset for this purpose called
VIODE[44], but this dataset is not real and may cause some biases to hybrid or
deep-NN-based SLAM frameworks. There should be a dataset with temporally
missing visual and inertial data, dynamic objects, challenging loop-closure
scenes, and scenarios in which the visual features are not trackable (e.g.
extreme motion blur). Such dataset may facilitate the design of super robust
SLAM frameworks.

## References

  * [1] L. An, X. Zhang, H. Gao, and Y. Liu, “Semantic segmentation–aided visual odometry for urban autonomous driving,” _International Journal of Advanced Robotic Systems_ , vol. 14, no. 5, p. 1729881417735667, 2017.
  * [2] M. Sabry, A. Al-Kaff, A. Hussein, and S. Abdennadher, “Ground vehicle monocular visual odometry,” in _2019 IEEE Intelligent Transportation Systems Conference (ITSC)_. IEEE, 2019, pp. 3587–3592.
  * [3] Y. Lin, F. Gao, T. Qin, W. Gao, T. Liu, W. Wu, Z. Yang, and S. Shen, “Autonomous aerial navigation using monocular visual-inertial fusion,” _Journal of Field Robotics_ , vol. 35, no. 1, pp. 23–51, 2018.
  * [4] J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 2502–2509.
  * [5] T. Do, L. C. Carrillo-Arce, and S. I. Roumeliotis, “High-speed autonomous quadrotor navigation through visual and inertial paths,” _The International Journal of Robotics Research_ , vol. 38, no. 4, pp. 486–504, 2019\.
  * [6] K. Wu, A. Ahmed, G. A. Georgiou, and S. I. Roumeliotis, “A square root inverse filter for efficient vision-aided inertial navigation on mobile devices.” in _Robotics: Science and Systems_ , vol. 2, 2015.
  * [7] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 1004–1020, 2018.
  * [8] L. Von Stumberg, V. Usenko, and D. Cremers, “Direct sparse visual-inertial odometry using dynamic marginalization,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 2510–2517.
  * [9] X. Xiong, W. Chen, Z. Liu, and Q. Shen, “Ds-vio: Robust and efficient stereo visual inertial odometry based on dual stage ekf,” _arXiv preprint arXiv:1905.00684_ , 2019.
  * [10] C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam,” _IEEE Transactions on Robotics_ , 2021\.
  * [11] R. Clark, S. Wang, H. Wen, A. Markham, and N. Trigoni, “Vinet: Visual-inertial odometry as a sequence-to-sequence learning problem,” in _Proceedings of the AAAI Conference on Artificial Intelligence_ , vol. 31, no. 1, 2017.
  * [12] N. Yang, L. v. Stumberg, R. Wang, and D. Cremers, “D3vo: Deep depth, deep pose and deep uncertainty for monocular visual odometry,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2020, pp. 1281–1292.
  * [13] D. Nistér, “An efficient solution to the five-point relative pose problem,” _IEEE transactions on pattern analysis and machine intelligence_ , vol. 26, no. 6, pp. 756–770, 2004.
  * [14] J. L. Schonberger and J.-M. Frahm, “Structure-from-motion revisited,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2016, pp. 4104–4113.
  * [15] 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.
  * [16] B. Bescos, J. M. Fácil, J. Civera, and J. Neira, “Dynaslam: Tracking, mapping, and inpainting in dynamic scenes,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 4076–4083, 2018.
  * [17] J. Shi _et al._ , “Good features to track,” in _1994 Proceedings of IEEE conference on computer vision and pattern recognition_. IEEE, 1994, pp. 593–600.
  * [18] J.-Y. Bouguet _et al._ , “Pyramidal implementation of the affine lucas kanade feature tracker description of the algorithm,” _Intel corporation_ , vol. 5, no. 1-10, p. 4, 2001.
  * [19] R. Li, S. Wang, Z. Long, and D. Gu, “Undeepvo: Monocular visual odometry through unsupervised deep learning,” in _2018 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2018, pp. 7286–7291.
  * [20] C. Liu, J. Gu, K. Kim, S. G. Narasimhan, and J. Kautz, “Neural rgb (r) d sensing: Depth and uncertainty from a video camera,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 10 986–10 995.
  * [21] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: a versatile and accurate monocular slam system,” _IEEE transactions on robotics_ , vol. 31, no. 5, pp. 1147–1163, 2015.
  * [22] G. Klein and D. Murray, “Parallel tracking and mapping for small ar workspaces,” in _2007 6th IEEE and ACM international symposium on mixed and augmented reality_. IEEE, 2007, pp. 225–234.
  * [23] M. Kaneko, K. Iwami, T. Ogawa, T. Yamasaki, and K. Aizawa, “Mask-slam: Robust feature-based monocular slam by masking using semantic segmentation,” in _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition Workshops_ , 2018, pp. 258–266.
  * [24] C. Yu, Z. Liu, X.-J. Liu, F. Xie, Y. Yang, Q. Wei, and Q. Fei, “Ds-slam: A semantic visual slam towards dynamic environments,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 1168–1174.
  * [25] S. Han and Z. Xi, “Dynamic scene semantics slam based on semantic segmentation,” _IEEE Access_ , vol. 8, pp. 43 563–43 570, 2020.
  * [26] H. Zhao, J. Shi, X. Qi, X. Wang, and J. Jia, “Pyramid scene parsing network,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2017, pp. 2881–2890.
  * [27] S. Li and D. Lee, “Rgb-d slam in dynamic environments using static point weighting,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 4, pp. 2263–2270, 2017.
  * [28] L. Xiao, J. Wang, X. Qiu, Z. Rong, and X. Zou, “Dynamic-slam: Semantic monocular visual localization and mapping based on deep learning in dynamic environment,” _Robotics and Autonomous Systems_ , vol. 117, pp. 1–16, 2019\.
  * [29] O. J. Woodman, “An introduction to inertial navigation,” University of Cambridge, Computer Laboratory, Tech. Rep., 2007.
  * [30] P. G. Savage, “Strapdown inertial navigation integration algorithm design part 1: Attitude algorithms,” _Journal of guidance, control, and dynamics_ , vol. 21, no. 1, pp. 19–28, 1998.
  * [31] C. Chen, X. Lu, A. Markham, and N. Trigoni, “Ionet: Learning to cure the curse of drift in inertial odometry,” in _Proceedings of the AAAI Conference on Artificial Intelligence_ , vol. 32, no. 1, 2018.
  * [32] M. Brossard, S. Bonnabel, and A. Barrau, “Denoising imu gyroscopes with deep learning for open-loop attitude estimation,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 3, pp. 4796–4803, 2020.
  * [33] A. Gawel, C. Del Don, R. Siegwart, J. Nieto, and C. Cadena, “X-view: Graph-based semantic multi-view localization,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 3, pp. 1687–1694, 2018.
  * [34] P.-E. Sarlin, D. DeTone, T. Malisiewicz, and A. Rabinovich, “Superglue: Learning feature matching with graph neural networks,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2020, pp. 4938–4947.
  * [35] Y. Yuan, X. Chen, and J. Wang, “Object-contextual representations for semantic segmentation,” in _Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, 2020, Proceedings, Part VI 16_. Springer, 2020, pp. 173–190.
  * [36] M. Schorghuber, D. Steininger, Y. Cabon, M. Humenberger, and M. Gelautz, “Slamantic-leveraging semantics to improve vslam in dynamic environments,” in _Proceedings of the IEEE International Conference on Computer Vision Workshops_ , 2019, pp. 0–0.
  * [37] M. A. Fischler and R. C. Bolles, “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography,” _Communications of the ACM_ , vol. 24, no. 6, pp. 381–395, 1981.
  * [38] D. DeTone, T. Malisiewicz, and A. Rabinovich, “Superpoint: Self-supervised interest point detection and description,” in _Proceedings of the IEEE conference on computer vision and pattern recognition workshops_ , 2018, pp. 224–236.
  * [39] D. Gálvez-López and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” _IEEE Transactions on Robotics_ , vol. 28, no. 5, pp. 1188–1197, 2012.
  * [40] S. Cortés, A. Solin, E. Rahtu, and J. Kannala, “Advio: An authentic dataset for visual-inertial odometry,” in _Proceedings of the European Conference on Computer Vision (ECCV)_ , 2018, pp. 419–434.
  * [41] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of rgb-d slam systems,” in _Proc. of the International Conference on Intelligent Robot Systems (IROS)_ , Oct. 2012.
  * [42] D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, and D. Cremers, “The tum vi benchmark for evaluating visual-inertial odometry,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 1680–1687.
  * [43] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, “The euroc micro aerial vehicle datasets,” _The International Journal of Robotics Research_ , vol. 35, no. 10, pp. 1157–1163, 2016.
  * [44] K. Minoda, F. Schilling, V. Wüest, D. Floreano, and T. Yairi, “Viode: A simulated dataset to address the challenges of visual-inertial odometry in dynamic environments,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 1343–1350, 2021.


11institutetext:  Computer Aided Medical Procedures, Technische Universität
München, Germany 22institutetext: Google 33institutetext: Computer Aided
Medical Procedures, Johns Hopkins University, Baltimore, USA

# Multimodal Semantic Scene Graphs for Holistic Modeling of Surgical
Procedures

Ege Özsoy Both authors share first authorship.11 Evin Pınar Örnek 11⋆⋆ Ulrich
Eck 11
Federico Tombari 1122 Nassir Navab 1133

###### Abstract

From a computer science viewpoint, a surgical domain model needs to be a
conceptual one incorporating both behavior and data. It should therefore model
actors, devices, tools, their complex interactions and data flow. To capture
and model these, we take advantage of the latest computer vision methodologies
for generating 3D scene graphs from camera views. We then introduce the
Multimodal Semantic Scene Graph (MSSG) which aims at providing a unified
symbolic, spatiotemporal and semantic representation of surgical procedures.
This methodology aims at modeling the relationship between different
components in surgical domain including medical staff, imaging systems, and
surgical devices, opening the path towards holistic understanding and modeling
of surgical procedures. We then use MSSG to introduce a dynamically generated
graphical user interface tool for surgical procedure analysis which could be
used for many applications including process optimization, OR design and
automatic report generation. We finally demonstrate that the proposed MSSGs
could also be used for synchronizing different complex surgical procedures.
While the system still needs to be integrated into real operating rooms before
getting validated, this conference paper aims mainly at providing the
community with the basic principles of this novel concept through a first
prototypal partial realization based on MVOR dataset.

###### Keywords:

Surgical Procedure Semantic Scene Graph Operating Room

##  1 Introduction

Figure 1: Illustration of MSSG generation from multi-view RGB-D images. Top
pipeline demonstrates the generation of SSG from MVOR dataset. On the bottom,
the proposed MSSG is conceptually figurated and an exemplary case of
vertebroplasty workflow modeling derived from MSSG is presented.

Modeling surgical procedures with all their different components is crucial to
develop intelligent systems that understand surgical operations, enable
computer interventions, and provide situational aware assistance [1]. Yet, as
surgical procedures take place in highly complex, dynamic, cluttered and
occluded environments, with the broad heterogenity of procedures, patients,
medical teams, imaging and life support devices, surgical tools, operating
table and monitors, they become a challenging input for visual perception and
understanding [2, 3]. Even though state-of-the-art computer vision techniques
have proved to be highly accurate for task-specifically designed datasets, it
is non-trivial to translate these methods to the highly complex real OR
environments. Lalys and Jannin [4] provide a review of surgical workflow
analysis and establish the definition of Surgical Process Models (SPM).
Furthermore, there is a great body of work on video and signal based surgical
phase recognition [5, 6, 7, 8], but the community has not yet focused on
holistic approaches enabling the required dynamic mappings between the
surgical process model, the human roles and the information processing and
visualization flow of each of the devices within the operating room. This is
however needed for digital systems to know who needs which information or tool
from whom or which system at each moment of the surgery.

In this work, we introduce a concept and methodology for modelling and
analyzing surgical procedures via multimodal 3D semantic scene graphs (MSSG).
Semantic scene graphs (SSG) are introduced in the literature as a symbolic
representation to connect visual attributes to semantic (_i.e_. language-
based) factors [9]. They provide an abstraction layer in between those, and
are specifically suitable for modeling the interactions between objects in
complex environments. Typically, nodes of the graph represent the objects, and
edges the corresponding relations between the objects. In the recent
literature, SSGs are used for image retrieval [9], image generation and
editing [10, 11], video understanding [12], 3D scan modeling [13, 14], and 3D
dynamic simulation modeling [15].

SSGs proved to be convenient for modeling environments, but have not yet been
studied in the context of highly complex environments such as OR rooms. Here
we take SSGs one step further. We connect the various OR domain components and
their multimodal data in a unified symbolic, spatiotemporal and semantic way
through Multimodal SSG (MSSG). This representation allows adding virtual
objects and connections between objects, connecting real objects to virtual
ones and creating semantic relationships in between. Furthermore, this graph
can be dynamically updated in real-time whenever a change is detected. One can
adjust different levels of complexities depending on the use-case of the graph
and the requirements (_e.g_. patient focus, device focus).

In the following, we first describe how to build a dynamic 3D scene graph
specifically for OR environments, through the combination of various computer
vision techniques for human pose estimation and object detection. We
demonstrate our concept by means of the publicly available multi-view
operating room dataset MVOR [16]. We provide a suite of graphical user
interface tools for surgical domain analysis, based on our symbolic
representation of MSSG: (a) Bird’s Eye View surgical process visualizer, and
(b) text-based report generator.

Furthermore, we define a MSSG distance function enabling the measurement of
similarity between two MSSG instances. We then use a simple Dynamic Time
Warping algorithm, based on the proposed MSSG distance function to synchronize
two surgical procedures. This demonstrates that MSSGs could offer many ways of
using multimodal heterogeneous data for synchronizing surgeries, while putting
the attention on different parameters or aspects of the surgical domain. For
example, the surgeries could be synchronized based on the MSSG graph centered
on the patient, surgeon or imaging device (see Fig. 1) and in each case by
putting the attention on different parameters based on the targeted
application.

##  2 Materials and Methodology

###  2.1 Scene Graphs

A scene graph $G$ can be defined as a set of tuples $G=(N,E)$ where
$N=\\{n_{1},...,n_{n}\\}$ is a set of nodes and $E\subseteq N\times R\times N$
is a set of edges with relationships $R$ [9]. Nodes typically have a class
label $c$ among different object categories (eg. medical objects) or human
types (eg. specialist, patient). Edges have some relations, or attributes,
$R$, from a set of different labels defining the relationships between nodes.
In 3D scene graphs, nodes represent 3D objects [13]. In order to build 3D
scene graphs including human interaction, one needs to detect the object
instances and humans in a given environment to be assigned as nodes and
estimate the relationships between them to select edge labels.

###  2.2 Dataset

In the light of the requirements to build a scene graph focusing on surgical
operation room, there is only limited data published [16, 17] to build a 3D
scene graph, whereas only [16] is publicly available. Multi-View Operating
Room (MVOR) dataset [16] provides 732 multi-view frames by three RGB-D cameras
recorded in an interventional room during vertebroplasty and lung biopsy
procedures in a 4 day setting. In order to create our system pipeline, we
first fused the RGBD images from three cameras to build a 3D point cloud
representation of the scene, by using the provided intrinsic and extrinsic
camera parameters. The MVOR dataset provides 2D and 3D human pose annotations
along with bounding boxes, however no annotations for objects nor patients.
For human pose estimation, we use the existing labels for training a neural
network, whereas for object detection we employ the classical computer vision
algorithms [18, 19].

###  2.3 Human Pose Estimation

####  2.3.1 3D Pose Prediction

Unlike other datasets where 3D scene graphs were applied [13], humans are
central to our dataset to model the complexity of OR (_e.g_. medical team and
patients). Therefore, a robust detection and pose estimation of humans is very
important. For detecting the 3D human poses, we selected VoxelPose [20], a
state-of-the-art 3D pose estimation method. As this method requires 2D pose
estimations as input, we employed HigherHRNet [21], a state-of-the-art 2D pose
estimation method. Both models are trained with the first three day data from
MVOR, and validated with the last day.

####  2.3.2 Patient Prediction

The MVOR dataset does not provide patient pose labels, so it is not possible
to involve patient prediction in human pose training. Instead, they provide
bounding box labels which we used to train Faster-RCNN [22] for patient
detection from the images. On test time, to locate the patient in the point
cloud, we apply this model to three images from three cameras. We crop out the
parts that contain the patient, and project it to 3D. We then calculate the
median of this point cloud for $(x,y,z)$, use this as the center of the
patient and place a virtual patient pose model while always assuming the same
position within the bounding box, head first, lying down orientation for the
patient.

###  2.4 Object Recognition and Pose Estimation

The MVOR dataset does not provide labels for objects either. Moreover, the
dataset domain is significantly different than most indoor datasets that are
commonly used in deep learning literature [23] in terms of environment and
presence of unknown objects. For these reasons, rather than depending on deep
learning based detection and segmentation, we formulate our pose estimation
task as detecting a 3D object scan from a 3D point cloud, and focus on the
objects that are detectable on MVOR data, such as monitoring devices and
surgical machines. With the presence of a data with finer details, it would
possible to detect smaller instruments along with different detection methods.

To achieve this, we manually extract the 3D point clouds of different objects
visible in the environment. Once the object scans are created, the object pose
estimation pipeline is employed. We iterate over the list of possible objects
and apply a global finding and local fine tuning step: for every scan S of one
object, we first calculate the normals, create downsampled version of it and
calculate feature descriptors (FPFH, see [24]). Then we use Random Sample
Consensus (RANSAC, see [18]) to find the global location of this object. We
assume the object is not in the scene if the RANSAC fitness is below a certain
threshold which was determined manually specifically for MVOR. Then we apply
colored Iterated Closest Point Algorithm (ICP, see [19]) to get a better
alignment for the object. Once this has been done for every scan of an object,
we use the pose with the highest RANSAC fitness value.

###  2.5 Multimodal Semantic Scene Graph (MSSG)

We propose the concept of MSSG, which extends the definition of SSG with
virtual multimodal components to allow modelling the complexity of the OR
environment. By virtual components, we refer to the electronic signals,
different outputs of the medical devices (_e.g_. X-Ray, computational
tomography, electrocardiogram, blood levels for anesthesia monitoring etc…),
and display objects (_e.g_. timer, BEV visualization display).

####  2.5.1 MSSG nodes

The real OR components (humans and objects) are the first building blocks of
MSSG, represented by graph nodes. Reconstruction and recognition of these
components are described in previous chapter. The second type of nodes are the
virtual components which can be added to MSSG to provide additional
synchronized multimodal semantic information.

Note that, objects present in the operating rooms could be defined together
with their attributes and relationships. For example, as seen in Fig 1, a
C-arm would define an object that comes with a connection to X-ray technician.
Once the C-arm enters in the room, graph should be updated with the C-arm
node. C-arm object can have different states: on/off/providing
acquisition/display transmission. An X-ray technician might turn on the C-arm,
start acquisition, and the C-arm device can transfer the data to medical
surgeon. Each of these steps can be represented via MSSGs.

####  2.5.2 MSSG edges

There are different types of relationships among nodes: (a) spatial, (b)
semantic, (c) virtual. The spatial relations are those that only describe the
relative positional difference between two nodes, such as left, right, up,
down, near or touching each other etc. The semantic relations carry non-
spatial meaning, such as ”getting scanned”, ”lying on”, or ”operating the
patient on the torso”. These are deducted using semantic information (_e.g_.
electric cautery on/off, coagulator on/off) and the spatial relationship
between the nodes (_e.g_. patient’s relative position to the bed). Finally,
virtual edges are those that describe the relations between virtual and real
components, such as ”CT scanning output (displayed on monitor 1)”, ”ECG signal
input (displayed on monitor 2)”.

####  2.5.3 MSSG distance function

We define a distance function to be able to measure the differences between
two given MSSG. Being a non-trivial task due to having different types of
factors (spatial, semantic, virtual), we propose a simple function that counts
all these factors, and can be modified according to user’s objective. To
calculate the distance between two scene graphs, we first retrieve elementary
information about them separately, such as if they include any medical staff
or a patient, how many medical staff is there, what objects are present in the
scene, and object’s relationship to the patient (_e.g_. CT devices). These
metrics are then compared between the two scene graphs and a distance is
computed by weighting every difference according to a chosen set of factors,
which can be listed as: presence of patient, medical staff, monitors, C-arm,
patient monitoring, CT, endoscopic visualization, number of medical staff,
presence of surgery on torso/head, position of patient and surgery acquisition
time.

##  3 Surgical Procedure Monitoring through MSSG

###  3.1 Bird’s Eye View Spatio-temporal and Semantic OR Visualizer

A surgical procedure requires often many different devices and can quickly
evolve towards a cluttered and complex environment, where one requires videos
from different viewpoints synchronized with other signals to obtain a complete
and informative overview during the surgery. For this reason, we propose a
surgical operation visualizer with the help of MSSG which connects all
components and provides a Bird’s Eye View reflection with a simplified
illustration. This visualization is created by projecting the scene graph
nodes from top view. Virtual objects, such as patient transporter bed (not
visible in any camera view), anesthetic data, X-Ray images, CT and its output,
are also visualized.

Figure 2: Automatic Bird’s Eye View visualization of a surgical procedure
through MSSG retrieved from 3 camera views. Visualization on the bottom left
shows the C-arm, CT machine, patient, medical team, CT monitor and virtual CT
display, virtual timer, along with conceptually proposed virtual edges.

###  3.2 Event Change Reporter

Our MSSG distance function allows comparing differences between two given
scene graphs. As the distance is a function of semantic parameters, we provide
an event change reporter which textually outputs the changes between two
scenes. This can result in smart reporting and semantic modelling which could
later be used for anomaly detection or augmented documentation.

Figure 3: Event change reporter outputs between two input scenes at time $t=1$
and time $t=20$, calculated from MSSG.

##  4 Surgical MSSG Synchronization

####  4.0.1 Problem Definition

We demonstrate one possible use-case of MSSG through the task of surgical
operation synchronization, which aims to detect the corresponding events and
landmarks between two given similar surgical operations [25, 26]. In our case,
we can map the relevant phases of surgical operations simply through MSSGs.
Our symbolic representation functions as a semantic abstraction layer allowing
intelligent matching and synchronization, which would not be possible if only
visual inputs were directly used.

####  4.0.2 Dynamic Time Warping

One common and powerful method to measure the similarity between two temporal
series is Dynamic Time Warping algorithm introduced by Bellman _et al_. [27].
Given two series $a=\\{a_{1},...,a_{m}\\}$ and $b=\\{b_{1},...,b_{n}\\}$, DTW
aims to build a $MxN$ distance matrix $D$ which provides the distance for any
path $P$ along the given sets [28]. The optimal path $P^{*}$ is the path with
minimum distance,

$P^{*}=\min_{p\in P}(D_{p}(a,b))$ (1)

which can be calculated through dynamic programming.

In our setting, MSSG is the input for the algorithm, and the previously
defined distance function is used to calculate the DTW distance matrix.
Furthermore, as different attributes can bring more information to the
synchronization, we applied the weighting scheme (see 2.5.2) for distance
function, where each attribute can have a different contribution on the
overall distance (cost) function.

####  4.0.3 Qualitative Results

A GUI application that implements the scene graph DTW algorithm is provided,
where a user can select the weighting mechanism and visualize the
synchronization results between two given surgical videos(Fig. 4).

Figure 4: Surgical MSSG synchronization tool based on DTW algorithm with MSSG
distance calculated in a weighting scheme. Left boxes show the frames of video
1 and 2, where points show the matching between frames. On the right, the
synchronized videos, on the bottom, slider buttons allow selecting weighting
factors.

##  5 Discussion and Conclusion

We proposed Multimodal Semantic Scene Graphs for modelling and analyzing
surgical procedures in a holistic manner, which can connect various OR
components in a semantically linked symbolic graph representation. The
conceptual framework and a building pipeline for MSSG is described through
different levels of visual recognition. A first set of surgical procedure
analysis tools built on top of MSSG is provided, which allow visualizing an
ongoing operation from Bird’s Eye View, offering selection of different
objects as the central node, and reporting the changes between two scenes.
Moreover, surgical synchronization task is proposed through our MSSG distance
based Dynamic Time Warping algorithm.

The introduced system has many possible uses, such as medical staff role
prediction, anomaly detection and smart reporting. Furthermore, more
sophisticated OR tools and devices can be also fused into MSSG in real time
and the semantic relationships can be enhanced through the prior knowledge and
machine learning. Overall, this conference paper proposes a novel concept and
creates new opportunities for learning from videos as well as synchronized
signals acquired within operating rooms, opening the path towards holistic
understanding and modeling of surgical procedures.

## References

  * [1] L. Maier-Hein, S. S. Vedula, S. Speidel, N. Navab, R. Kikinis, A. Park, M. Eisenmann, H. Feussner, G. Forestier, S. Giannarou, M. Hashizume, D. Katic, H. Kenngott, M. Kranzfelder, A. Malpani, K. März, T. Neumuth, N. Padoy, C. Pugh, N. Schoch, D. Stoyanov, R. Taylor, M. Wagner, G. D. Hager, and P. Jannin, “Surgical data science for next-generation interventions,” Nature Biomedical Engineering, vol. 1, p. 691–696, Sept. 2017.
  * [2] L. R. Kennedy-Metz, P. Mascagni, A. Torralba, R. D. Dias, P. Perona, J. A. Shah, N. Padoy, and M. A. Zenati, “Computer vision in the operating room: Opportunities and caveats,” IEEE Transactions on Medical Robotics and Bionics, pp. 1–1, 2020.
  * [3] Z. Li, A. Shaban, J. Simard, D. Rabindran, S. P. DiMaio, and O. Mohareri, “A robotic 3d perception system for operating room environment awareness,” CoRR, vol. abs/2003.09487, 2020.
  * [4] F. Lalys and P. Jannin, “Surgical process modelling: a review.,” International Journal of Computer Assisted Radiology and Surgery, Springer Verlag, vol. 9, pp. 495–511, 2014.
  * [5] C. R. Garrow, K.-F. Kowalewski, L. Li, M. Wagner, M. W. Schmidt, S. Engelhardt, D. A. Hashimoto, H. G. Kenngott, S. Bodenstedt, S. Speidel, B. P. Müller-Stich, and F. Nickel, “Machine learning for surgical phase recognition: A systematic review,” Annals of surgery, November 2020.
  * [6] C. I. Nwoye, D. Mutter, J. Marescaux, and N. Padoy, “Weakly supervised convolutional lstm approach for tool tracking in laparoscopic videos,” International journal of computer assisted radiology and surgery, vol. 14, no. 6, pp. 1059–1067, 2019.
  * [7] T. Czempiel, M. Paschali, M. Keicher, W. Simson, H. Feussner, S. T. Kim, and N. Navab, “Tecno: Surgical phase recognition with multi-stage temporal convolutional networks,” in Medical Image Computing and Computer Assisted Intervention - MICCAI 2020 - 23nd International Conference, Shenzhen, China, October 4-8, 2020, Proceedings, Part III, vol. 12263 of Lecture Notes in Computer Science, pp. 343–352, Springer, 2020.
  * [8] S. Bodenstedt, D. Rivoir, A. Jenke, M. Wagner, M. Breucha, B. Müller-Stich, S. T. Mees, J. Weitz, and S. Speidel, “Active learning using deep bayesian networks for surgical workflow analysis,” International Journal of Computer Assisted Radiology and Surgery, vol. 14, pp. 1079–1987, 2019.
  * [9] J. Johnson, R. Krishna, M. Stark, L.-J. Li, D. Shamma, M. Bernstein, and L. Fei-Fei, “Image retrieval using scene graphs,” in Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), June 2015\.
  * [10] J. Johnson, A. Gupta, and L. Fei-Fei, “Image generation from scene graphs,” in Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), June 2018.
  * [11] H. Dhamo, A. Farshad, I. Laina, N. Navab, G. D. Hager, F. Tombari, and C. Rupprecht, “Semantic image manipulation using scene graphs,” in CVPR, 2020.
  * [12] J. Ji, R. Krishna, L. Fei-Fei, and J. C. Niebles, “Action genome: Actions as compositions of spatio-temporal scene graphs,” in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), June 2020\.
  * [13] J. Wald, H. Dhamo, N. Navab, and F. Tombari, “Learning 3d semantic scene graphs from 3d indoor reconstructions,” in Conference on Computer Vision and Pattern Recognition (CVPR), 2020.
  * [14] I. Armeni, Z.-Y. He, J. Gwak, A. R. Zamir, M. Fischer, J. Malik, and S. Savarese, “3d scene graph: A structure for unified semantics, 3d space, and camera,” in Proceedings of the IEEE International Conference on Computer Vision, pp. 5664–5673, 2019.
  * [15] A. Rosinol, A. Gupta, M. Abate, J. Shi, and L. Carlone, “3D dynamic scene graphs: Actionable spatial perception with places, objects, and humans,” in Robotics: Science and Systems (RSS), 2020.
  * [16] A. Kadkhodamohammadi, A. Gangi, M. de Mathelin, and N. Padoy, “A multi-view rgb-d approach for human pose estimation in operating rooms,” in 2017 IEEE Winter Conference on Applications of Computer Vision (WACV), pp. 363–372, 2017.
  * [17] A. Sharghi, H. Haugerud, D. Oh, and O. Mohareri, “Automatic operating room surgical activity recognition for robot-assisted surgery,” in Medical Image Computing and Computer Assisted Intervention – MICCAI 2020 (A. L. Martel, P. Abolmaesumi, D. Stoyanov, D. Mateus, M. A. Zuluaga, S. K. Zhou, D. Racoceanu, and L. Joskowicz, eds.), (Cham), pp. 385–395, Springer International Publishing, 2020.
  * [18] M. Fischler and R. Bolles, “Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography,” Communications of the ACM, vol. 24, no. 6, pp. 381–395, 1981.
  * [19] K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of two 3-d point sets,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. PAMI-9, no. 5, pp. 698–700, 1987.
  * [20] H. Tu, C. Wang, and W. Zeng, “Voxelpose: Towards multi-camera 3d human pose estimation in wild environment,” in European Conference on Computer Vision (ECCV), 2020.
  * [21] B. Cheng, B. Xiao, J. Wang, H. Shi, T. S. Huang, and L. Zhang, “Higherhrnet: Scale-aware representation learning for bottom-up human pose estimation,” in CVPR, 2020.
  * [22] S. Ren, K. He, R. Girshick, and J. Sun, “Faster r-cnn: Towards real-time object detection with region proposal networks,” in Advances in Neural Information Processing Systems (C. Cortes, N. Lawrence, D. Lee, M. Sugiyama, and R. Garnett, eds.), vol. 28, Curran Associates, Inc., 2015.
  * [23] A. Dai, A. X. Chang, M. Savva, M. Halber, T. Funkhouser, and M. Nießner, “Scannet: Richly-annotated 3d reconstructions of indoor scenes,” in Proc. Computer Vision and Pattern Recognition (CVPR), IEEE, 2017.
  * [24] R. B. Rusu, N. Blodow, and M. Beetz, “Fast point feature histograms (fpfh) for 3d registration,” in 2009 IEEE International Conference on Robotics and Automation, pp. 3212–3217, 2009.
  * [25] G. Forestier, F. Lalys, L. Riffaud, B. Trelhu, and P. Jannin, “Classification of surgical processes using dynamic time warping,” Journal of Biomedical Informatics, vol. 45, no. 2, pp. 255–264, 2012.
  * [26] S.-A. Ahmadi, T. Sielhorst, R. Stauder, M. Horn, H. Feussner, and N. Navab, “Recovery of surgical workflow without explicit models,” in Medical Image Computing and Computer-Assisted Intervention – MICCAI 2006 (R. Larsen, M. Nielsen, and J. Sporring, eds.), (Berlin, Heidelberg), pp. 420–428, Springer Berlin Heidelberg, 2006.
  * [27] R. Bellman and R. Kalaba, “On adaptive control processes,” IRE Transactions on Automatic Control, vol. 4, no. 2, pp. 1–9, 1959.
  * [28] A. Bagnall, J. Lines, A. Bostrom, J. Large, and E. Keogh, “The great time series classification bake off: A review and experimental evaluation of recent algorithmic advances,” Data Min. Knowl. Discov., vol. 31, p. 606–660, May 2017.