# Probabilistic Qualitative Localization and Mapping

authors  Roee Mor and Vadim Indelman R. Mor is with the Department of Computer
Science, Technion - Israel Institute of Technology, Haifa 32000, Israel. V.
Indelman is with the Department of Aerospace Engineering, Technion - Israel
Institute of Technology, Haifa 32000, Israel<EMAIL_ADDRESS><EMAIL_ADDRESS>This work was partially supported by the Israel
Ministry of Science & Technology (MOST).

###### Abstract

Simultaneous localization and mapping (SLAM) are essential in numerous
robotics applications, such as autonomous navigation. Traditional SLAM
approaches infer the metric state of the robot along with a metric map of the
environment. While existing algorithms exhibit good results, they are still
sensitive to measurement noise, sensor quality, and data association and are
still computationally expensive. Alternatively, some navigation and mapping
missions can be achieved using only qualitative geometric information, an
approach known as qualitative spatial reasoning (QSR). We contribute a novel
probabilistic qualitative localization and mapping approach in this work. We
infer both the qualitative map and the qualitative state of the camera poses
(localization). For the first time, we also incorporate qualitative
probabilistic constraints between camera poses (motion model), improving
computation time and performance. Furthermore, we take advantage of
qualitative inference properties to achieve very fast approximated algorithms
with good performance. In addition, we show how to propagate probabilistic
information between nodes in the qualitative map, which improves estimation
performance and enables inference of unseen map nodes - an important building
block for qualitative active planning. We also conduct a study that shows how
well we can estimate unseen nodes. Our method particularly appeals to
scenarios with few salient landmarks and low-quality sensors. We evaluate our
approach in simulation and on a real-world dataset and show its superior
performance and low complexity compared to the state-of-the-art. Our analysis
also indicates good prospects for using qualitative navigation and planning in
real-world scenarios.

##  1 Introduction

Robotic and autonomous navigation has much impact on state-of-the-art
applications in various domains. Image-based navigation and simultaneous
localization and mapping (SLAM) are vital in this field. The SLAM problem has
been extensively investigated in the past three decades (see [1] for a recent
survey of state-of-the-art approaches and challenges). In particular, highly-
efficient open source SLAM software packages [2, 3, 4, 5] have been developed
and are gradually incorporated into real-world applications. Lately, the
problem of planning under uncertainty and active SLAM has also received
considerable research attention (see [6] for a recent survey).

Some challenges, however, remain. Firstly, while passive SLAM often achieves
online performance, real-time performance for low-cost platforms is more
challenging. In active planning, complexity is still an obstacle. Secondly,
state-of-the-art approaches are mainly based on linearization of the non-
linear geometric problem to use fast solvers [7, 8, 3, 9]. These approaches
usually use many landmarks to enable noise filtering and outlier removal
algorithms, another factor in high complexity and error accumulation. In many
cases, these approaches require an accurate initial guess for the estimated
variables, achieved using good GPS or IMU sensors or via accurate image-based
camera re-sectioning techniques. [10, 11] is an overview of basic methods.
Some advanced robust graph optimization techniques that attempt to be
resilient, or less sensitive, to outliers are [12, 13, 14, 15, 16, 17]. Non-
parametric approaches to the SLAM problem (e.g. [18, 19]) try to overcome some
of these issues by directly solving the nonlinear problem. These approaches,
however, still need to be performed online.

A different approach is topological mapping, which considers relative
attributes between different places. In this approach, a graph represents the
environment with vertices for different places and edges for relative
attributes (e.g., reachability between places). This problem is known as
”Visual Place Recognition,” as the estimation usually handles a discrete set
of places. These approaches usually contain minimal or no geometrical data and
do not fully integrate geometric inference. The advantage is that no
continuous geometric estimation means no error accumulation and low noise
dependency. On the other hand, the lack of geometric constraints in the
estimation process usually makes the approaches computationally expensive.
[20] surveys topological mapping and place recognition approaches.

Qualitative spatial reasoning (QSR) is yet another approach in which an agent
perceives the environment through the geometric relation between objects.
However, qualitative geometric relations are considered instead of metric
locations and orientations. In addition, in many cases, instead of using a
global frame of reference, local object-relative reference frames are used to
relate to close-by landmarks. As discussed in this work, QSR is the base for
our approach. While recent QSR works include [21],[22] and [23], section 1.2
provides a more detailed overview of QSR.

###  1.1 Motivation

QSR research was initially motivated by how humans and animals efficiently
perform complex path planning and navigation tasks. They often relate to close
landmarks instead of a large-scale global frame of reference and use a
qualitative perception of the environment instead of an accurate metric
perception (e.g., in figure 1). In QSR approaches, estimation of the map and
robot states is qualitative, hence, less metrically accurate but also less
noise dependent. The general idea is that this more straightforward approach
might be easier and more adequate for many robotic autonomous tasks.

In traditional metric SLAM approaches, noise sensitivity demands using many
landmarks for noise filtering, which partially accounts for the computational
load. In addition, using many landmarks makes data association harder.

Since qualitative inference is less sensitive to noise, it has the potential
to be easier to implement. First, lesser noise sensitivity might require less
noise filtering and, therefore, might be suited for using a smaller number of
landmarks. Using a small number of salient landmarks can, in turn, improve
landmark association. Secondly, since qualitative inference is less accurate,
there is room for coarser approximations that produce simpler algorithms.
These possibilities can lead to computationally light inference algorithms. In
addition to the insight that many robotic tasks do not require accurate metric
navigation, these properties motivate the research reported herein.

In this work, we contribute a probabilistic QSR-based mapping and localization
framework designed for large-scale navigation with simple sensors and low
complexity. Before stating the specific contributions of this work, we discuss
the most relevant QSR approaches.

(a)

(b)

Figure 1: Qualitative geometry used by humans for various tasks: (a) planning
route and navigating relative to landmarks and qualitatively using topographic
map; (b) Football players plan and execute qualitative relative positioning.

###  1.2 Related Work

The application of QSR to robotic navigation and mapping started in the 90s.
The early work by [24] suggests qualitative localization of a robot about
landmarks, given their azimuth ordering as seen by the agent in a single view.
This approach has been extended in [25, 26] and [27] to include multi-view
inference and some aspects of data association and place re-identification,
but not a complete SLAM problem. Other methods address the qualitative
representation of relative orientation between two oriented landmarks, such as
”bi-pole orientation”[28] and ”OPRAM”[21]. Another approach for representing
spatial location was suggested by Freksa [29]. This approach localizes in a
relative frame regarding two landmarks. Instead of considering the metric
location in this frame, Freksa partitions the space into a discrete set of
qualitative states known as ”Freksa’s single cross.” The ”Freksa’s double
cross” was suggested in [30] and was recently extended by McClelland et al.
[22] to the ”extended double cross” (EDC) for a more detailed representation.
Other qualitative partitions were also proposed over the years, including a
”close and far pie” (TPCC) by [31] and ”Left Right” by [32]. Figure 2
illustrates these partitions.

(a)

(b)

(c)

(d)

Figure 2: Different partitions of the metric space: (a) left right [32]; (b)
Freksa’s double cross [30]; (c) Extended double cross [22]; (d) TPCC [31].

The first comprehensive QSR-based mapping and navigation framework was
proposed by McClelland et al. [22] for NASA’s planetary rover. The authors
proposed a qualitative algorithm for 2D large-scale mapping with a low-quality
monocular camera and no GPS or ego-motion sensing. They use azimuth
measurements and range ordering of each triplet of landmarks observed together
to estimate one landmark location in a local frame defined by the other two.
Instead of considering the metric location of the landmark, they use a
discrete set of EDC-partitioned qualitative states. Geometric estimation
classifies each qualitative state as ”feasible” or ”non-feasible” in a binary
manner. The mapping goal is to infer all feasible qualitative states for each
landmark triplet. Measurements from different viewpoints can reduce ambiguity.
An additional ”composition” stage propagates qualitative data through triplets
with common landmarks, including triplets that were never viewed together by
the agent.

Further work [23] extends this method to probabilistic estimation. Instead of
assigning true or false labels, they infer the probability for each
qualitative state of each landmark triplet. [33] takes another step and
addresses active qualitative planning.

However, [22] and [23] do not model any spatial connection between different
camera views of the same triplet (such as motion model or geometric
triangulation). They also do not address probabilistic ”composition.” Finally,
in both papers, the focus is on mapping, not camera localization. The work of
[34] on the other end specifically addresses the use of qualitative
composition between landmark triplets in QSR problems. It focuses on the
optimal sequence of compositions for propagating data in a qualitative map
given a specific source and target nodes. It also addresses the question of
which new nodes can be added to the map by composition.

###  1.3 Contributions

In this work, we take a few steps closer to a full probabilistic qualitative
framework for localization and mapping. We aim for simple sensors (such as a
low-quality monocular camera with no GPS or significant IMU), low complexity,
and large-scale navigation. The potential of qualitative geometry for simple
and light inference (as mentioned in 1.1), and the insight that many robotic
tasks do not require accurate metric navigation, drive us to extend existing
QSR approaches. Moreover, we envision our approach as a step towards active
QSR planning, leveraging belief space planning formulation (e.g. [35, 36,
17]). We use a different and innovative formulation of the problem to address
some of the key limitations and introduce several key improvements to state-
of-the-art, most notably [22] and [23].

In particular, our _main contributions_ are as follows:

  1. 1.

While state-of-the-art QSR approaches focus mainly on mapping, we develop a
holistic probabilistic QSR approach that also addresses localization.

  2. 2.

We incorporate a motion model to improve both performance and complexity.

  3. 3.

We develop a global non-linear solver with a simple algorithmic flow. It is
better suited for various measurement types and does not require prior
knowledge. We also utilize qualitative inference robustness to develop a very
fast approximated algorithm with similar performance.

  4. 4.

We develop probabilistic composition - a novel way for efficiently propagating
information in the qualitative map, extending the deterministic approach in
[22]. Probabilistic composition is key for active qualitative planning, as
shown in our follow-up work [37].

  5. 5.

We formulate a discrete factor graph representation for the qualitative map
and use it for propagating information. The full solution for getting
marginals by elimination is computationally infeasible. Therefore, we develop
an approximate fast and effective algorithm.

  6. 6.

We analyze how propagating qualitative information in the factor graph is
related to graph topology and prior information. For this purpose, we suggest
new information-related metrics and a simplified model that gives an
empirically strong correlation to actual qualitative data propagation.

  7. 7.

We evaluate the performance of our approach in simulation and on a real-world
dataset and compare it against the state-of-the-art.

  8. 8.

We also made available an open-source code repository [38] that implements all
parts of our work. This repository holds a python-based implementation meant
for performance analysis and to be a reference for future work. It is only
partially real-time optimized.

The present paper is an extension of the work presented in [39], where we
introduced our innovative approach for the qualitative localization and
mapping problem. The formulation presented in the current manuscript is more
general and includes more extensive evaluations. As further core
contributions, we address the map as a qualitative factor graph and provide
insights about how much data can be propagated (items 5 and 6 above). In this
paper, we bring a full and comprehensive formulation and analysis of our
approach.

This paper’s organization is as follows. Section 2 introduces notations and
provides problem formulation. Section 3 describes in detail our approach. It
presents a probabilistic formulation of the qualitative localization and
mapping problem. Then it presents our proposed algorithm for the 2D case in
detail while also addressing run-time aspects. Lastly, it also includes the
derivation of our probabilistic composition technique and how to use it in a
factor graph for propagating information in a qualitative map. Section 6
provides performance evaluation. It also includes an analysis of the relations
between propagating qualitative information in the map to its factor graph
topology. Section 7 concludes the discussion and suggests several avenues for
future research.

##  2 Notations and Problem Formulation

We consider a robot navigating in and mapping an unknown environment. As the
robot moves, it tracks landmarks across different image frames. We aim to
_qualitatively_ describe the environment and camera trajectory. Motivated by
the approach in [22],[23], we consider multiple relative landmark-centric
frames of reference and use qualitative geometry to describe camera and
landmark locations in these frames.

While our formulation is general, we test our approach on the 2D case. For
every three landmarks $A,B,C$ observed together, we set a landmark-centric
local frame, so that landmark A location is $L^{A}=(0,0)$ and landmark B
location is $L^{B}=(0,1)$ (see Figure 3a). We describe the qualitative
location of landmark $C$ in this frame using a discrete space partition into a
set of $m$ qualitative states (see Figure 3b). The qualitative state of
landmark $C$ is $S^{AB:C}$. It is a vector of dimension $m\times 1$ that
contains the hypothesis for $C$ being in each qualitative state. We denote the
event that $C$ is in a specific a qualitative state
$S^{AB:C}=i;\in\\{1,...,m\\}$ as $s^{AB:C}_{i}$. We also denote the metric
location of landmark $C$ as $L^{AB:C}$.

Similarly, the metric location of the camera in the $AB$ frame at time step
$n$ is denoted by $X^{AB}_{n}$, and the corresponding camera qualitative state
is denoted by $S^{AB:X}_{n}$. The camera qualitative location in the entire
map at time step $n$ is then the collection of its states regarding all local
frames: $S^{X}_{n}=\\{S^{i,j:X}_{n}\\}$.

One can choose the space partition to fit different tasks, platforms, or
scenarios (see Figure 2). In our implementation and tests, we employ the EDC
partition used in previous works [23], [22].

(a)

(b)

Figure 3:  (a) AB landmark relative metric frame of coordinates. (b) AB frame
qualitative state probability distribution $\mathbb{P}({S^{AB:C}})$ (Freksa’s
double cross in this example).

The true mapping and localization qualitative states ($S^{ij:k}$ and
$S^{i,j:X}_{1:n}$) are unknown. In this work, we infer them within a Bayesian
framework using a set of measurements
$Z^{ABC}_{1:n}=\\{Z^{ABC}_{j};j\in\\{1,...,n\\}\\}$ from time steps $1,...,n$.
The observation of landmarks $A,B,C$ from time instant $j$ is $Z^{ABC}_{j}$.
We note that while, for simplicity, this notation suggests observations are
assumed to exist for all time instances $[1,n]$, in practice, our method does
not require this assumption, as further elaborated in Section 5.1.

Herein, we also introduce an innovative usage of a motion model within a
qualitative formulation, which extends previous work. As will be seen, this
enables us to use more substantial geometric constraints to improve
estimation. Thus, we assume action $a_{n-1}$ for moving the camera between
time instances $n-1$ and $n$.

Our goal at time instant $n$ is to estimate the posterior probabilities of
qualitative landmark C and camera states, given history
$H^{ABC}_{n}\doteq\\{Z^{ABC}_{1:n},a_{1:n-1}\\}$:

$\mathbb{P}({S^{AB:C}|H^{ABC}_{n}})\ \ ,\ \
\mathbb{P}({S^{AB:X}_{1:n}|H^{ABC}_{n}}).$ (1)

The formulation in this paper is general and can use any measurement and
motion model (although we use the Markov assumption). We consider 2D
coordinate systems and a monocular camera setup in our implementation and
results. Therefore landmark 2D location is $L^{AB:C}=(x^{AB:C},y^{AB:C})$ and
camera pose 2D is $X^{AB}_{n}=(x^{AB:X}_{n},y^{AB:X}_{n},\alpha^{AB:X}_{n})$,
where $\alpha$ is the camera orientation angle. Measurements are bearing
angles to landmarks $A,B$ and $C$, i.e.
$Z^{ABC}_{n}=\\{\phi^{A}_{n},\phi^{B}_{n},\phi^{C}_{n}\\}$.

For this setup, we assume a Gaussian measurement model for $i\in\\{A,B,C\\}$:

$\mathbb{P}({\phi^{i}_{n}|L^{i},X_{n}})\propto\exp\left\\{-\frac{1}{2}\|\phi_{n}^{i}-f(L^{i},X_{n})\|^{2}_{\Sigma_{v}}\right\\},$
(2)

where $\Sigma_{v}$ is the measurement noise covariance and

$f(L^{i},X_{n})\doteq\arctan(\frac{y^{i}-y^{X}_{n}}{x^{i}-x^{X}_{n}}).$ (3)

Considering bearing measurements to different landmarks statistically
independent, the joint likelihood for $Z^{ABC}_{n}$ is readily obtained as a
product of individual likelihood terms (2) for each bearing measurement
$\phi_{n}^{i}\in Z^{ABC}_{n}$.

In our setup, we consider a simple motion model that can be used with a
monocular camera and basic azimuth-keeping control. Specifically, we consider
the robot is moving along a specific heading $a_{n}=\psi_{n}$ relative to the
previous camera pose and assume Gaussian noise as in

$\mathbb{P}({\\!X^{AB}_{n}|X^{AB}_{n-1},a_{n-1}\\!})\\!\propto\\!\exp\\!\left\\{\\!\\!-\frac{1}{2}\|a_{n-1}\\!-\\!g(X^{AB}_{n}\\!,\\!X^{AB}_{n-1})\|^{2}_{\Sigma_{w}}\\!\right\\}\\!\\!,$
(4)

where $\Sigma_{w}$ is the motion (process) noise covariance and $g(.)$ is
defined, similarly to Eq. (3), as
$g(X_{n},X_{n-1})\doteq\arctan(\frac{y^{X}_{n}-y^{X}_{n-1}}{x^{X}_{n}-x^{X}_{n-1}})$.
The motion model (4) does not constrain the camera orientation. As will be
seen, incorporating a motion model, even as simple as this, leads to a much
better qualitative state estimation.

The second part of our work (section 5) addresses the entire qualitative map
of all landmarks and how to propagate information between landmark triplets in
this map. The environment is a set of $m$ landmarks; we denote the group of
all possible landmark triplets as $M_{all}=\\{S^{i,j:k}\\}_{i,j,k=1...m;j\neq
i,k\neq j,i}$. The qualitative map is then a subset of the triplets that we
are interested in $M=\\{S^{i,j:k}\\}\subseteq M_{all}$. These can be triplets
that have been observed or otherwise ones we want to estimate (see Figures 7a
and 7b).

In section 5, we also approach the propagation of information in the map.
Propagating information is the process of inferring a specific triplet state
given its history and the history of other triplets:
$\mathbb{P}({S^{AB:D}|H^{AB:C},H^{BC:D},H^{AB:D}})$, or alternatively given
only the estimation of other triplets qualitative state
$\mathbb{P}({S^{AB:D}|S^{AB:C},S^{BC:D},S^{AB:D}})$. In this section, we will
use a shortened notation to refer to triplets to simplify formulation. Instead
of specifying the two frame landmarks and the third one: AB:C, we just refer
to the triplet as $ti$ for $i=1,2,...$; so for example, $AB:C\equiv
t1,BC:D\equiv t2,CD:E\equiv t3$ and this way we annotate
$S^{AB:C},S^{BC:D},S^{CD:E}$ as $S^{t1},S^{t2},S^{t3}$ and
$H^{AB:C},H^{BC:D},H^{CD:E}$ as $H^{t1},H^{t2},H^{t3}$. In this notation a map
with $k$ triplets is notated as $M=\\{S^{ti}\\};i=1...k$.

##  3 Approach overview

In this work, we present our probabilistic qualitative localization and
mapping approach. The first part of our work (section 4) discusses the single
triplet problem. First, we generally formulate the probabilistic inference of
camera and landmark triplet qualitative states when seen from multiple views
and consider a motion model. We then address the 2D problem as a test case in
more detail. We analyze the problem geometry and describe our unique
qualitative inference algorithms (sections 4.3 and 4.4).

The second part of our work (section 5) addresses data propagation between
different triplets in the qualitative map. We derive a novel probabilistic
composition algorithm for propagating information between different landmark
triplets in the qualitative map. We formulate it using a factor graph
representation. Then, we analyze the dependency of propagated information on
the graph topology and prior knowledge. We use unique information metrics and
graph propagation algorithms.

Section 6 includes test results and analysis for both parts of our approach
and comparison to state-of-the-art.

##  4 Single Triplet

As specified in Section 2, our approach considers multiple landmark-centric
triplet frames. In this section, we focus on a single triplet of landmarks
$A,B$, and $C$ viewed together from multiple views (at time steps
$1,\ldots,n$). In section 4.1, we infer camera and landmark qualitative states
given multiple views of the landmarks and a motion model. Then, we address the
2D SLAM problem as a test case in more detail. We discuss the geometric
properties of the 2D problem (section 4.2) and present our unique algorithm
for solving it in detail. In addition, we develop a very fast approximated
algorithm that achieves good performance utilizing the advantages of
qualitative inference (sections 4.3 and 4.4).

###  4.1 Probabilistic Formulation

Let us look at a single triplet of landmarks $A,B$, and $C$ viewed together at
time steps $1,\ldots,n$. When considering a specific $AB:C$ local frame, we
aim to estimate the qualitative state of landmark C and the camera at each
time step using measurements (1). The state-of-the-art [23] directly
formulates this problem, considering only landmark to camera measurements. We
notice that using other types of information may improve inference and
therefore adopt a different approach.

The qualitative problem is derived from an underlying fundamental metric SLAM
problem. This underlying problem is a small three landmark multiple view SLAM
problem of determining camera poses and landmark C location, given a set of
noisy measurements $\mathbb{P}({X^{AB}_{1:n},L^{AB:C}|Z^{ABC}_{1:n}})$. It is
well known that incorporating a motion model makes the problem easier to solve
and requires fewer measurements and less prior knowledge. This insight is also
valid for the qualitative problem. We, therefore, take a more general
formulation than the one in [23], which enables us to naturally translate the
effect of the motion model into the qualitative problem. We do this by
formulating the qualitative problem directly related to the underlying metric
problem.

Given the landmark measurements (2) and motion model (4) for each camera
transition, we want to infer the posterior probabilities of the landmark $C$
qualitative state and camera qualitative trajectory (1), both in the $AB$
frame. To reduce clutter, we drop the superscript ${AB}$ notation in this
section as long as everything is in the $AB$ frame. Also, for an easier
explanation, instead of looking at $\mathbb{P}({S^{C}|H_{n}})$ we look at the
separate components of this random vector: $\mathbb{P}({s_{i}^{C}|H_{n}})$
(i.e., the probabilities of the landmark $C$ to be in each qualitative state
separately). Generalizing to $\mathbb{P}({S^{C}|H_{n}})$ is trivial.

We formulate $\mathbb{P}({s_{i}^{C}|H_{n}})$ through the underlying metric
problem. To do so, we start by marginalizing over the metric camera poses and
landmark locations, writing the belief over $s^{C}_{i}$ as:

$\mathbb{P}({s_{i}^{C}|H_{n}})=\iint\displaylimits_{X_{1:n},L^{C}}{\mathbb{P}({s_{i}^{C},X_{1:n},L^{C}|H_{n}})dL^{C}dX_{1:n}}.$
(5)

We shall now apply chain rule:

$\\!\begin{multlined}\mathbb{P}({s_{i}^{C}|H_{n}})\\!\\!=\\!\\!\\!\\!\\!\\!\iint\displaylimits_{X_{1:n},L^{C}}{\\!\\!\\!\\!\mathbb{P}({s_{i}^{C}|X_{1:n},L^{C}\\!\\!,H_{n}})\mathbb{P}({X_{1:n},L^{C}|H_{n}})}dL^{C}dX_{1:n}.\end{multlined}\mathbb{P}({s_{i}^{C}|H_{n}})\\!\\!=\\!\\!\\!\\!\\!\\!\iint\displaylimits_{X_{1:n},L^{C}}{\\!\\!\\!\\!\mathbb{P}({s_{i}^{C}|X_{1:n},L^{C}\\!\\!,H_{n}})\mathbb{P}({X_{1:n},L^{C}|H_{n}})}dL^{C}dX_{1:n}.$
(6)

Note that landmark C metric location $L^{C}$ uniquely determines its
qualitative state $s_{i}^{C}$ so that $\mathbb{P}({s_{i}^{C}|L^{C}})=1$ for
$L^{C}\in s_{i}^{C}$ and 0 else. Since $\mathbb{P}({s_{i}^{C}|L^{C}})$ is
independent of any other history and can also be replaced by the corresponding
integration range, we get:

$\displaystyle\mathbb{P}({s_{i}^{C}|H_{n}})$
$\displaystyle=\iint\displaylimits_{X_{1:n},L^{C}}{\mathbb{P}({s_{i}^{C}|L^{C}})\mathbb{P}({X_{1:n},L^{C}|H_{n}})dL^{C}dX_{1:n}}$
$\displaystyle=\iint\displaylimits_{L^{C}\in
s_{i}^{C},X_{1:n}}{\mathbb{P}({X_{1:n},L^{C}|H_{n}})dL^{C}dX_{1:n}}.$ (7)

Similarly, inferring the camera’s qualitative state:

$\mathbb{P}({s_{i}^{X_{i}}|H_{n}})=\\!\\!\iint\displaylimits_{X_{i}\in
s_{i},X_{1:n/i},L^{C}}{\\!\\!\\!\\!\mathbb{P}({X_{1:n},L^{C}|H_{n}})dL^{C}dX_{1:n}}.$
(8)

We get an intuitive result: solve $\mathbb{P}({X_{1:n},L^{C}|H_{n}})$ \- the
corresponding SLAM problem in the $AB$ frame and marginalize over camera
trajectory $X_{1:n}$ and landmark C metric locations $L^{C}$ that belong to
the relevant qualitative state.

This approach is very different from previous works [23], [22]. It has several
advantages: (i) Summing over qualitative states can be trivially adjusted to
any space partition (see Figure 2). (ii) Solving the _small_ metric SLAM
problem can be done using any existing method or code to fit different
applications or scenarios.

We now further break down the SLAM problem $\mathbb{P}({X_{1:n},L^{C}|H_{n}})$
into simpler factors using a standard SLAM formulation and show how to in-
cooperate a motion model. Applying Bayes’ theorem and recalling the
measurement model $\mathbb{P}({Z_{n}|X_{n},L_{C}})$ is independent of history
$H_{n}$ gives:

$\mathbb{P}({X_{1:n},L_{C}|H_{n}})\\!=\\!\frac{1}{\zeta_{n}}\mathbb{P}({Z_{n}|X_{n},L_{C}})\\\
\mathbb{P}({X_{1:n},L_{C}|H_{n}^{-}}).$ (9)

Where $H_{n}^{-}\doteq\\{a_{1:n-1},Z_{1:n-1}\\}$ is the history without
measurements from current time, such that $H_{n}=H_{n}^{-}\cup\\{Z_{n}\\}$. In
addition, $\zeta_{n}\doteq\mathbb{P}({Z_{n}|H_{n}^{-}})$ is independent of
integration variables, so we can normalize outside the integrals.

We proceed by using the formula of total probability over $X_{n}$, and the
Markov property of the motion model to get the following recursive
formulation:

$\mathbb{P}({X_{1:n},L_{C},H_{n}})=\frac{1}{\zeta_{n}}\mathbb{P}({Z_{n}|X_{n},L_{C}})\mathbb{P}({X_{n}|X_{n-1},a_{n-1}})\mathbb{P}({X_{1:n-1},L_{C}|H_{n-1}}).$

Repeating these two steps $n-1$ times we get

$\mathbb{P}({X_{1:n},L_{C}|H_{n}})=\frac{\mathbb{P}({Z_{1}|X_{1},L_{C}})\mathbb{P}({X_{1},L_{C}})}{\mathbb{P}({Z_{1}})}\prod_{i=2}^{n}{\frac{1}{\zeta_{i}}\mathbb{P}({Z_{i}|X_{i},L_{C}})}\mathbb{P}({X_{i}|X_{i-1},a_{i-1}}),$
(10)

where $\zeta_{i}\doteq\mathbb{P}({Z_{i}|a_{1:i-1},Z_{1:i-1}})$ is the
normalization factor. Using this standard decomposition, we can see how
measurement and motion models can solve the underlying SLAM problem.

Recalling Eqs. (7) and (8), we can now integrate over the camera trajectory
and landmark C location to estimate the required qualitative states.

###  4.2 Basic Problem Geometry

After formulating the problem, we look at the 2D SLAM problem as a test case.
In the following sections (4.3, 4.4), we suggest novel algorithms for solving
this problem while taking advantage of qualitative geometry’s unique
properties to simplify the inference. This section lays the basics for
understanding our motivation and choices. We analyze the geometric properties
of the 2D problem and the effect of incorporating a motion model on the
solution quality.

As specified in Section 2, our approach considers multiple landmark-centric
triplet frames. For each local frame, the underlying fundamental metric
problem $\mathbb{P}({X^{AB}_{1:n},L^{AB:C}|Z^{ABC}_{1:n}})$ determines camera
poses and landmark C location in the local AB frame, given a set of
measurements. When considering measurement noise, the problem is a small
metric three landmark - multi-view SLAM problem. This SLAM problem is
observable only with enough landmark measurements and camera poses or with
priors. The thing is that achieving enough measurements or priors for each
local frame can be problematic in some robotic platform scenarios.

While the state-of-the-art only uses measurements, we consider the extended
problem $\mathbb{P}({X^{AB}_{1:n},L^{AB:C}|Z^{ABC}_{1:n},a_{1:n-1}})$ and
incorporate the corresponding motion model. The motion model enables solving
the problem better and with fewer measurements. A simple degree of freedom
analysis can demonstrate this for the 2D case. The unknowns are landmark $C$
location $(x_{C},y_{C})$, and camera poses $(x_{1:n},y_{1:n},\alpha_{1:n})$.
For $n$ time steps, we have $3n+2$ unknowns. When using only azimuth
measurements $(\phi^{A}_{1:n},\phi^{B}_{1:n},\phi^{C}_{1:n})$ to landmarks
$A,B$ and $C$, the number of equations is $3n$. This problem is, therefore,
under-determined. When considering actions $a_{1:n-1}$ (i.e. the azimuth
angles from one camera pose to the next, $\psi_{1:n-1}$) and the corresponding
motion model (4), the number of equations is $4n-1$. For $n\geq 3$, the number
of equations equals or exceeds the number of unknowns, and the problem becomes
fully observable. The intuition is that incorporating a motion model enables
solving landmark $C$ location by triangulating line-of-sight vectors from
multiple views.

This well-known insight related to the metric problem is also valid for the
qualitative problem. Therefore, incorporating a motion model will solve the
qualitative problem with fewer measurements and prior knowledge. In sections
4.4 and 6.3 we will look at the effect in more detail.

We now dive into the 2D problem basic geometry to better understand our
algorithm and the effect of using a motion model in sections 4.4 and 4.5. We
look at the basic deterministic geometry, so in this explanation, we consider
the ideal noise-free measurement and motion models that we shall denote by
$\bar{Z}$.

First, we regard the single-view case. Landmarks A and B are known (we work in
the AB frame), while the camera pose and landmark C are unknown. Determining
camera pose by 2D azimuth measurements to two known landmarks is a well-known
2D 2-point camera resection problem. [40] specifies a complete and efficient
analytic solution to this problem. Given these azimuth measurements, the
camera’s location must be on a _circle_ that goes through the two landmarks A
and B. [40] gives the equations for the ”locus circle” center and radius.
Azimuth ordering of $\phi^{A}_{n},\phi^{B}_{n}$ further confines the camera
location to the left or right to the AB vector, see Figure 4a. It is also easy
to show that assuming a specific camera location on the locus circle directly
determines the camera orientation $\alpha_{n}$:

$\alpha_{n}=\arctan(\frac{y_{n}^{X}}{x_{n}^{X}})+\phi_{A}=\arctan(\frac{y^{X}_{n}-1}{x^{X}_{n}})+\phi_{B}.$
(11)

Per every possible camera pose, landmark C can be located somewhere on the
line of sight corresponding to the azimuth measurement $\phi^{C}_{n}$. A
diagram illustrating these aspects is given in Figure 4a, while Figures 5a and
5b have a stimulative example. A crucial insight is that even in this
deterministic setting, the problem is not fully observable, i.e. solutions for
metric camera poses and landmark locations have a continuous distribution. We
denote this joint distribution by
$\mathbb{P}({X^{AB}_{n},L^{AB:C}|\bar{Z}^{ABC}_{n}})$.

If measurements from several time instances are available, the joint pdf is:

$\mathbb{P}({X^{AB},L^{AB:C}|\bar{Z}^{ABC}_{1:n}})=\\!\prod^{n}_{i=1}{\mathbb{P}({X^{AB},L^{AB:C}|\bar{Z}^{ABC}_{i}})}.$
(12)

The posterior distribution over qualitative states can be extracted by
integration.

(a)

(b)

(c)

Figure 4:  (a) 2D 2-point camera resection from single view. (b) camera
resection and landmark triangulation with two views. (c) camera resection and
landmark triangulation with three views.

Now we examine the impact of incorporating a motion model. The second camera
location $(x_{2}^{*},y_{2}^{*})$ is determined by the combination of the first
camera pose $(x_{1}^{*},y_{1}^{*},\alpha^{*}_{1})$, the action $\psi_{1}$, and
the second measurement $Z_{2}=\\{\phi^{A}_{2},\phi^{B}_{2},\phi^{C}_{2}\\}$
(obtained after executing the action). It is the intersection of the line of
motion with the valid part of the second measurement locus circle (see Figure
4b). The intersection can occur once, twice, or not at all. As a result, some
of the first camera poses are disqualified. Another consequence is geometric
ambiguity: some measurements can support two solutions for the second camera
pose (and the location of landmark C correspondingly).

Given the two camera poses
$(x_{1}^{*},y_{1}^{*},\alpha_{1}^{*}),(x_{2}^{*},y_{2}^{*},\alpha_{2}^{*})$
and azimuth measurements $\phi^{C}_{1},\phi^{C}_{2}$ to landmark C, its
location $(x_{c}^{*},y_{c}^{*})$ can be triangulated. Considering all possible
poses for the first camera reduces the possible locations landmark C to a
curve (might be split into two curves in case of geometric ambiguity) - see
Figure 4b. A simulation example that displays how our motion model allows
disqualifying a part of the first camera poses and triangulating the landmark
C - see Figure 4b.

When considering three or more measurements, only one or a few discrete
possible locations for landmark C and the corresponding camera trajectories
are left. Triangulation consistency further disqualifies most camera poses and
landmark locations. (Figure 4c). The surviving landmark C locations are an
intersection of the curve estimates for pairs of consecutive views.

Using the motion model significantly increased the quality of the solution for
the metric problem. In the probabilistic case, the probability for qualitative
states should also improve as it is an integration on the metric pdf. This
fact demonstrates the motivation for our work.

We also notice that the geometric ambiguity enables several distinct solutions
to the problem. Therefore probabilistic solutions based on linearization might
converge to local minima and achieve subpar results. A global solution or
hypothesis-based approach is therefore needed.

(a)

(b)

(c)

Figure 5:  Simulation example for first camera resection and landmark
estimation: Actual camera positions and landmark positions are marked in
circles. (a) First view. Estimated camera position marked by blue +. Estimated
landmark C location marked in magenta (.) - see the legend. (b) Second view.
Same markings. (c) Estimation by both views using motion model. Estimated
camera position marked by +,x. Estimated landmark C location marked in magenta
circles (see legend).

###  4.3 Algorithm Design Considerations

Standard SLAM approaches usually solve big bundle adjustment problems with
many landmarks and camera poses. Achieving a global solution for such problems
is hard. Therefore, It is common to model the estimated variables as multi-
variate Gaussian and resort to linearization-based local optimum. These
approaches require enough measurements and prior knowledge or an initial guess
for the solution.

Alternatively, we solve many small landmark-centric SLAM problems with three
landmarks, and a few camera poses at a time. We also want to work with a small
number of measurements and no prior knowledge. Under these conditions, we
choose a sampling-based approach to solve the _global_ non-linear _small_ SLAM
problem. This approach avoids linearization and gets a robust global solution.
In addition, it does not need prior information or an initialization stage.
Also, integration over coarse resolution qualitative states compensates for
some sampling error. This robust and much simpler algorithmic flow is well
suited for low-compute platforms (see figure 6).

This approach can be computationally expensive for a large SLAM problem, but
it is feasible for many small problems. Furthermore, as discussed in chapter
4.2, using a motion model and triangulation allows us to disqualify
inconsistent samples rapidly, drastically reducing valid samples for two or
more views, making this method fast.

(a)

(b)

Figure 6:  (a) general algorithmic flow of standard multi-variate Gaussian
linearization based algorithm. (b) general algorithmic flow of direct global
sample based algorithm.

###  4.4 Detailed Algorithm

We now describe our algorithm for estimating a single landmark triplet
observed from multiple views using noisy measurements and a motion model.
Usually, SLAM solvers use a camera-centric global frame. We use a different,
landmark-centric frame by fixing landmarks $A=(0,0)$ and $B=(0,1)$, as we
believe it is more intuitive to infer qualitative landmark-related states
(Figure 3). [40] gives a complete and efficient solution to the
_deterministic_ problem.

First, we introduce a few notations to help describe the algorithm. While the
random variable $X_{n}$ represents camera pose at time $n$, we denote
_samples_ of this camera pose as $X^{(k_{n})}_{n}$, where $n$ and $k_{n}$ are,
respectively, time and sample indices. We also define a ”trajectory
hypothesis” $th^{j}_{n}\doteq\\{X^{(k_{1})}_{1},\dots,X^{(k_{n})}_{n}\\}$ as a
set of specific camera pose samples - one for each time step. Index
$j\mapsto\\{k_{1},\ldots,k_{n}\\}$ is a simplified notation for a trajectory
hypothesis. With a slight abuse of notation, we denote $X^{j}_{l}$ as the
camera pose sample from time $l$ in trajectory hypothesis $th^{j}_{n}$. The
set of all trajectory hypotheses at time $n$ is
$TH_{n}\doteq\\{th^{j}_{n}\\}$.

To estimate the qualitative state of a landmark triplet with measurements from
$n$ time steps, we iteratively apply a three-stage algorithm for each time
step:

_Sampling step:_ Generate $m_{n}$ camera pose samples for time $n$
$X^{(k_{n})}_{n}$, with $k_{n}\in[1,m_{n}]$. We samples from the distribution
$\mathbb{P}({X_{n}|\phi_{n}^{A},\phi_{n}^{B}})$ using bearing measurements
$\phi_{n}^{A}$ and $\phi_{n}^{B}$ to landmarks $A$ and $B$. Given these
measurements, the 2D camera pose is on a specific part of a _circle_ that goes
through the two landmarks A and B (see Figure 4a). [40] specifies the
calculation of the _locus circle_ parameters. Camera poses are sampled in the
vicinity of this locus circle considering the noisy nature of $\phi_{n}^{A}$
and $\phi_{n}^{B}$.

_Motion step:_ For each trajectory hypothesis $th^{j}_{n-1}\in TH_{n-1}$, we
can use $X^{j}_{n-1}$ and motion azimuth $\psi_{n-1}$ to intersect the locus
circle from time $n$ (see Figure 4b). Camera pose samples $X^{(k_{n})}_{n}$
that are consistent with this intersection are found, also considering the
noisy nature of the motion model (4). Using these matches, we generate
multiple new, extended trajectory hypotheses $th^{j}_{n}$. For each
$th^{j}_{n}$, we also calculate and keep motion model consistency wight:
$wm^{j}_{n}=\mathbb{P}({X^{j}_{n}|X^{j}_{n-1},\psi_{n-1}})$.

_Resection step:_ For each valid trajectory hypothesis $th_{n}^{j}$, we test
the consistency of the bearing measurements from all cameras to landmark C.
First, we use camera poses $X^{j}_{1:n}$ and bearing measurements
$\phi^{C}_{1:n}$ to triangulate landmark C location, denoted by $L^{C,j}$. It
is estimated to be the centroid of all line of sight pairs intersection points
(see Figure 4b). Then we estimate a triangulation weight as the probability
for this configuration:
$wr^{j}_{i}=\mathbb{P}({\phi^{C}_{i}|L^{C,j},X^{j}_{i}}),\forall
i\in\\{1...n\\}$ (assuming independent measurement noise). We disqualify
trajectories that do not intersect or have low probability.

Thus far, the results of our algorithm are the set of valid trajectory
hypotheses $th^{j}_{n}\in TH_{n}$. For each , $th^{j}_{n}$ we keep a single
landmark location hypothesis $L^{C,j}$ along with corresponding consistency
weights $wm^{j}_{i}$ and triangulation weights $wr^{j}_{i}$ $\forall
i\in\\{1...n\\}$.

_Note:_ Using a single $L^{C,j}$ for each $th^{j}_{n}$ is a heuristic. We can
sample over the area of line-of-sight intersections to cover all probable
$L^{C}$ locations. We choose the simpler way for run-time considerations.

_Note:_ If we have measurements from only one time step, we sample landmark C
location $L^{C}$ around each sampled camera pose $X_{1}^{(k1)}$ line of site.
We consider measurement noise and sample from
$\mathbb{P}({L^{C}|X_{1}^{(k1)},\phi_{1}^{C}})$.

Finally, we approximate Eqs. (7) and (8) by summing over each qualitative
state to get state probability distribution (see the similarity to (10)):

$\mathbb{P}({s_{k}^{C}|H_{n}})\approx\eta^{C}\sum\displaylimits_{j}{\mathds{1}(L^{C,j}\in
s_{k}^{C})wr_{1}\prod_{i=2}^{n}wm^{j}_{i}wr^{j}_{i}},$ (13)
$\mathbb{P}({s_{k}^{X_{i}}|H_{n}})\approx\eta^{X_{i}}\sum\displaylimits_{j}{\mathds{1}(X^{j}_{i}\in
s_{k}^{X_{i}})wr_{1}\prod_{i=2}^{n}wm^{j}_{i}wr^{j}_{i}}.$ (14)

Here, $\eta^{C}$ and $\eta^{X_{i}}$ are normalization constants, and the sum
is over all trajectory hypotheses $th_{n}^{j}\in TH_{n}$. Note the term
$wr_{1}\prod_{i=2}^{n}wm^{j}_{i}wr^{j}_{i}$ is a sampled approximation of the
joint pdf $\mathbb{P}({X_{1:n},L_{C}|H_{n}})$.

This algorithm seems to handle many trajectory hypotheses that grow
exponentially in time. Practically, the geometric constraints enforced in the
”motion” step and the ”resection” step dramatically decrease the number of
hypotheses for two views and even more for three views or more, making our
algorithm much faster (experiment example in figure 18). This reduction is an
effect of incorporating a motion model into the qualitative estimation. It
also means that using this algorithm incrementally requires saving only a
small number of hypotheses and therefore is not memory intensive. A pseudo-
code for a simplified version of this approach is in Algorithm 1.

Algorithm 1 Single triplet qualitative state estimation

1:sample camera poses
$X^{(k_{1})}_{1}\sim\mathbb{P}({X_{1}|\;\phi_{1}^{A},\,\phi_{1}^{B}})$ with
$k_{1}\in[1,m_{1}]$

2:initialize trajectory hypothesis set: $TH=\\{X^{(k_{1})}_{1}\\}$

3:for $i=1,...,n$ do

4: // sampling step:

5: sample camera pose
$X^{(k_{i})}_{i}\sim\mathbb{P}({X_{i}|\;\phi_{i}^{A},\,\phi_{i}^{B}})$ with
$k_{i}=[1,m_{i}]$

6: // motion model step:

7: \- $\forall th^{j}_{i-1}$ find $X^{j}_{i}$ that are consistent with motion
azimuth $\psi_{i-1}$

8: \- extend $TH_{i}$ with new matches

9: \- keep consistency weight:
$wm_{i}^{j}=\mathbb{P}({X^{(k_{i})}_{i}|\;X^{j}_{i-1},\,\psi_{i-1}})$

10: // resection step:

11: \- $\forall\;th^{j}_{i}$ find $L^{C,j}$ by line of sight triangulation

12: \- keep consistency weight:
$wr_{t}^{j}=\mathbb{P}({\phi^{C}_{t}|\;L^{C,j},\,X^{j}_{t}}),\,\forall\,t\in\\{1...i\\}$

13: \- dismiss low probability hypotheses

14:end for

15:estimate landmark qualitative state probability via Eq. (13)

16:estimate camera qualitative state probability via Eq. (14)

_Remark:_ This algorithm is simplified for the sake of explanation. Our actual
implementation is more optimized.

###  4.5 Faster Variant Algorithm

We seek to utilize further the ability of the qualitative coarse resolution to
absorb errors, aiming to achieve an even faster algorithm. With this
motivation in mind, we introduce an approach similar to the sampling-based
approach described above. However, sampling is done only to represent the
prior term in Eq. (10). No sampling is done to represent the motion model or
measurement noise.

In Section 6.3, we evaluate this algorithm variant to see how many noise-
related errors it can handle. As we shall see, due to the inherent properties
of qualitative estimation, it can handle reasonable noise levels with accuracy
similar to the full algorithm and with a significant speedup.

##  5 Data Propagation in a Qualitative Map

Up until now, we discussed how to solve a single landmark triplet problem. We
showed how to estimate the qualitative state of the camera and landmarks in
landmark relative frames.

The second part of our work addresses the multiple landmark qualitative map
and how to propagate data between different landmark triplets in this map. We
start with the basic concept of _qualitative composition_ \- an inherent
geometric attribute of landmark triplets with partially common landmarks to
transfer qualitative data between them (section 5.1). Qualitative data
propagation was first approached in the seminal work [22] but in a basic non-
probabilistic sense. We are the first ones (as far as we know) to formulate
probabilistic composition. We also formulate the connection between basic
qualitative composition inference to the full solution of the problem of
propagating information in a qualitative map given prior data. Then in chapter
5.2 we discuss the representation of a qualitative map as a factor graph.
Finally, in section 5.3, we present a study of how composition propagates data
in the qualitative factor graph regarding graph topology and prior knowledge,
including novel information metrics and an algorithm for graph propagation we
developed for this end.

###  5.1 Propagating Data Between Triplets - Probabilistic Composition

In [22], it is noted that if three landmark triplets share two common
landmarks between each pair (e.g., AB:C, BC:D, AB:D), there are intrinsic
geometric constraints about the joint feasibility of the three qualitative
states $(s^{AB:C}_{i},s^{BC:D}_{j},s^{AB:D}_{k})$ (without referring to any
prior knowledge). Applying these constraints to infer the joint qualitative
state is called _composition_. Composition enables enhanced estimation for
existing overlapping triplets and to infer triplets never viewed directly (see
Figure 7). Among other things, this is an essential basis for managing large-
scale qualitative maps and active qualitative planning.

However, [22] only refers to the non-probabilistic joint feasibility of
qualitative EDC states. Also, [22] does not formulate the general problem of
propagating data between the three triplets given measurement history or show
how composition relates to it. In this section, we develop a probabilistic
composition variant and show how to use it for data propagation. We also
formulate the general problem of inferring the joint qualitative states
probability given measurements
$\mathbb{P}({S^{AB:C},S^{BC:D},S^{AB:D}|H_{n}^{AB:C},H_{n}^{BC:D},H_{n}^{AB:D}})$.
Then we show how composition-based inference relates to the full solution.

(a)

(b)

(c)

Figure 7: qualitative map: (a) Landmarks A,B,C observed from camera pose 1.
Landmarks B,C,D observed from camera pose 2. (b)qualitative map is represented
as landmark triplet graph (c) Composition: try to estimate triplet AB:D given
only AB:C and BC:D estimations (for unobserved triplets, or improving existing
estimations).

For the sake of readability, we simplify notations in this section. We drop
the time index $n$ and assume all history is considered. We also use shortened
notations for triplets, as shown in section 2. AB:C, BC:D, AB:D are denoted as
$t1,t2$ and $t3$ respectively. So for example: qualitative states is denoted
$S^{AB:C}\equiv S^{t1}$, history is denoted $H^{AB:C}\equiv H^{t1}$, and the
landmark metric location is denoted $L^{AB:C}\equiv L^{t1}$.

We address inferring joint qualitative probability
$\mathbb{P}({S^{t1},S^{t2},S^{t3}|H_{n}^{t1},H_{n}^{t2},H_{n}^{t3}})$, where
triplets AB:C, BC:D, AB:D are denoted as $t1,t2$ and $t3$ respectively.
Specifically, this formulation is also valid in case some of the triplets have
no observations, e.g.,
$\mathbb{P}({S^{t1},S^{t2},S^{t3}|H_{n}^{t2},H_{n}^{t3}})$.

For the same reasons as in section 4.1, we consider the underlying 4-landmark
$(A,B,C,D)$ metric problem. We look at the joint probability of the three
triplets and marginalize the metric locations for each triplet:

$\\!\begin{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\iiint\displaylimits_{L^{p1},L^{p2},L^{t}}{\mathbb{P}({S^{t1},S^{t2},S^{t3},L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})dL^{t1}dL^{t2}dL^{t3}}.\end{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\iiint\displaylimits_{L^{p1},L^{p2},L^{t}}{\mathbb{P}({S^{t1},S^{t2},S^{t3},L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})dL^{t1}dL^{t2}dL^{t3}}.$

We break the integration argument using the formula of total probability:

$\\!\begin{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3},L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})=\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3},H^{t1},H^{t2},H^{t3}})\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}}).\end{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3},L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})=\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3},H^{t1},H^{t2},H^{t3}})\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}}).$

Note that given the metric location of the landmarks $L^{t1},L^{t2},L^{t3}$,
the qualitative states $S^{t1},S^{t2},S^{t3}$ are directly determined, and are
independent in anything else. Therefore we get the following:

$\\!\begin{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3},L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})==\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}}),\end{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3},L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})==\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}}),$

and overall:

$\\!\begin{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\\!\\!\\!\\!\\!\\!\iiint\displaylimits_{L^{t1},L^{t2},L^{t3}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})}dL^{t1}dL^{t2}dL^{t3}.\end{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\\!\\!\\!\\!\\!\\!\iiint\displaylimits_{L^{t1},L^{t2},L^{t3}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})}dL^{t1}dL^{t2}dL^{t3}.$
(15)

This formulation resembles equation (7). It gives a similar intuitive result:
For each combination of qualitative states $S^{t1},S^{t2},S^{t3}$, we solve
the metric 4-landmark SLAM problem
$\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})$, and integrate
probability distribution over all landmark location that corresponds with this
combination of qualitative states.

However, using this method in a sizeable multiple-landmark map will eventually
translate (when using variable elimination) to solving the big multi-landmark
SLAM metric problem. We want to use qualitative geometry’s unique properties
to find an approximate faster and more straightforward solution and still
achieve good results. Specifically, we want to replace the second factor in
equation (15) with a probabilistic version of the composition described in
[22].

Our main approximation is as follows:

  * •

Instead of considering the full 4-landmark$(A,B,C,D)$ problem, we estimate
each triplet’s qualitative state using only its own history.

  * •

When propagating information between different triplets, we use only the
estimated qualitative state of the triplets instead of the entire history.

This approximation might be considered a blunt for metric SLAM. However, we
will show that since we care only about the qualitative states, it is much
less influential and still enables considerable data propagation in the
qualitative map. On the other hand, it gives us several advantages looking at
the overall approach:

  1. 1.

Each triplet can be estimated individually using the incremental algorithm in
Section 4.

  2. 2.

Map propagation uses only the qualitative state for each triplet. So in a
large map, triplets can be saved efficiently using very little data (if no
intermediate estimation results are saved for incremental algorithms).

  3. 3.

We will show that the composition-based algorithm is straightforward and fast
and can be mostly calculated offline. Low computing is significant for online
map propagation since every new measurement propagates multiple times through
the map in each time step.

To apply these approximations for the second factor in equation (15), we start
by marginalizing over the qualitative states:

$\\!\begin{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3},s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}})}.\end{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3},s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}})}.$

Using the formula of total probability to break down the sum argument we get:

$\\!\begin{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k},H^{t1},H^{t2},H^{t3}})}\mathbb{P}({s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}}).\end{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k},H^{t1},H^{t2},H^{t3}})}\mathbb{P}({s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}}).$

The first part of our approximation uses only the qualitative states to
estimate landmark metric locations instead of the entire history:

$\\!\begin{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})\approx\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})\mathbb{P}({s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}})}.\end{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})\approx\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})\mathbb{P}({s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}})}.$

The second part of our approximation assumes that each qualitative state is
estimated only based on its own history and correlations between different
triplets are dropped:

$\\!\begin{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})\approx\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})}.\end{multlined}\mathbb{P}({L^{t1},L^{t2},L^{t3}|H^{t1},H^{t2},H^{t3}})\approx\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})}.$

We get the approximated
$\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})$ By
substituting this back to equation (15):

$\\!\begin{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})\approx\iiint\displaylimits_{L^{t1},L^{t2},,L^{t3}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})}\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})}\\\
\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})dL^{t1}dL^{t2}dL^{t3}\triangleq\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}}).\end{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})\approx\iiint\displaylimits_{L^{t1},L^{t2},,L^{t3}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})}\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})}\\\
\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})dL^{t1}dL^{t2}dL^{t3}\triangleq\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}}).$

Now we change the order of integration and sum:

$\\!\begin{multlined}\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})}\\\
\iiint\displaylimits_{L^{t1},L^{t2},L^{t3}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})}\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})dL^{t1}dL^{t2}dL^{t3},\end{multlined}\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})}\\\
\iiint\displaylimits_{L^{t1},L^{t2},L^{t3}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})}\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})dL^{t1}dL^{t2}dL^{t3},$

and we note that

$\\!\begin{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})=\begin{cases}1,&\text{if}\
L^{ti}\in S^{ti}\quad\forall i=1,2,3\\\
0,&\text{otherwise}\end{cases}.\end{multlined}\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})=\begin{cases}1,&\text{if}\
L^{ti}\in S^{ti}\quad\forall i=1,2,3\\\ 0,&\text{otherwise}\end{cases}.$ (16)

Therefore $\mathbb{P}({S^{t1},S^{t2},S^{t3}|L^{t1},L^{t2},L^{t3}})$ can be
translated into integral bounds:

$\\!\begin{multlined}\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\\\
=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})}\iiint\displaylimits_{L^{t1}\in
S^{t1},L^{t2}\in S^{t2},L^{t3}\in
S^{t3}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})}dL^{t1}dL^{t2}dL^{t3}.\end{multlined}\tilde{\mathbb{P}}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})=\\\
=\sum_{s^{t1}_{i}}\sum_{s^{t2}_{j}}\sum_{s^{t3}_{k}}{\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})}\iiint\displaylimits_{L^{t1}\in
S^{t1},L^{t2}\in S^{t2},L^{t3}\in
S^{t3}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})}dL^{t1}dL^{t2}dL^{t3}.$

This approximate formulation is very enlightening. The first term is the
stand-alone estimation of each triplet separately:
$\mathbb{P}({s^{t1}_{i}|H^{t1}})\mathbb{P}({s^{t2}_{j}|H^{t2}})\mathbb{P}({s^{t3}_{k}|H^{t3}})$
as detailed in section 4.1 equation 7. The second term is _qualitative
composition_. It is an intrinsic geometric constraint between the triplets
independent of measurements. It represents the probability of each specific
combination of qualitative states:
$\\{s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}\\}\forall i,j,k\in\\{1...m\\}$. Therefore
it can also be calculated _offline_ for each of the $m^{3}$ combinations of
the three qualitative states.

We implement this _offline_ calculation using a semi-sample-based approach.
For each qualitative state combination
$(s^{AB:C}_{i},s^{BC:D}_{j},s^{AB:D}_{k})$ we sample $L^{AB:C}$ from
$s^{AB:C}_{i}$ uniformly. Then we calculate the intersection area of the
geometric polygons that fit $s^{BC:D}_{j}\cap s^{AB:D}_{k}$ in the $AB$ frame
(for a specific qualitative space partition). Finally, we sum the intersection
areas for all samples and normalize the probability for all
$(s^{AB:C}_{i},s^{BC:D}_{j},s^{AB:D}_{k})$ combinations to sum up to 1. This
process is illustrated in figure 8 using Freska’s double cross space partition
[30]. In 8a,8b we see two different $(s^{BC:D}_{j},s^{AB:D}_{k})$ combinations
given a specific sample of $L^{AB:C}\in s^{AB:C}_{i}$, each with a different
intersection area. Some combinations of $(s^{BC:D}_{j},s^{AB:D}_{k})$ are
infeasible given a specific sample of $L^{AB:C}\in s^{AB:C}_{i}$ and have $0$
intersection area, and therefore $0$ probability (e.g. figure 8c). On the
other hand, some combinations have an infinite intersection area (e.g., figure
8d). If not handled correctly, this could zero out all finite intersection
combinations. Therefore we enforce integration borders for each landmark
triplet relative frame. We choose these borders to achieve as similar area
size for all states as possible and thereby minimize bias (different for each
qualitative space partition). As an example, for Freska’s double cross [30] we
use $-1<x<1,-1<y<2$. The deterministic qualitative composition in [22]
considers only the feasibility of qualitative state combinations. This is
equivalent to reducing the probabilities for each
$(s^{AB:C}_{i},s^{BC:D}_{j},s^{AB:D}_{k})$ combination to $1$ or $0$ (and
normalize).

(a)

(b)

(c)

(d)

(e)

Figure 8:  Example for our implementation of qualitative composition offline
calculation in section 5.1. This example uses Freska’s double cross space
partition (e). For each specific combination of qualitative states
$(s^{AB:C}_{i},s^{BC:D}_{j},s^{AB:D}_{k})$ we sample $L^{AB:C}\in
s^{AB:C}_{i}$ uniformly. Given each sample, we calculate the intersection area
of the polygons corresponding to $s^{BC:D}_{j}$ and $s^{AB:D}_{k}$ in the $AB$
frame. We sum for all $L^{AB:C}$ samples and normalize. Figures (a-d)
demonstrate a specific sample $L^{AB:C}\in S^{AB:C}=2$ and different
$s^{BC:D}_{j}$ and $s^{AB:D}_{k}$ combinations: (a) small intersection for
$S^{BC:D}=4$ and $S^{AB:D}=2$. (b) larger intersection for $S^{BC:D}=4$ and
$S^{AB:D}=4$. (c) No intersection for $S^{BC:D}=5$ and $S^{AB:D}=2$. (d)
Infinite intersection for $S^{BC:D}=2$ and $S^{AB:D}=2$ requires special
treatment.

The online calculation of equation 5.1 can now be very efficient. This
efficiency is more clearly demonstrated by using vector and matrix notations:

$\begin{split}&V^{ti}_{m\times 1}\triangleq\mathbb{P}({S^{ti}|H^{ti}}),\\\
&\\!\begin{multlined}T^{t1,t2,t3}_{m\times m\times
m}(i,j,k)\triangleq\iiint\displaylimits_{L^{t1}\in S^{t1},L^{t2}\in
S^{t2},L^{t3}\in
S^{t3}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})}dL^{t1}dL^{t2}dL^{t3}.\end{multlined}T^{t1,t2,t3}_{m\times
m\times m}(i,j,k)\triangleq\iiint\displaylimits_{L^{t1}\in S^{t1},L^{t2}\in
S^{t2},L^{t3}\in
S^{t3}}{\mathbb{P}({L^{t1},L^{t2},L^{t3}|s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}})}dL^{t1}dL^{t2}dL^{t3}.\end{split}$

Here
$\mathbb{P}({S^{t1}|H^{t1}}),\mathbb{P}({S^{t2}|H^{t2}}),\mathbb{P}({S^{t3}|H^{t3}})$
are denoted as a $m\times 1$ vectors, and the offline integral term in Eq. 5.1
for each of the $m^{3}$ combination of qualitative states
$\\{s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}\\}$ is denoted as a $m\times m\times m$
tensor. We also note that it is sparse to some degree, e.g. offline
composition for EDC space partition is a $T^{t1,t2,t3}_{20\times 20\times 20}$
tensor with 8000 elements in total, but only 2257 non-zero elements.

For example, if we want the probability for a specific combination of
qualitative states, equation 5.1 translates to:

$\\!\begin{multlined}\tilde{\mathbb{P}}({s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}})=V^{t1}(i)V^{t2}(j)V^{t3}(k)T(i,j,k),\end{multlined}\tilde{\mathbb{P}}({s^{t1}_{i},s^{t2}_{j},s^{t3}_{k}|H^{t1},H^{t2},H^{t3}})=V^{t1}(i)V^{t2}(j)V^{t3}(k)T(i,j,k),$
(17)

and if we want to calculate the marginal for a specific triplet, equation 5.1
translates to:

$\\!\begin{multlined}\tilde{\mathbb{P}}({S^{t1}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t2}_{i}}{\sum_{s^{t3}_{j}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t},H^{p1},H^{p2}})}}=(T_{m\times
m\times m}\times V^{t2}_{m\times 1})\times V^{t3}_{m\times
1}.\end{multlined}\tilde{\mathbb{P}}({S^{t1}|H^{t1},H^{t2},H^{t3}})=\sum_{s^{t2}_{i}}{\sum_{s^{t3}_{j}}{\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t},H^{p1},H^{p2}})}}=(T_{m\times
m\times m}\times V^{t2}_{m\times 1})\times V^{t3}_{m\times 1}.$ (18)

This is a speedy way to update joint probabilities and marginals since it uses
only (and not too many) mult-add operations. We will also see in sections
6.4.1 and 6.4.2 that although the composition is an approximation, it is
effective and still propagates significant amounts of qualitative data. Thus
our primary motivation to enable online data propagation in the qualitative
map for low-compute platforms is supported.

To summarize, in this section, we formulated the probabilistic qualitative
composition for the first time and discussed the exact meaning of the
approximation concerning the full solution for propagating data in the
qualitative map. Note that this formulation of qualitative composition is a
generalized version of the formulation from the early conference version of
this work [39]. We give here the full joint probability
$\mathbb{P}({S^{t1},S^{t2},S^{t3}|H^{t1},H^{t2},H^{t3}})$, instead of the
marginal $\mathbb{P}({S^{t1}|H^{t2},H^{t3}})$, and discuss the meaning of the
approximation and implementation in more detail.

###  5.2 Qualitative Map As a Factor Graph

We discussed the basic geometry of propagating data between landmark triplets
and a fast approximate algorithm. We will show how to use this in a factor
graph representation of the multi-landmark qualitative map to maintain and
propagate data.

Factor graphs are used in state-of-the-art SLAM (and active planning)
approaches as discussed in detail, e.g. in [41]. Generally speaking, a factor
graph is a bipartite graph $G$ that contains two types of nodes. Variable
nodes $V_{v}$ represent the random variables of the problem, and factor nodes
$V_{f}$ encode probabilistic constraints between different variables. The
edges in the graph $E$ connect each factor node to all the variable nodes it
engages (see Fig. 9):

$\begin{split}&G=(V,E)\\\ &V=V_{v}\cup V_{f}.\end{split}$

In our framework, the qualitative map is the collection of the qualitative
states of all landmark triplets we are interested in $M=\\{S^{i,j:k}\\}$. In
the current formulation, we do not include camera poses as part of the map for
simplicity. The unknown variable nodes, in this case, are the corresponding
qualitative states:

$V_{v}=\\{v_{v}^{ij:k}\\}\quad\forall S^{i,j:k}\in M.$

These landmark triplets can be those we have seen together and measured in the
past, denoted as $V_{v}^{seen}$, or unseen triplets $V_{v}^{unseen}$ that we
are interested in for accomplishing a specific task or mission such as
planning (see Fig. 9). Note that unseen triplets mean the landmarks were not
observed together from the same view but might have been observed separately
from different views.

$\begin{split}&V_{v}=V_{v}^{seen}\cup V_{v}^{unseen}\\\
&V_{v}^{seen}=\\{v_{v}^{ij:k}\\}\quad\forall S^{i,j:k}\in
M,H^{ij:k}\neq\emptyset\\\ &V_{v}^{unseen}=\\{v_{v}^{ij:k}\\}\quad\forall
S^{i,j:k}\in M,H^{ij:k}=\emptyset.\end{split}$

We also discuss two types of factors: First is the estimation of each landmark
triplet qualitative state by its own measurement history
$\mathbb{P}({S^{ij:k}|H^{ij:k}_{n}})$ as calculated in section 4.1. This
factor is unary (connected only to one variable node) and exists for any
landmark triplet $V_{v}^{seen}\in V^{seen}$ with observations. The group of
all unary factors is denoted as $V_{fu}$. The second factor is a _”composition
factor”_. It is a trinary factor that involves landmark triplets with common
landmarks connected by composition, as discussed in Section 5.1. The group of
all composition factors is denoted as $V_{fc}$. Therefore we get the
following:

$\begin{split}&V_{f}=V_{fu}\cup V_{fc}\\\
&V_{uf}=\\{v_{fu}^{ij:k}\\}\quad\forall\;v_{v}^{ij:k}\in V_{v}^{seen}\\\
&V_{cf}=\\{v_{fc}^{i,j,k,s}\\}\quad\forall\;i,\,j,\,k,\,s;\;S^{i,j:k},\,S^{j,k:s},\,S^{i,j:s}\in
M.\end{split}$

Factor graph edges correspondingly are of two types. _Unary edges_ exist
between every unary factor and its corresponding triplet qualitative state,
and _composition edges_ exist between any composition factor to the three
common landmark triplets it is associated with:

$\begin{split}&E=E_{u}\cup E_{c}\\\
&E_{u}=\\{(v_{v}^{ij:k},v_{uf}^{ij:k})\\}\quad\forall v_{v}^{ij:k}\in
V_{v}^{seen}\\\
&\\!\begin{multlined}E_{c}=\\{(v_{fc}^{i,j,k,s},v_{v}^{ij:k}),(v_{fc}^{i,j,k,s},v_{v}^{jk:s}),(v_{fc}^{i,j,k,s},v_{v}^{ij:s})\\}\forall
v_{fc}^{i,j,k,s}\in
V_{cf}.\end{multlined}E_{c}=\\{(v_{fc}^{i,j,k,s},v_{v}^{ij:k}),(v_{fc}^{i,j,k,s},v_{v}^{jk:s}),(v_{fc}^{i,j,k,s},v_{v}^{ij:s})\\}\forall
v_{fc}^{i,j,k,s}\in V_{cf}.\end{split}$

It is important to note that our factor graph represents a joint distribution
over _discrete_ random variables. As mentioned in section 5.1, variable nodes
and unary factor nodes are $m\times 1$ vectors, whereas composition nodes are
$m\times m\times m$ tensors. Fig. 9 is a visual example of this qualitative
factor graph.

Figure 9: Qualitative map as a factor graph: Variable nodes are landmark
triplet qualitative states. Prior factor nodes - estimating landmark triplet
qualitative state based on its measurements. Composition factor nodes -
connecting between three triplets with common landmarks by composition.

###  5.3 A Study Of Factor Graph Propagation By Composition

Having defined our qualitative SLAM framework and how to represent it as a
qualitative factor graph, we now study how data propagation by composition
behaves in the factor graph. The main goal is to see how much information can
composition propagate through the graph in relation to prior knowledge and
graph topology. This study is also essential for addressing active qualitative
planning (specifically the ability to estimate unseen triplets). We only
perform a basic study and do not suggest a generally optimal solution for
qualitative factor graph data propagation (this can be thoroughly done in
future work).

This study directly relates to topological signatures in traditional metric
SLAM factor graphs with Gaussian distributions. The works [42, 43, 44, 45]
show that topological graph signatures are highly correlated to differential
entropy (under certain assumptions). We provide the first empirical indication
that a similar notion could potentially apply also to factor graphs with
discrete variables.

The simplest way to measure data propagation in the factor graph is by looking
at unseen landmark triplets. Since unseen triplets have no stand-alone
estimation, we can propagate data from seen triplets with unary factors
through the graph and examine how much information was propagated to the
unseen triplets. Intuitively we expect the amount of propagated information to
be dependent on both the initial amount of information in the graph and the
topology of the unseen triplets relating to all of the seen triplets with
unary factors.

The way we address this study is as follows:

  * •

We suggest a simple _information decay_ model for describing composition-based
propagation. This model was empirically derived also using a basic
understanding of the process (described in section 5.1).

  * •

We suggest a simple algorithm for propagating data from seen triplets to
unseen triplets by composition factors. Then we measure the amount of
information in each variable node given its qualitative state probability
using a unique _information score_ metric.

  * •

We use the information decay model to predict the information propagated to
each unseen node. We call this prediction _”topology Score”_. We show a
reasonable correlation to the composition-based information score, which means
we have a simple model for understanding how composition-based propagation
depends on graph topology.

This study uses a simulation that generates random realizations of factor
graphs that contain seen and unseen triplets with various levels of
information and topologies as detailed in sections 6.1 and 6.4.2. This section
specifies our information decay model, graph propagation algorithm,
information score, and topology score.

We use a few additional notations to describe our information decay model and
propagation algorithm. Our algorithm needs to keep track of which triplet
estimations have been updated in different stages of its operation. Therefore
we divide the variable nodes $V_{v}$ into two subgroups: the set of triplets
marked as updated $V_{v}^{u}$, and the set of triplets marked as not yet
updated $V_{v}^{\neg u}$. The algorithm updates these groups during its
operation.

$V_{v}=V_{v}^{u}\cup V_{v}^{\neg u}.$

For the reader’s convenience, a recap of all factor graph-related notations
can be found in Table 1.

qualitative factor graph notations
---
notation |  description
G |  qualitative factor graph
V |  graph nodes
E |  graph edges
$V_{v}$ |  variable graph nodes (each corresponds to a triplet qualitative state $S^{i,j:k}$)
$V_{f}$ |  factor graph nodes
$V_{v}^{seen}$ |  variable graph nodes that where observed
$V_{v}^{unseen}$ |  variable graph nodes that where never observed together
$V_{v}^{u}$ |  variable graph nodes that have already been updated during some stage of data propagation through the graph
$V_{v}^{\neg u}$ |  variable graph nodes that have not yet been updated during some stage of data propagation through the graph
Table 1: Qualitative factor graph notations used in 5.3.

####  5.3.1 Information Score

Our graph propagation algorithm and topology score refer to the information in
each variable node. To measure the information of a triplet with a specific
qualitative state probability $\mathbb{P}({S^{AB:C}})$ (ground truth,
estimated or prior), we introduce a new _”information Score”_ metric denoted
as $ISC^{AB:C}$. To make this metric intuitive, we want it to maintain $0\leq
ISC^{AB:C}\leq 1$ where 0 means no information (uniform probability) and 1
means full information (qualitative state is perfectly known).

Respectively we define _information Score_ of an arbitrary landmark triplet
AB:C as the normalized entropy distance of its qualitative state probability
from uniform probability:

$ISC^{AB:C}\triangleq\frac{\mathcal{H}_{max}-\mathcal{H}^{AB:C}}{\mathcal{H}_{max}-\mathcal{H}_{min}},$
(19)

where $\mathcal{H}^{AB:C}$ is the entropy of the given qualitative state
probability, $\mathcal{H}_{max}$ is maximal entropy (no information), and
$\mathcal{H}_{min}$ is minimum entropy (full information). Remembering the
dimension of the discrete qualitative state probability vector is $d$
(dependent on the specific qualitative space partition we use), we get:

$\begin{split}&\\!\begin{multlined}\mathcal{H}^{AB:C}=-\sum_{i=1}^{d}{\mathbb{P}({S^{AB:C}=i|H^{AB:C}_{n}})}\\\
\log{\mathbb{P}({S^{AB:C}=i|H^{AB:C}_{n}})}\end{multlined}\mathcal{H}^{AB:C}=-\sum_{i=1}^{d}{\mathbb{P}({S^{AB:C}=i|H^{AB:C}_{n}})}\\\
\log{\mathbb{P}({S^{AB:C}=i|H^{AB:C}_{n}})}\\\
&\mathcal{H}_{max}=\log{(\frac{1}{d})}\\\ &\mathcal{H}_{min}=0.\end{split}$

####  5.3.2 Information Decay Model

A significant part of this study is finding a simplified model for the
composition behavior in factor graph data propagation. Using a basic
understanding of composition and empiric trial and error, we achieved two
important insights: First, the qualitative composition cannot fully preserve
the information and makes it decay (partially explained in section 5.1).
Second, the composition is a product of two problems (partially described in
section 5.1). Remembering that composition factors connected three triplet
nodes (e.g. AB:C, BC:D, AB:D), the decay effect for the case where one triplet
has information and the other two do not is roughly quadratic in reference to
the case where two triplets have information and one does not.

Therefore, we divide the model into two cases (see Figure 10). In case of two
non-updated nodes and one updated node (e.g., $v_{v}^{BC:D},v_{v}^{AB:D}\in
V_{v}^{\neg u};v_{v}^{AB:C}\in V_{v}^{u}$) we model the information score
decays by a factor of $(1-\alpha)$ where $0<\alpha<1$:

$ISC^{BC:D}=ISC^{AB:D}=(1-\alpha)ISC^{AB:C},$ (20)

and in case one node is not updated, and two nodes are (e.g.,
$v_{v}^{AB:C},v_{v}^{BC:D}\in V_{v}^{u};v_{v}^{AB:D}\in V_{v}^{\neg u}$) we
model a smaller decay factor of $(1-\alpha^{2})$:

$ISC^{AB:D}=(1-\alpha^{2})\frac{ISC^{AB:C}+ISC^{BC:D}}{2}.$ (21)

The scalar $\alpha$ is the decay factor and is empirically set to
$\alpha=0.5$. Section 6.4.2 presents the results of this study and the
correlation between ”topological score” to actual ISC.

(a)

(b)

Figure 10:  Each composition factor is connected to three variable nodes. Data
is propagated through these factors during map update. Our information decay
model for a single composition factor is dividend into two cases: (a) In case
two variable nodes are non updated, and one is updated, decay model is
$(1-\alpha)$ (b) I case one variable node is not updated and two nodes are,
decay model is $(1-\alpha^{2})$. (see section 5.3.2). note that $0<\alpha<1$,
so less information decay in the second case.

####  5.3.3 Graph Propagation Algorithm

In the scope of this study, we consider factor graphs that contain seen and
unseen triplets. We want to propagate the initial data from the seen triplets
to the unseen ones using composition factors to test propagation by the
composition process.

The accurate way of doing this is by variable elimination (as discussed in
[46]). Unfortunately, this approach is impractical in our case. Variable
elimination replaces a group of connected variables and factor nodes with a
single factor that connects all involved variables (see figure 11). Generally,
a discrete factor is a $d$ dimensional tensor with $d^{\\#variables}$ entries
(In our case, $d$ is the number of sections in the qualitative space
partition). The factors generated in elimination usually involve multiple
variables, are too big, and are practically infeasible in our case.
Practically even very efficient implementations, such as in GTSAM [47]
discrete factor graph, do not scale well.

(a)

(b)

Figure 11:  Discrete factor graph variable elimination: (a) a discrete
qualitative factor graph contains only trinary (and unary) factors. If each
discrete variable dimension id $d$, each trinary factor is a $d\times d\times
d$ tensor. (b) eliminating variables
$V_{v}^{AB:C},V_{v}^{AB:D},V_{v}^{AB:E},V_{v}^{BD:E}$, we get a new factor
that is a function of four qualitative states, and therefore is a $d\times
d\times d\times d$ tensor. Similarly, multiple variables elimination may lead
to factors that are a function of multiple variables and, therefore, are high
dimensional tensors that are infeasible to compute.

Another well-known approximated solution is belief propagation, also known as
sum-product message passing (see, e.g., [48], [49]). This solution mainly
holds for non-loopy graphs (which is not the case for us). Loopy belief
propagation algorithms support general factor graph topology but are not
proven to converge [50].

For our study, we chose a simpler algorithm that is very stable and good
enough to investigate the behavior of qualitative composition concerning graph
topology. Finding a generally good approximated algorithm for this problem is
still to be studied, and we leave this for future work. The algorithm we
suggest inspired by label correcting algorithms is detailed in Alg. 2.

To better describe our algorithm, we add a few notations. During its progress,
our algorithm needs to determine which composition factors are available for
data propagation in each step. Accordingly, we divide the group of all
composition factors $V_{fc}$ into two groups. The first group comprises
composition factors connected to at least one updated triplet and one not
updated triplet. These factors are considered available for propagating new
information. We call these _open_ composition factors and denote this subgroup
as $V_{fc}^{open}$. The second group includes composition factors that are
connected only to updated or only to not updated triplets. These factors are
considered unavailable for new information propagation and are called _closed_
composition factors. We denote this subgroup as $V_{fc}^{closed}$. The
algorithm initializes both subgroups when it starts and updates them during
its progress.

$\begin{split}&V_{fc}=V_{fc}^{open}\cup V_{fc}^{closed}\\\
&V_{fc}^{open}=\\{v_{fc}^{ijks}\\};\exists e(v_{fc}^{ijks},v\in
V_{v}^{u})\land\exists e(v_{fc}^{ijks},v\in V_{v}^{\neg u})\\\
&V_{fc}^{closed}=\\{v_{fc}^{ijks}\\};\nexists e(v_{fc}^{ijks},v\in
V_{v}^{u})\lor\nexists e(v_{fc}^{ijks},v\in V_{v}^{\neg u}).\end{split}$

Algorithm 2 qualitative factor graph data propagation

1: // init all seen triplets as updated

2:$V_{v}^{u}=V_{v}^{seen}$

3:$V_{v}^{nu}=V_{v}\setminus V_{v}^{seen}$

4:init $V_{fc}^{open},\,V_{fc}^{closed}$ by $V_{v}^{u},\,V_{v}^{\neg u}$

5:init $ISC^{i,j:k}\forall\;v_{v}^{i,j:k}\in V_{v}^{seen}$

6:while $V_{v}^{\neg u}$ not empty and $V_{fc}^{open}$ not empty do

7: // propagate all open composition factors

8: for all $v_{fc}^{i}\in V_{fc}^{open}$ do

9: calculate qualitative state probability of non updated triplets -eq. (18)

10: calculate ISC of non updated triplets

11: end for

12: // keep only best ISC triplet

13: Find best new ICS triplet $v_{v}^{best}\in V_{v}^{\neg u}$

14: mark best triplet as updated: $v_{v}^{best}\to V_{v}^{u}$

15: keep $v_{v}^{best}$ estimation

16: mark relevant composition factor as closed $V_{fc}^{i}\to V_{fc}^{closed}$

17:end while

18: // handle unreachable triplets

19:if $V_{v}^{\neg u}$ not empty then

20: $\forall\;v_{v}^{ij:k}\in V_{v}^{\neg u}$ set uniform state probability

21:end if

In Alg. 2 all triplets with unary factors $V_{v}^{seen}$ are ”source” nodes.
They are initialized as $V_{v}^{u}$ and are not updated again (lines 2-5).
Every open composition factor connects updated and non-updated variable nodes.
On every step of the algorithm, all open composition factors $V_{fc}^{open}$
propagate new data to their _non-updated_ triplets (lines 8-11). However, only
the update of the factor that achieves the best information score for its _not
updated_ nodes is saved. The selected triplet nodes are marked as _updated_
and are not updated again (lines 12-16). This way, every unseen node is
updated only once. Finally, the algorithm halts when there are no open factors
left. Figure 12 shows an example of the graph propagation algorithm.

(a)

(b)

(c)

(d)

Figure 12:  Example for the factor graph propagation algorithm (described in
2). In this example, we show a specific qualitative factor graph. Yellow
triangles are composition factors. Small blue circles are non-updated variable
nodes, while big green circles are updated nodes. Each variable node has a
notation of Id(ISC level). In each step of the algorithm, several non-updated
candidate nodes are considered for propagation (marked in red circles), but
only the one with the best ISC is updated. (a) Initial state. Nodes 1,2,12 are
source nodes and are marked as updated. (b) Nodes 3,4,6,11 are candidates to
be updated. Node 6 gets the best ISC and is updated. (c) Nodes 3,4,5,7,11 are
candidates to be updated. Node 5 gets the best ISC and is updated. (d) Nodes
3,4,7,8,11 are candidates to be updated. Node 3 gets the best ISC and is
updated. (Remark: This example does not reach the end of the propagation).

We chose this algorithm for three reasons: First, it is fast - a single update
for each triplet. Speedy runtime lets us go over many scenarios quickly and
conduct a comprehensive study. Second, it is very stable and not affected by
graph loops. Finally, it proved helpful for our purpose (as shown in section
6.4.2).

####  5.3.4 Topology Score

The last part of this study shows how our information decay model explains the
behavior of composition-based propagation in qualitative factor graphs. Our
information decay model uses only the initial information of seen triplets
(not the entire probability distribution) and graph topology to predict how
much information will propagate to each variable node in the graph. We call
this prediction _”_ topology score” (TSC) and denote it as
$TSC(v_{v}^{AB:C})=TSC^{AB:C}$. In section 6.4.2, we show that this prediction
has a reasonable correlation to the ISC of unseen nodes from the composition-
based propagation. Therefore we conclude it is a valid model.

The decay model prediction might be used when addressing a globally optimized
solution for qualitative factor graph propagation or active qualitative
planning.

The process of generating a ”topology score” is as follows: we start by
setting the topology score for all seen triplets $V_{v}^{seen}$ to be their
initially estimated ISC:

$TSC^{AB:C}=ISC^{AB:C}\quad\forall v_{v}^{AB:C}\in V_{v}^{seen}.$

Then we use the same graph propagation algorithm described in Alg. 2, but
instead of using actual composition factors for propagating data, we use our
”information decay” model. The ”topology score” variant of the algorithm is
described in Alg. 3. Note that while we use Alg. 2 to propagate actual data in
the graph, our decay-based ”topology score” can be applied to any graph
propagation algorithm.

Algorithm 3 topology score propagation

1: $\triangleright$ differences from in Alg. 2 in red

2: // init all seen triplets as updated

3:$V_{v}^{u}=V_{v}^{seen}$

4:$V_{v}^{nu}=V_{v}\setminus V_{v}^{seen}$

5:init $V_{fc}^{open},\,V_{fc}^{closed}$ by $V_{v}^{u},\,V_{v}^{\neg u}$

6: init $TSC^{i,j:k}=ISC^{i,j:k}\quad\forall\,v_{v}^{i,j:k}\in V_{v}^{seen}$

7:while $V_{v}^{\neg u}$ not empty and $V_{fc}^{open}$ not empty do

8: // propagate all open composition factors

9: for all $v_{fc}^{i}\in V_{fc}^{open}$ do

10: calculate TSC of non updated triplets - eqs. (20) and (21)

11: end for

12: // keep only best  TCS triplet

13: Find best new  TCS triplet $v_{v}^{best}\in V_{v}^{\neg u}$

14: mark best triplet as updated: $v_{v}^{best}\to V_{v}^{u}$

15: keep $v_{v}^{best}$ estimation

16: mark relevant composition factor as closed $v_{fc}^{i}\to V_{fc}^{closed}$

17:end while

18: // handle unreachable triplets

19:if $V_{v}^{\neg u}$ not empty then

20: $\forall\;v_{v}^{ij:k}\in V_{v}^{\neg u}$ set uniform state probability

21:end if

Section 6.4.2 presents the results of this study and the correlation between
”topological score” to actual ISC.

##  6 Results

We thoroughly evaluate our approach to test its performance and effectiveness.
We discuss our test and analysis methodology in sections 6.1 and 6.2. The
first part of our approach concerns single landmark triplet estimation. In
section 6.3, we use simulation to compare our approach to the state of the art
[23] and specifically show the effect of in-cooperating a motion model on
speed and performance. We also compare our full, accurate algorithm to our
approximated one (As presented in sections 4.4, 4.5), further improving the
speed with a slight performance cost. In section 6.3.3, we use the MCRLAM
dataset [51] for testing in a more realistic scenario.

The second part of our work addresses data propagation in the qualitative map.
This part is innovative and without existing work as a reference. In section
6.4.1, we evaluate the basic performance of qualitative composition, and in
section 6.4.2, we show the results of our study of composition behavior in a
qualitative factor graph as discussed in section 5.3.

###  6.1 Simulation

The main tool we use to evaluate our approach is a MATLAB-based simulation. We
consider a 2D scenario with point landmarks and a mobile camera. Specifically
we uniformly choose positions for $m$ landmarks $L^{i}=(x_{i},y_{i}),i\in
1,...,m$. The set of landmarks is our metric ground truth map
$M_{metric}=\\{L^{i}\\}_{i=1...m}$. Then we uniformly choose camera trajectory
$Trajectory=\\{X_{j}\\}_{j=1...n}$ (where $X_{j}$ is camera pose at time
instant $j$). We randomly select which landmarks are observed from each pose
and randomize measurements and actions $H_{n}$ using Gaussian measurement and
motion models. Camera to landmark measurements are noisy azimuth measurements,
and actions are heading commands as described in Section 2. To evaluate the
different parts of our work, we randomize numerous scenarios with various
noise levels. Figure 13 illustrates an example scenario.

To keep the analysis general, we consider locations and poses as uniform as
possible: Landmark locations and camera trajectory are sampled uniformly; each
pose observes only three landmarks. This constraint prevents biasing results
with specific assumptions on camera trajectory or landmark visibility. Note
that our method is more comprehensive than observing only three landmarks at a
specific time. If a specific view observes more than three landmarks, all
landmark triplets involved will generate measurements.

An important parameter of the simulation is its geometric dynamic range
(_GDR_), i.e. the ratio between the minimal distance between any two objects
and the maximal distance between any two objects (cameras or landmarks). All
2D locations are randomised within $x\in[-3,3]$ and $y\in[-3,4]$, and we
enforce a minimum distance of $0.01$ between any two objects. This means
$GDR\leq 800$. It is also a valid assumption for real systems (e.g., a robot
moving in a $60$m $\times 70$m area with landmarks and cameras not closer than
$10$ cm from each other). Limiting the GDR enables a numerically stable
solution.

We also note that landmark recognition is ideal with no false landmark
identification. Our work focuses on basic qualitative geometry. Identification
errors can be modeled in our approach (an important direction for future
work).

Figure 13:  Simplified illustration of a simulation scenario. The camera
trajectory is presented in red. The camera gets measurements of two landmark
triplets: 1,2:4 and 3,5:6. Each is observed from 3 different viewpoints. In
the actual simulation, the order of observations is random.

###  6.2 Performance Metrics

We use various metrics to analyze our inference performance. Using various
metrics is an important addition compared to the analysis made in state-of-
the-art approaches [22] and [23]. It provides us with a deeper understanding
of qualitative inference. We use two main types of metrics:

  1. 1.

Ground truth (GT) related metrics:

     * •

_Probability DMSE:_ The mean square error for the difference between the
estimated qualitative state probability vector to the GT state vector
$DMSE=\sqrt{\sum_{i=1}^{m}{(s_{i}^{AB:C}-s_{GT}^{AB:C})^{2}}}$. It tests how
accurate the estimation is.

     * •

_Ground truth rating:_ The rank of the GT qualitative state when ordering
qualitative states by their estimation likelihood (1 is most likely).

     * •

_Geometric distance:_ The mean distance from qualitative state centroids to GT
state centroids weighted by the state estimated probability.
$gmd=\sum_{i=1}^{m}{||c_{i}^{AB:C}-c_{GT}^{AB:C}||_{2}}$ This metric tells us
how close are the estimated states to GT (1 is the distance between A and B).

     * •

_Ground truth likelihood:_ estimated posterior probability of GT qualitative
state $\mathbb{P}({s_{GT}^{AB:C}})$. This metric measures the accuracy of
estimation but ignores false qualitative states.

     * •

_Ground truth likelihood ratio:_ The ratio between the estimated probability
of GT qualitative state to the estimated probability of the most likely state
$\frac{\mathbb{P}({s_{GT}})}{max(S^{AB:C})}$. This metric measures how close
the GT state is to being most likely.

  2. 2.

Information (or entropy) related metrics:

     * •

_Entropy:_ estimation probability entropy
$E=-\sum_{i=1}^{m}{\mathbb{P}({s_{i}^{AB:C}\log{s_{i}^{AB:C}}})}$ measures how
distributed is the qualitative state probability (or how much information is
in the distribution).

     * •

_Likelihood ratio:_ the ratio between the second most likely qualitative state
to the most likely qualitative state. This measures how ”decisive” the
estimation result is. Very important to maximal likelihood approaches.

Using all these metrics in different stages of result analysis teaches us much
about the quality of our algorithms.

###  6.3 Single Triplet

To evaluate the effect of our motion-model-incorporated inference, we compare
three different algorithms: (a) State-of-the-art baseline [23] (b) Our full
multi-view inference as described in Section 4.1. (c) Our fast approximated
multi-view inference (section 4.1).

The baseline for evaluating our performance is the previous work of [23]
(mapping only). To compare running time and complexity, we do not directly
implement [23], but an equivalent algorithm. We use our formulation from
Section 4.1 that leads to equations 7 and 10. Since [23] does not consider a
motion model, views are independent. Under these assumptions, we now get:

$\\!\begin{multlined}\mathbb{P}({s_{i}^{C}|H_{n}})=\iint\displaylimits_{L^{C}\in
s_{i}^{C},X_{1:n}}{\mathbb{P}({X_{1:n},L^{C}|H_{n}})dL^{C}dX_{1:n}},\end{multlined}\mathbb{P}({s_{i}^{C}|H_{n}})=\iint\displaylimits_{L^{C}\in
s_{i}^{C},X_{1:n}}{\mathbb{P}({X_{1:n},L^{C}|H_{n}})dL^{C}dX_{1:n}},$ (22)

where

$\\!\begin{multlined}\mathbb{P}({X_{1:n},L_{C}|H_{n}})=\frac{\mathbb{P}({X_{1},L_{C}})}{\mathbb{P}({Z_{1}})}\prod_{i=1}^{n}{\frac{1}{\zeta_{i}}\mathbb{P}({Z_{i}|X_{i},L_{C}})},\end{multlined}\mathbb{P}({X_{1:n},L_{C}|H_{n}})=\frac{\mathbb{P}({X_{1},L_{C}})}{\mathbb{P}({Z_{1}})}\prod_{i=1}^{n}{\frac{1}{\zeta_{i}}\mathbb{P}({Z_{i}|X_{i},L_{C}})},$
(23)

and $\zeta_{i}\doteq\mathbb{P}({Z_{i}|a_{1:i-1},Z_{1:i-1}})$. We then
correspondingly implement a reduced version of our sample-based algorithm 1 in
section 4.4.

To evaluate the single triplet inference, we randomize 300 scenarios with 36
different combinations of measurement and motion model noise levels for each
scenario (10800 scenarios in total). Measurement and motion model has Gaussian
noise as described in section 2, where measurement noise is
$\sigma_{v}\in[0^{\circ},10^{\circ}]$ and motion model noise is
$\sigma_{w}\in[0^{\circ},20^{\circ}]$. We also use landmark triplets with
views from 3-time steps, as shorter trajectories will not show the effect of
using a motion model, and longer trajectories typically do not improve results
significantly.

####  6.3.1 General performance

Figure 14 shows performance results and compares our motion-model-incorporated
inference to the state-of-the-art. Performance is represented by _DMSE_ and
_Geometric distance_ as specified in section 6.2. In this table, for the ease
of visualization, results are a function of azimuth measurement noise, while
motion model heading noise is $\times 2$ bigger for each run correspondingly.

EDC estimation with motion model
---
Metric |  baseline |  ours |  ours-fast
DMSE |  0.39, 0.63, 0.71 |  0, 0.16, 0.63 |  0, 0.21, 0.62
geometric distance |  0.28, 1.10, 2.30 |  0, 0.25, 1.15 |  0, 0.27, 1.16
Entropy |  0.28, 0.66, 0.87 |  0, 5e-3, 0.58 |  0, 0.07, 0.64
time[sec] |  26 |  18 |  0.05

Table 2: EDC state estimation for single triplet. measurement noise:
$\sigma_{v}=2^{\circ}$, motion model noise: $\sigma_{w}=5^{\circ}$

.

(a)

(b)

Figure 14:  EDC single triplet estimation. Our motion model in-cooperated
algorithm is compared to the state-of-the-art baseline [23]. The plot shows
median and percentiles 25 and 75 for each algorithm. (a) DMSE Vs. measurement
noise (motion model noise is $\times 2$). (b) mean geometric distance vs.
measurement noise (motion model noise is $\times 2$).

We observe that using a motion model dramatically improves performance. Up to
measurement noise of $2^{\circ}$ (which is reasonable even for low-end camera-
based platforms), results are much better than state-of-the-art and almost
perfect. Up to $7^{\circ}$ (which is a significant error for camera-based
systems), our estimated probability _DMSE_ is still better than state-of-the-
art. We can also see that for $7^{\circ}$ $\emph{gmd}<1$, all of the
qualitative states that are wrongly estimated are likely to be close to the
ground truth state.

Another significant result concerns our fast-approximated algorithm. Table 2
shows various performance metrics, including run time for each algorithm. We
show 25 percentile, median, and 75 percentile. Our fast approximated algorithm
achieves performance very close to the full one. While our full algorithm is
about $\times 2$ faster than the baseline, the approximation is about $\times
100$ faster than both. These results show a successful usage of the ability of
the coarse qualitative estimation to absorb errors and enable fast
approximations. While we use a simple MATLAB implementation, absolute run
times are irrelevant but can be used for comparing the algorithms.

Figure 15 includes additional performance metrics. Unlike _DMSE_ and
_Geometric distance_ , which measure the correctness of the estimation,
_Entropy_ measures only the steepness of it. Entropy performance shows similar
behavior. On the other hand, looking at _ground truth rating_ , we note that
for high noise levels, our estimation is sometimes lesser than the baseline
method. Remembering that even in these conditions, the correctness of our
probability distribution is better (as seen in _DMSE_) we conclude that this
is caused by the fact that scenarios with higher noise may lead to multiple
(mostly two and up to 4) likely qualitative states. In these cases, minor
estimation errors may translate to a change in likelihood rating between these
close states. We also note that these high noise levels are unlikely for real
camera-based applications.

(a)

(b)

Figure 15:  EDC single triplet estimation. Our motion model in-cooperated
algorithm is compared to the state-of-the-art baseline [23]. The plot shows
median and percentiles 25 and 75 for each algorithm. (a) Entropy Vs.
measurement noise (motion model noise is $\times 2$). (b) Ground truth rating
vs. measurement noise (motion model noise is $\times 2$).

####  6.3.2 Fast Variant Noise Sensitivity

Observing that the fast approximation algorithm presents results close to the
full algorithm, we analyzed its performance more extensively. Since our fast
variant is noise ignorant (as detailed in section 4.5), we want to
specifically test the different effects of motion model noise and measurement
noise separately. We use the same simulation and scenario set but look at
different cross-sections of noise levels.

Figure 16 shows the effect of motion model noise on the performance. We look
at scenarios with 0 measurement noise and various motion model noise levels.
Motion model noise has a small effect on ground truth likelihood and geometric
distance. Even with a very high noise of $20^{\circ}$, the estimated
probabilities are relatively unaffected. However, it has more effect on the
ground truth rating (but only on the 75 percentile), although GT is typically
in the top 3 most likely states.

We conclude that this is caused by scenarios with more challenging geometry
(landmarks close to qualitative state edges or ill-conditioned camera poses)
that may lead to two or three likely qualitative states. In these cases, the
minor errors may translate to a change in likelihood rating between these
close states.

(a)

(b)

(c)

Figure 16:  Sensitivity to motion model error. The plot shows median and
percentiles 25 and 75. (a) mean geometric distance Vs. Motion model noise. (b)
ground truth rating Vs. Motion model noise (c) ground truth likelihood Vs.
Motion model noise.

Figure 17 shows the effect of measurement noise on the performance. We look at
scenarios with 0 motion mode noise and various azimuth measurement noise
levels. The effect of measurement noise is more substantial. Both geometric
distance and ground truth likelihood respond as noise increases above
$2^{\circ}$, but still, medians are not very strongly affected. Again GT
rating responds harder on the 75 percentile but is stable on the median,
leading us to the same conclusions.

(a)

(b)

(c)

Figure 17:  Sensitivity to measurement model error. The plot shows median and
percentiles 25 and 75. (a) mean geometric distance Vs. Measurement model
noise. (b) ground truth rating Vs. Measurement model noise (c) ground truth
likelihood Vs. Measurement model noise.

####  6.3.3 MRCLAM Dataset Results

To evaluate our method in a realistic scenario, we use the _MRCLAM_ dataset
[51], which comprises several scenarios of multiple robots moving around pre-
set landmarks with unique markers. We choose this dataset since it has robot
pose GT and landmark identification and location GT. In addition, landmarks
are round and, therefore, act as point landmarks. This real-world dataset
allows us to evaluate how informative our qualitative estimation framework is
(e.g. in terms of entropy) and quantify performance concerning GT, which has
yet to be tackled in previous work [23].

We use five scenarios, each with five robots and 15 landmarks. We get 16-230
landmark triplets observed three times or more in each scenario. Our
qualitative approach thus generates rich mapping for these scenarios.

A typical estimation result for a single landmark triplet observed from three
camera views is in Figure 18. We can see that the usage of the motion model
reduces the trajectory sampling hypotheses $TH$ (blue color), which is further
refined by the landmark $C$ triangulation step to only a small subset (green
color). The resulting landmark $C$ location hypotheses are in red. One can
observe that the camera and landmark hypotheses are very close to GT. As a
result, the likelihood of the GT qualitative state in this example run is 1.

Table 3 summarizes the results for this dataset. We compare our fast algorithm
variant to an uninformative uniform distribution. Looking at _Entropy_ and
_DMSE_ , we conclude that estimation is informative and is close to the truth.
_GT rating_ shows that most of the time, the GT state is the most likely, and
always within the top 2. Also, looking at _gmd_ , we can see that all likely
qualitative states are close to GT. In conclusion, we note that results show
meaningful and informative estimation, which concurs with our stimulative
results considering the reasonable noise levels in the dataset.

Figure 18:  MRCLAM dataset: EDC example scenario result. Ground truth camera
poses are red triangles, and ground truth landmark locations are blue circles.
Thin green lines are camera locus circles, while thick blue and green are
valid hypotheses after two and three view constraints. Redline is the landmark
C location hypothesis after three-view constraints. All estimations are close
to GT, and the error is still inside GT’s qualitative state (light green
area). MRCLAM dataset EDC estimation
---
|  ours-fast |  uniform
DMSE |  0.03, 0.45, 0.69 |  0.97
gmd |  5e-3, 0.27, 0.71 |  2.2
Entropy |  4e-3, 0.38, 0.69 |  3
GT rating |  1, 1, 2 |  -
Table 3: MRCLAM dataset results summary: We show 25 percentile, median, and 75
percentile for each metric. Our approximated fast algorithm is compared to an
uninformative uniform estimation.

####  6.3.4 Discussion

Generally, our full algorithm and fast approximation have several good
properties. First, they perform very well for azimuth measurement noise of up
to $2^{\circ}$ and motion model heading noise of up to $10^{\circ}$, which is
a reasonable range even for low-quality camera-based platforms. Secondly, they
are more sensitive to measurement model noise than motion model noise, which
is also practical since measurement noise is usually smaller in real-world
scenarios. Furthermore, performance degradation in high noise levels is
gradual and generally stable.

We also successfully demonstrate the unique attributes of qualitative
inference to hold approximations well and achieve a very low compute global
nonlinear algorithm. We believe there is a good potential for this approach to
be practical for real-world qualitative autonomous navigation and mapping. It
may also even be more meaningful when addressing online qualitative active
planning.

###  6.4 Qualitative Map Data Propagation Results

We now analyse data propagation in the qualitative map as described in section
5. First, in section 6.4.1 we show the results of a basic single triplet
simulation that measures how much information is propagated through a single
trinary composition factor (as described in section 5.1). Then, in section
6.4.2 we present the results of our qualitative graph data propagation study
(as described in section 5.3). We discuss the specifics of how we simulate the
problem, and show the correlation between our topological information decay
model to actual composition composition based propagation.

####  6.4.1 Basic Composition Factor Analysis

In this section, we evaluate the basic composition operator for propagating
data between three common landmark triplets as detailed in section 5.1. While
composition can improve existing estimation, we only test its performance in
propagating data to un-estimated triplets, enabling us to understand its
operation clearly.

Specifically, we simulate 1000 scenarios, each comprising three inter-
connected landmark triplets $(\text{AB:C, BC:D, AB:D})$ and camera
trajectories with different noise levels. Histories, in terms of measurements
and controls $H^{AB:C}$ and $H^{BC:D}$ are available, but
$H^{AB:D}=\emptyset$. We estimate $\mathbb{P}({S^{AB:C}|H^{AB:C}})$ and
$\mathbb{P}({S^{BC:D}|H^{BC:D}})$ using measurement history and our fast
approximated algorithm. Then, $\mathbb{P}({S^{AB:D}|H^{AB:C},H^{BC:D}})$ is
inferred only through composition.

(a)

(b)

(c)

Figure 19:  EDC probabilistic composition. The plot shows median and
percentiles 25 and 75. (a) DMSE vs. measurement noise. Composition results Vs.
uniform probability (qualitative state unknown) (b) mean geometric distance
vs. measurement noise. In addition to our composition, we also show results
for uninformative estimation.

Figure 19 summarizes the performance evaluation. The propagated data is
compared to the initial state of uniform estimation for the unseen triplets.
Results are not perfect, but considering this triplet has no direct
measurements, we see that composition still propagates significant
information. The GT rating tells us that for reasonable measurement noise of
up to $2^{\circ}$, the GT state is mostly among the four most probable EDC
states (and half the time among the two leaders). The $gmd$ plot shows that
the False EDC states are mainly close to the GT state. Considering that these
are triplets that were never measured together and could not be estimated
otherwise, these are significant results that might enable the use of unseen
triplets in active qualitative planning.

####  6.4.2 Factor Graph Propagation Analysis

The last part of our work studies how composition factors propagate data
through the qualitative factor graph. As explained in section 5.3 this is done
in 4 steps:

  1. 1.

We generate a factor graph with seen triplets that has an initial independent
estimation and unseen triplets with no independent estimation.

  2. 2.

We use the composition-based Alg. 2 to propagate data from seen triplets to
all unseen triplets and calculate the ”information score” for each one
(section 5.3.3).

  3. 3.

We use our information decay model and Alg. 3 to generate a ”topology score”
for all triplets (sections 5.3.2, 5.3.4). This score is a prediction of the
amount of information that will get to each triplet based on our decay model
and graph topology.

  4. 4.

Finally, we compare the ”topology score” to the actual ”information score” for
each triplet and see if the information decay model correlates to the actual
composition-based propagation.

Next, we specify this study in more detail.

####  6.4.3 Factor Graph Simulation

First, we must simulate scenarios with complex factor graphs containing seen
and unseen triplets in various topologies. To do so, we use the same
simulation described in section 6.1 to organize landmarks and camera poses
uniformly and randomly. Then we consider all possible landmark triplets
$V_{v}^{full}$, and all possible composition factors $V_{fc}^{full}$. We
choose a random subgroup of $n_{fc}$ composition factors $V_{fc}\subseteq
V_{fc}^{full}$, and all related triplets $V_{v}\subseteq V_{v}^{full}$ to make
the factor graph. Then out of all the $n_{v}=\left|V_{v}\right|$ triplets we
randomly choose $n_{seen}=\frac{n_{v}}{\text{coverage rate}}$ triplets to have
observations. Coverage rate is a parameter of the simulation that affects how
close unseen triplets will be to the closest seen triplet. A high coverage
rate means most triplets are seen, so many unseen triplets are directly
connected to seen triplets via composition. A low coverage rate means few
triplets are seen, so many unseen triplets are a few composition factors away
from the closest seen triplet. We empirically set it to 0.5, which gives us a
good variety of typologies. Azimuth and motion model measurements are then
randomized using the proper noise models (section 2) from camera trajectory to
the selected triplets. The rest are unseen triplets $V_{v}^{unseen}$.

For our study, we prefer factor graphs with various levels of connectivity
between seen and unseen triplets. Therefore, we also apply a process for
selecting factor graphs with various connectivity levels. Given a factor
graph, we calculate TSC for each triplet as described in 5.3.4. Since
measurement-based estimation is not relevant in this stage, we just initialize
TSC to be 1 for all seen triplets $TSC^{i,j:k}=1$, $\forall V_{v}^{i,j:k}\in
V_{v}^{seen}$ and 0 for all $V_{v}^{unseen}$, and we apply Alg. 3. We look at
the histogram of topology scores of all triplets in the graph and calculate
the corresponding entropy (figure 20). We call this _connectivity score_. It
is a measure of the variability of topology in the graph. If the connectivity
score is high, it means the distribution of TCP in the graph is close to
uniform, and a low connectivity score means there are only a few dominant TCP
values in the graph. Therefore we randomly generate multiple-factor graphs and
choose the best connectivity score.

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

Figure 20:  For generating factor graph scenarios, we randomize multiple
graphs and use a TSC histogram to select graphs with various connectivity
levels. (a), (c), (e), and (g) are examples of selected factor graphs. (b),
(d), (f), and (h) are the corresponding TPC histograms and connectivity
scores.

####  6.4.4 Information Decay Model vs. Composition

With 5400 simulated factor graph scenarios, we solve all seen triplets using
their measurements and our fast variant of the single triplet solver (section
4.5). We then use the composition-based Alg. 2 to propagate data from seen
triplets to all unseen triplets and calculate the information score for each
one (section 5.3.3).

Then we generate a _topology score_ for all triplets with our information
decay model and Alg. 3 as detailed in sections 5.3.2 and 5.3.4. This score
predicts the amount of information that will get to each triplet based on
graph topology. Finally, we compare the topology score to the actual
information score for each triplet.

Figure 21 shows how our information decay model prediction relates to the
actual composition-based information score. In this figure, we collect
topology scores of all triplets in all scenarios and bin them into ten bins.
For each bin, we collect the information scores of the corresponding triplets
and show percentile 25, median, and percentile 75 statistics. We see that the
information decay model correlates reasonably to the composition-based
propagation.

Figure 21:  The correlation between the Information decay model and
composition. Topology scores of all triplets in all scenarios are collected
into ten bins. For each bin, we show the percentile 25, median, and percentile
75 statistics of the corresponding triplets information scores.

This is a validation of our information decay model and the main contribution
of this study. Having a simplified model for qualitative factor graph data
propagation means we understand this process, which also might be valuable for
active qualitative planning.

To test if our information decay model is trivial, we also tried several other
simplified models for predicting qualitative data propagation via composition.
An example is a model we call _composition level_ (CL). This model sets the
composition level for seen triplets $V_{v}^{seen}$ to be 0. Propagation via a
composition factor just increases the composition level by 1. We denote the
composition level of a triplet node $v_{v}^{AB:C}$ to be: $CL^{AB:C}$. As in
section 5.3.2 we address the propagation through a composition factor that
involves three triplet nodes (e.g., AB:C, BC:D, AB:D) and divides the model
into two cases. In case of two non-updated nodes and one updated node (e.g.,
$V_{v}^{BC:D},V_{v}^{AB:D}\in V_{v}^{\neg u};V_{v}^{AB:C}\in V_{v}^{u}$):

$CL^{BC:D}=CL^{AB:D}=CL^{AB:C}+1,$ (24)

and in case one node is not updated, and two nodes are (e.g.,
$V_{v}^{AB:C},V_{v}^{BC:D}\in V_{v}^{u};V_{v}^{AB:D}\in V_{v}^{\neg u}$):

$CL^{AB:D}=\min\\{CL^{AB:C},CL^{BC:D}\\}+1.$ (25)

Then composition level is propagated through the graph using an algorithm
similar to Alg. 3.

Comparing the information decay-based topological score to the composition
level model, we see that TCP correlates more to the actual composition-based
information score (figure 22). Since the composition level does not use the
information in the unary factors, we use a simplified version of TCP to make
an unbiased comparison, using the same information decay model and graph
propagation algorithm but initializing TCP to be 1 for all $V_{v}^{seen}$
instead of $ISC(\mathbb{P}({S^{ij:K}}))$. Statistics binning is done the same
way for both. The composition level graph is incomplete because it naturally
gets limited values but is binned into the same ten bins. Also, to make the
graphs comparable, in figure 22, we present a normalized version of the
composition level:

$\hat{CL}^{ij:k}=1-\frac{CL^{ij:k}}{CL_{m}ax},$ (26)

where $CL_{m}ax$ is the single maximal composition level in the graph.

(a)

(b)

Figure 22:  Comparing two simplified models for qualitative data propagation
to actual composition-based information score (as described in 6.4.4).
Topology score (a) represents our information decay model; (b) is an
alternative composition level model. For both scores, all triplets from all
scenarios are collected into (the same) 10 bins. For each bin, percentile 25,
median, and 75 statistics are shown. (a) exhibits a better correlation than
(b).

We conclude that only some ways of modeling composition factor propagation are
valuable and that the information decay model is noticeably useful.

####  6.4.5 Discussion

Looking at data propagation in the qualitative map gives us several important
insights. First, the qualitative composition can propagate significant
information, even to landmark triplets that were never seen together.

We also show that our information decay model reasonably predicts how much
information is propagated by composition. Hence, we have a good simple model
for understanding data propagation in the qualitative map regarding its
topology and existing information.

This process of data propagation and the innovative connection between data
propagation and graph topology are essential tools when addressing real-world
qualitative autonomous planning.

##  7 Conclusions

This paper presents a new approach for localization and mapping based on
qualitative spatial reasoning. We use a new and more natural and general
formulation to achieve several innovations and improvements, taking a few
steps towards a whole and valuable qualitative SLAM framework. We enable
incorporating a motion model in qualitative estimation to improve run time and
performance. Our method allows easy use of various qualitative space
partitions and underlying SLAM solvers. To demonstrate the benefits of
qualitative inference, we suggest a sampling-based global non-linear algorithm
that does not need initialization and then develop a very fast approximated
algorithm with minimal compromise on performance.

Furthermore, we show how to represent the problem using a factor graph and
propagate data efficiently using the inherent properties of qualitative
geometry. We also infer landmark triplets with no direct observations, an
essential building block for active qualitative planning. Furthermore, we give
a simple model for understanding the behavior of this qualitative data
propagation. Our approach can be complemented by finding more general or
optimal algorithms for data propagation in the factor graph and understanding
their performance limits.

In addition to improving complexity and performance compared to the state-of-
the-art [22] and [23], we show that our approach gets practical performance
for low-compute mapping and localization in cases where the exact metric
location is not essential.

This work lays the basics for further investigating and expanding qualitative
localization and mapping. We see an indication that additional research may
lead to a practical low-compute alternative for low-cost autonomous agents in
various tasks. Some of the directions we envision for future research are
handling recognition errors, addressing complex volumed landmarks and 3D
geometry, active qualitative planning [37], and building a large and diverse
qualitative SLAM or active planning dataset based on existing standard
datasets.

## References

  * [1] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. D. Reid, and J. J. Leonard, “Simultaneous localization and mapping: Present, future, and the robust-perception age,” _IEEE Trans. Robotics_ , vol. 32, no. 6, pp. 1309 – 1332, 2016.
  * [2] F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” Georgia Institute of Technology, Tech. Rep., 2012, gTSAM.
  * [3] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in _Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA)_ , Shanghai, China, May 2011.
  * [4] S. A. et al, “Ceres solver,” Google, Tech. Rep., 2016, http://ceres-solver.org,.
  * [5] R. F. Salas-Moreno, R. A. Newcombe, H. Strasdat, P. Kelly, and A. J. Davison, “Slam++: Simultaneous localisation and mapping at the level of objects,” in _IEEE Conf. on Computer Vision and Pattern Recognition (CVPR)_ , 2013, pp. 1352–1359.
  * [6] J. Placed, J. Strader, H. Carrillo, N. Atanasov, V. Indelman, L. Carlone, and J. A. Castellanos, “A survey on active simultaneous localization and mapping: State of the art and new frontiers,” Tech. Rep., July 2022. [Online]. Available: https://arxiv.org/abs/2207.00254.pdf
  * [7] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” _Intl. J. of Robotics Research_ , vol. 31, pp. 217–236, Feb 2012.
  * [8] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smoothing and mapping,” _IEEE Trans. Robotics_ , vol. 24, no. 6, pp. 1365–1378, Dec 2008\.
  * [9] L. Polok, V. Ila, M. Solony, P. Smrz, and P. Zemcik, “Incremental block cholesky factorization for nonlinear least squares in robotics,” in _Robotics: Science and Systems (RSS)_ , 2013.
  * [10] D. Scaramuzza and F. Fraundorfer, “Visual odometry: Part i the first 30 years and fundamentals,” 2011.
  * [11] F. Fraundorfer and D. Scaramuzza, “Visual odometry. Part II: Matching, robustness, optimization, and applications,” 2012.
  * [12] L. Carlone, A. Censi, and F. Dellaert, “Selecting good measurements via l1 relaxation: A convex approach for robust estimation over graphs,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_. IEEE, 2014, pp. 2667–2674.
  * [13] G. H. Lee, F. Fraundorfer, and M. Pollefeys, “Robust pose-graph loop-closures with expectation-maximization,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , 2013, pp. 556–563.
  * [14] V. Indelman, E. Nelson, J. Dong, N. Michael, and F. Dellaert, “Incremental distributed inference from arbitrary poses and unknown data association: Using collaborating robots to establish a common reference,” _IEEE Control Systems Magazine (CSM), Special Issue on Distributed Control and Estimation for Robotic Vehicle Networks_ , vol. 36, no. 2, pp. 41–74, 2016.
  * [15] N. Sünderhauf and P. Protzel, “Switchable constraints vs. max-mixture models vs. RRR - a comparison of three approaches to robust pose graph SLAM,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , 2013.
  * [16] E. Olson and P. Agarwal, “Inference on networks of mixtures for robust robot mapping,” _Intl. J. of Robotics Research_ , vol. 32, no. 7, pp. 826–840, 2013.
  * [17] S. Pathak, A. Thomas, and V. Indelman, “A unified framework for data association aware robust belief space planning and perception,” _Intl. J. of Robotics Research_ , vol. 32, no. 2-3, pp. 287–315, 2018.
  * [18] D. Fourie, J. Leonard, and M. Kaess, “A nonparametric belief solution to the bayes tree,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , 2016.
  * [19] Q. Huang, C. Pu, D. Fourie, K. Khosoussi, J. P. How, and J. J. Leonard, “Nf-isam: Incremental smoothing and mapping via normalizing flows,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , 2021.
  * [20] S. Lowry, S. Niko, N. Paul, J. L. John, C. David, C. Peter, and J. M. Michael, “Visual place recognition: A survey,” _IEEE Transactions on Robotics 32.1 (2016): 1-19._ , 2016.
  * [21] T. Mossakowski and M. Reinhard, “Qualitative reasoning about relative direction of oriented points,” 2012.
  * [22] M. McClelland, M. Campbell, and T. Estlin, “Qualitative relational mapping for mobile robots with minimal sensing,” _Journal of Aerospace Information Systems_ , vol. 11, no. 8, pp. 497–511, 2014.
  * [23] J. Padgett and M. Campbell, “Probabilistic qualitative mapping for robots,” _Robotics and Autonomous Systems_ , vol. 98, pp. 292–306, 2017.
  * [24] T. S. Levitt and D. T. Lawton, “Qualitative navigation for mobile robots,” _Artificial intelligence_ , vol. 44, no. 3, pp. 305–360, 1990.
  * [25] J. Y. Zheng and S. Tsuji, “Panoramic representation for route recognition by a mobile robot,” _International Journal of Computer Vision_ , vol. 9, no. 1, pp. 55–76, 1992.
  * [26] C. Schlieder, “Representing visible locations for qualitative navigation,” _Qualitative reasoning and decision technologies_ , vol. 523532, 1993.
  * [27] T. Wagner, U. Visser, and O. Herzog, “Egocentric qualitative spatial knowledge representation for physical robots,” _Robotics and Autonomous Systems_ , vol. 49, no. 1, pp. 25–42, 2004.
  * [28] R. Moratz, L. Dominik, and M. Till, “A condensed semantics for qualitative spatial reasoning about oriented straight line segments,” 2011.
  * [29] C. Freksa, “Using orientation information for qualitative spatial reasoning,” 1992\.
  * [30] C. Freksa and K. Zimmermann, “On the utilization of spatial structures for cognitively plausible and efficient reasoning,” in _IEEE International Conference on Systems, Man and Cybernetics_ , 1992, pp. 261–266.
  * [31] R. Moratz and R. Marco, “Qualitative spatial reasoning about relative point position,” 2008.
  * [32] A. Scivos and N. Bernhard, “The finest of its class: The natural point-based ternary calculus lr for qualitative spatial reasoning.” _International Conference on Spatial Cognition. Springer, Berlin, Heidelberg._ , 2004.
  * [33] J. Padgett and M. Campbell, “Q-link: A general planning architecture for navigation with qualitative relational information,” _Robotics and Autonomous Systems_ , vol. 108, pp. 51–65, 2018.
  * [34] I. Zilberman, E. Rivlin, and V. Indelman, “Incorporating compositions in qualitative approaches,” _IEEE Robotics and Automation Letters_ , 2022.
  * [35] V. Indelman, L. Carlone, and F. Dellaert, “Planning in the continuous domain: a generalized belief space approach for autonomous navigation in unknown environments,” _Intl. J. of Robotics Research_ , vol. 34, no. 7, pp. 849–882, 2015.
  * [36] D. Kopitkov and V. Indelman, “No belief propagation required: Belief space planning in high-dimensional state spaces via factor graphs, matrix determinant lemma and re-use of calculation,” _Intl. J. of Robotics Research_ , vol. 36, no. 10, pp. 1088–1130, August 2017.
  * [37] I. Zilberman and V. Indelman, “Qualitative belief space planning via compositions,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , October 2022.
  * [38] Technion’s ANPL Lab, Roee Mor, “qsrslam.” [Online]. Available: https://github.com/anpl-technion/QSR_SLAM
  * [39] R. Mor and V. Indelman, “Probabilistic qualitative localization and mapping,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , 2020\.
  * [40] V. Pierlot and M. Van Droogenbroeck, “A new three object triangulation algorithm for mobile robot positioning,” _IEEE Transactions on Robotics_ , vol. 30, no. 3, pp. 566–577, 2014.
  * [41] F. Dellaert and Kaess, “Factor graphs for robot perception,” _Foundations and Trends® in Robotics_ , vol. 6, no. 1-2, pp. 1–139, 2017.
  * [42] K. Khosoussi, M. Giamou, G. S. Sukhatme, S. Huang, G. Dissanayake, and J. P. How, “Reliable graph topologies for SLAM,” _Intl. J. of Robotics Research_ , 2018.
  * [43] A. Kitanov and V. Indelman, “Topological multi-robot belief space planning in unknown environments,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , 2018.
  * [44] ——, “Topological information-theoretic belief space planning with optimality guarantees,” _arXiv preprint arXiv:1903.00927_ , 3 2019.
  * [45] M. Shienman, A. Kitanov, and V. Indelman, “Ft-bsp: Focused topological belief space planning,” _IEEE Robotics and Automation Letters (RA-L)_ , vol. 6, no. 3, pp. 4744–4751, 2021.
  * [46] F. Dellaert and M. Kaess, “Square root sam: Simultaneous localization and mapping via square root information smoothing.” _The International Journal of Robotics Research_ , vol. 25, no. 12, pp. 1181–1203, 2006.
  * [47] Georgia Tech’s BORG Lab, Frank Dellart, “gtsam.” [Online]. Available: https://gtsam.org/
  * [48] Q. Su and Y.-C. Wu, “On convergence conditions of gaussian belief propagation,” _IEEE Transactions on Signal Processing_ , vol. 63, no. 5, pp. 1144–1155, 2015.