# Bayesian Heuristics for Robust Spatial Perception

Aamir Hussain Chughtai, Muhammad Tahir and Momin Uppal The authors are with
Department of Electrical Engineering, Lahore University of Management
Sciences, DHA Lahore Cantt., 54792, Lahore Pakistan. (email:
<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

Spatial perception is a key task in several machine intelligence applications
such as robotics and computer vision. In general, it involves the nonlinear
estimation of hidden variables that represent the system’s state. However, in
the presence of measurement outliers, the standard nonlinear least squared
formulation results in poor estimates. Several methods have been considered in
the literature to improve the reliability of the estimation process. Most
methods are based on heuristics since guaranteed global robust estimation is
not generally practical due to high computational costs. Recently general
purpose robust estimation heuristics have been proposed that leverage existing
non-minimal solvers available for the outlier-free formulations without the
need for an initial guess. In this work, we propose three Bayesian heuristics
that have similar structures. We evaluate these heuristics in practical
scenarios to demonstrate their merits in different applications including 3D
point cloud registration, mesh registration and pose graph optimization. The
general computational advantages our proposals offer make them attractive
candidates for spatial perception tasks.

###### Index Terms:

Spatial Perception, Measurement Outliers, Nonlinear Estimation, Variational
Bayes, Expectation-Maximization, Statistical Inference, Parameter and State
Estimation.

##  I Introduction

Several machine intelligence tasks in robotics depend on reliable spatial
perception which involves the estimation of the unknown latent variable
representing the state describing the system. Examples of spatial perception
include object detection and localization, motion estimation, simultaneous
localization and mapping (SLAM) [1, 2, 3, 4] etc. The information for
inference is available in form of noisy observations which can generally be
represented as transformations of the hidden variable given as

$\mathbf{y}_{i}=\mathbf{h}_{i}(\mathbf{x})+\mathbf{\epsilon}_{i}$ (1)

where the ith measurement $\mathbf{y}_{i}$ ($i=1,\ldots,m$), of a batch of
data $\mathbf{y}$, is expressed as a known nonlinear function
$\mathbf{h}_{i}(.)$ of the unknown variable of interest $\mathbf{x}$ corrupted
by random noise $\mathbf{\epsilon}_{i}$. Under the assumption that
$\mathbf{\epsilon}_{i}$, described with zero-mean Gaussian noise statistics
with the precision matrix $\bm{\Omega}_{i}$ (inverse of the covariance
matrix), is uncorrelated across each measurement channel, the maximum a
posteriori (MAP) estimate has the following equivalent least square
formulation [3]

$\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i=0}^{m}\left\|\mathbf{y}_{i}-\mathbf{h}_{i}(\mathbf{x})\right\|_{\bm{\Omega}_{i}}^{2}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i=0}^{m}{\big{(}r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right)\big{)}}^{2}$
(2)

where $\mathcal{X}$ denotes the domain of $\mathbf{x}$, the notation
$\|\mathbf{e}\|_{\bm{\Omega}}^{2}={\mathbf{e}}^{\top}\bm{\Omega}\mathbf{e}$
and $r_{i}(\mathbf{y}_{i},\mathbf{x})$ is the residual error for the ith
measurement. Note that $r_{0}(\mathbf{y}_{0},\mathbf{x})$ incorporates the
regularizing term considering a Gaussian prior for $\mathbf{x}$. It is well
known that the cost function in (2) leads to brittle estimates in face of
measurement outliers owing to unwanted over-fitting to the corrupted data [5].
The observations can be easily plagued with outliers in practice due to sensor
failures, environmental factors, or due to erroneous data association by the
preprocessing front-end algorithms [6, 7]. This limits the reliability of
perception-based tasks and is therefore a dynamic research area in robotics.

Owing to the underlying functional nonlinearities and nonconvexity of the
domain, even solving (2) globally for common spatial perception applications
can be challenging. However, several estimators have been devised in this
regard for various applications including point cloud registration, mesh
registration, pose graph optimization [8, 9, 10] etc. These are commonly
termed as non-minimal solvers which utilize all the measurements for
estimation. On the other hand, minimal solvers use the smallest number of
observations for estimation [11].

The problem of estimation in the presence of outliers becomes more complicated
since the standard cost function in (2) is inadequate for this purpose.
Therefore, several approaches have been devised in this regard. Most of the
methods are heuristics that offer efficient solutions mostly without
guarantees [12, 13, 14, 15, 11]. Other specialized guaranteed approaches also
exist that provide solutions with formal certificates. However, their
practicality gets limited due to issues with scalability to large-scale
problems as the underlying semidefinite programs (SDP) can have a very high
computational budget [16, 17]. Naturally, efficient heuristics become the
default choice to enable such applications. In the literature, hybrid
approaches have also been advocated where the solutions obtained from
heuristics are subsequently evaluated for optimality [18]. Since these hybrid
approaches have been found to work effectively in practice, efficient
heuristics are highly desired.

Different general purpose heuristics for robust spatial perception have been
designed based on consensus maximization aiming to maximize observations
within a predefined inlier threshold during estimation [6]. The famous random
sample consensus (RANSAC) approach [12] is extensively employed relying on
minimal solvers for operation. Realizing the fragility of RANSAC under a high
outlier regime and its scalability issues, Adaptive Trimming (APAPT) has been
proposed to address these limitations [13]. ADAPT adopts non-minimal solvers
in its design.

Another popular approach is M-estimation [19] which relies on robust cost
functions applied to the residuals for resilience against data corruption.
Since these formulations are generally inefficient to solve globally [6], the
recently introduced graduated non-convexity (GNC) approach proposes an
efficient heuristic [11]. It replaces the underlying non-convex functions with
surrogate functions for two common cost functions namely the Geman McClure
(GM) and the truncated least square (TLS). Using the Black-Rangarajan duality
it casts an equivalent formulation which is then iteratively solved using
variable and weight update steps alternatively resulting in GNC-GM and GNC-TLS
methods. These are general-purpose robust estimators that employ non-minimal
solvers during the variable update step and require no initial guess for
operation.

The relevant literature also indicates the use of Bayesian methods for
outlier-robust estimation. In [20], a method to tune M-estimators is
suggested. It aims to estimate the tuning parameters of the M-estimators
within an Expectation Maximization (EM) framework. However, the overall scheme
is complicated due to its reliance on adaptive importance sampling. Another
Bayesian EM-based method is reported in [15] where the parameters of a general
robust function are estimated along with the primary variable of interest. The
method invokes iteratively reweighted least squares (IRLS) within its EM
framework which adds an additional layer of approximation during inference
since IRLS performance can be sensitive to initialization. Other Bayesian
methods, reported in the filtering context, do not involve M-estimation for
inference. For example, methods in [21, 22] propose handling outliers by
modifying the standard Gaussian likelihood distributions and subsequently
perform inference. However, these algorithms do not treat outliers
independently for each measurement channel due to under-parameterization [23].
Obviating this limitation, two similar Bayesian methods have been proposed:
the recursive outlier-robust (ROR) [24] and the selective observations
rejecting (SOR) [23] method. We build on these two methods for the general
nonlinear robust estimation problem with the following contributions.

  * •

We study the limitations of the standard ROR and SOR methods which shows why
these standard approaches falter in spatial perception applications. The
analysis suggests the need for adapting the hyper-parameters, governing the
outlier characteristics, during inference.

  * •

We propose three methodologies to overcome the shortcomings of the standard
approaches. Similar to the GNC methods, we are interested in invoking the
existing non-minimal estimators. To this end, we consider point estimates for
$\mathbf{x}$ and use the EM framework. Since EM can be viewed as a special
case of variational Bayes (VB), the adaptation is possible. The proposed
approaches are similar to the iterative GNC methods with alternating variable
and weight update steps.

  * •

We also evaluate the proposals in several experimental scenarios and benchmark
them against the GNC methods which are the state-of-the-art general purpose
heuristics for robust spatial perception.

The structure of the remaining paper is as follows. In Section II, we motivate
the selection of the ROR and SOR methods which we build upon. Moreover, we
discuss the inferential tools employed in our proposals. In Section III, we
discuss the limitations of the standard approaches and present methodologies
to overcome their shortcomings. In Section IV, we discuss the experimental
results. Lastly, Section V provides concluding remarks along with some future
directions.

##  II Relevant Bayesian methods and tools

In this section, we first briefly discuss the motivation for choosing the two
Bayesian methods: ROR and SOR. Moreover, we provide a short primer on the
Bayesian tools that we leverage in our proposals in the upcoming section.

###  II-A Choice of the Bayesian methods for robust estimation

Recently, ROR and SOR methods have been successfully applied for devising
robust nonlinear filtering techniques with satisfactory performance results.
In these methods, estimation of $\mathbf{x}$ in (1) relies on modifying the
measurement noise statistics. The choice is motivated by the inability of the
nominal Gaussian noise to describe the data in face of outliers. Subsequently,
the noise parameters are jointly estimated with $\mathbf{x}$. Bayesian theory
offers attractive inferential tools for estimating the state and parameters
jointly enabling iterative solutions [25, 26]. We adapt these methods to the
general nonlinear estimation context of (1) and (2) where non-minimal solvers
for estimation are available for the outlier-free cases. The motivation for
choosing these particular methods is twofold. First, the formulations of these
filters lend their modification conveniently to the nonlinear problem at hand.
Moreover, owing to the modeling simplicity, the choice of the hyperparameters
for the noise statistics is intuitive for adaptation to our case.

###  II-B EM as a special case of VB

For solving the problem in (2), we aim to use non-minimal solvers, which have
been developed and tested for different applications. To that end, we need to
cast the ROR and SOR algorithms in a way that existing nonlinear least squared
solvers are invoked during inference. These Bayesian methods are devised using
VB which leads to distributions for the state and parameters. However, the
available solvers generally result in point estimates for the state.
Therefore, to enable adoption of these Bayesian approaches for robust spatial
perception applications, we adopt the EM method which as shown in the Bayesian
literature can be viewed as a special case of the VB algorithm [27]. We first
present the VB method and then interpret EM method as its special case.

####  II-B1 VB

Suppose that we are interested in estimating multivariate parameter
$\bm{\theta}$ from data $\mathbf{y}$. For tractability, we can resort to the
VB algorithm considering the mean-field approximation where the actual
posterior is approximated with a factored distribution as [28]

$p(\bm{\theta}|\mathbf{y})\approx\prod_{j=1}^{J}q(\bm{\theta}_{j})$ (3)

where $J$ partitions of $\bm{\theta}$ are assumed with the $j$th partition
given as $\bm{\theta}_{j}$. The VB marginals can be obtained by minimizing the
Kullback-Leibler (KL) divergence between the product approximation and the
true posterior resulting in

$\displaystyle q(\bm{\theta}_{j})$ $\displaystyle\propto
e^{\big{(}\big{\langle}\mathrm{ln}(p(\bm{\theta}|\mathbf{y}))\big{\rangle}_{q(\bm{\theta}_{-j})}\big{)}}\
\forall\ j$ (4)

where ${q(\bm{\theta}_{-j})}=\prod_{k\neq j}q(\bm{\theta}_{k})$ and
$\langle.\rangle_{q(\mathbf{{x}})}$ denotes the expectation of the argument
with respect to the distribution $q(\mathbf{{x}})$. The VB marginals can be
obtained by iteratively invoking (4) till convergence.

####  II-B2 EM

From the Bayesian literature, we know that the EM method can be viewed as a
special case of the VB algorithm considering point densities for some of the
factored distributions in (3). In particular, the factored distributions which
are assumed as point masses in (3) can be written as delta functions

$\displaystyle
q(\bm{\theta}_{n})=\delta(\bm{\theta}_{n}-\hat{\bm{\theta}}_{n})$ (5)

with $n$ denoting the indices where such assumption is taken. Resultingly, we
can update the parameter of $q(\bm{\theta}_{n})$ using as

$\hat{\bm{\theta}}_{n}=\underset{\bm{\theta}_{n}}{\operatorname{argmax}}\big{\langle}\mathrm{ln}(p(\bm{\theta}|\mathbf{y}))\big{\rangle}_{q(\bm{\theta}_{-n})}\forall\
n$ (6)

The expression (6) is formally known as the M-Step of the EM method. The
remaining factored distributions not considered as point masses can be
determined using (4) where the expectation with respect to
$q(\bm{\theta}_{n})$ would simply result in sampling
$\mathrm{ln}(p(\bm{\theta}|\mathbf{y})$ at $\hat{\bm{\theta}}_{n}$. This is
formally called as the E-Step in the EM method.

Treating EM as particular case of VB allows us another advantage in addition
to leveraging the existing non-minimal point estimators for the system state.
It allows us the liberty to treat those parameters with point masses where the
expectation evaluation with respect to that parameter would otherwise be
unwieldy.

##  III Proposed Algorithms

Having chosen the two particular methods for application in robust perception
tasks and having interpreted EM as a special case of VB, we are in a position
to present our proposals. In this section, we first present the standard ROR
method and discuss its limitations. Based on the analysis, we propose a
methodology to overcome the drawbacks. Then we shift our attention to the SOR
method. We present the standard SOR technique and explain its drawbacks. Based
on the insights drawn, we propose two frameworks to deal with the
shortcomings.

###  III-A ROR Methods

####  III-A1 Standard ROR

We use the version of the ROR method as originally reported in Section 2.5 of
[24] where conditionally independent measurements are considered. The ROR
method, as originally reported, assumes $\mathbf{y}_{i}$ (measurement from
each channel) to be scalar but it can be a vector in general. Accordingly, the
likelihood to robustify (1) is the multivariate Student-t density. We denote
the distribution as
${\mathrm{St(\mathbf{z}_{s}|\bm{\phi}_{s},\mathbf{\Sigma}_{s},\eta)}}$ where
the random vector $\mathbf{z}_{s}$ obeys the Student-t density and the
parameters include $\bm{\phi}_{s}$ (mean), $\mathbf{\Sigma}_{s}$ (scale
matrix) and $\eta$ (degrees of freedom) which controls the kurtosis or heavy-
tailedness. Resultingly, we can write the likelihood density as [24]

$\displaystyle p(\mathbf{y}_{i}|\mathbf{x})$
$\displaystyle={\mathrm{St}}\big{(}\mathbf{y}_{i}|\mathbf{h}_{i}(\mathbf{x}),\bm{\Omega}_{i}^{-1},\nu_{i}\big{)}=\int
p(\mathbf{y}_{i}|\mathbf{x},\lambda_{i})p(\lambda_{i})d\lambda_{i}$ (7)

with the conditional likelihood following the multivariate Gaussian density
given as

$p(\mathbf{y}_{i}|\mathbf{x},\lambda_{i})=\mathcal{N}(\mathbf{y}_{i}|\mathbf{h}_{i}(\mathbf{x}),(\lambda_{i}\bm{\Omega}_{i})^{-1})$
(8)

where $\mathcal{N}(\mathbf{z}_{n}|\bm{\phi}_{n},\mathbf{\Sigma}_{n})$
symbolizes that the random vector $\mathbf{z}_{n}$ follows the Gaussian
distribution parameterized by $\bm{\phi}_{n}$ (mean) and $\mathbf{\Sigma}_{n}$
(covariance matrix). $\lambda_{i}$ in (7) obeys the univariate Gamma
distribution given as [24]

$p(\lambda_{i})=\mathcal{G}(\lambda_{i}|\frac{\nu_{i}}{2},\frac{\nu_{i}}{2})$
(9)

where $\mathcal{G}({z}_{g}|a_{g},b_{g})$ denotes that the random variable
$z_{g}$ follows the Gamma distribution with the shape parameter $a_{g}$ and
the rate parameter $b_{g}$ [29]. The normalizing constant of the distribution
is denoted as

$f(a,b)=\frac{b^{a}}{\Gamma(a)}$ (10)

where ${\Gamma(a)}$ denotes the Gamma function.

Denoting $\bm{\lambda}$ as the vector with ${\lambda_{i}}$ its $i$th element,
we can write the following using the Bayes theorem

$p(\mathbf{x},\bm{\lambda}|\mathbf{y})\propto{p(\mathbf{y}|\bm{{\lambda}},\mathbf{x})p(\mathbf{x})p(\bm{{\lambda}})}$
(11)

Resultingly, the log-posterior, can be written as

$\displaystyle
p(\mathbf{x},\bm{\lambda}|\mathbf{y})=\Big{\\{}\sum_{i=1}^{m}\Big{(}-0.5\lambda_{i}{\big{(}r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right)\big{)}}^{2}-0.5\nu_{i}\lambda_{i}$
$\displaystyle+(0.5(\nu_{i}+d)-1)\ln({\lambda}_{i})\Big{)}-0.5{\big{(}r_{0}\left(\mathbf{y}_{0},\mathbf{x}\right)\big{)}}^{2}+constant\Big{\\}}$
(12)

To proceed further, we seek the following VB factorization of the posterior
distribution

$p(\mathbf{x},\bm{{\lambda}}|\mathbf{y})\approx
q(\mathbf{x})q(\bm{{{\lambda}}})$ (13)

Based on (6) and (12), the state variable $\hat{\mathbf{x}}$ is estimated
using the VB/EM theory as [24]

$\hat{\mathbf{x}}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i=0}^{m}w_{i}{\left(r_{i}(\mathbf{y}_{i},\mathbf{x})\right)}^{2}$
(14)

where $w_{i}=\langle{{\lambda}}_{i}\rangle_{q({{\lambda}}_{i})}\forall\ i>0$.
We have considered a point estimator for $\mathbf{x}$ i.e.
$q(\mathbf{x})=\delta(\mathbf{x}-\hat{\mathbf{x}})$ to utilize existing
solvers for spatial perception tasks mainly available in this form.

Similarly, the weights can be updated as [24]

$w_{i}=\left({1+{\frac{{\hat{r}_{i}^{2}}-d}{\nu+d}}}\right)^{-1}\ \forall\
i>0$ (15)

where $d$ denotes the dimension of $\mathbf{y}_{i}$ and
${\hat{r}_{i}^{2}}={\big{(}r_{i}\left(\mathbf{y}_{i},\hat{\mathbf{x}}\right)\big{)}}^{2}$.
We assume $\nu_{i}=\nu\ \forall\ i$ and use $w_{i}$ to weight the precision
matrix instead of $\bar{\lambda}$ as in the original work to remain consistent
with comparative works. Since we assume outliers occur only in the
measurements, the weight for the regularizing term remains fixed as 1 i.e.
$w_{0}=1$ in (14).

### Limitations of standard ROR

Starting with $w_{i}=1\ \forall\ i$, the standard ROR invokes (14) and (15)
iteratively till convergence. The technique has shown good performance in
filtering context for different practical examples such as target tracking
[24] and indoor localization [23]. This can be attributed to the appearance of
well-regularized cost functions (2). However, for advanced problems in robust
spatial perception, the performance of standard ROR compromises at high
outlier ratios since it fails to capture the outlier characteristics by fixing
$\nu$ which governs the heavy-tailedness of noise or the characteristics of
outliers. Empirical evidence confirms this observation.

Figure 1: $w_{i}$ vs ${\hat{r}_{i}^{2}}/\mu$ in ROR.

For further understanding of this limitation consider how $w_{i}$ changes with
parameters of Student-t distribution. The variation of $w_{i}$ against
${\hat{r}_{i}^{2}}/\mu$ is shown in Fig. 1 where $\mu=\nu+d$. The plot of (15)
would only be a shifted version of the plot in Fig. 1. Since
$\hat{r}_{i}^{2}\gg d$ when outlier appears in the ith dimension, we can
simplify (15) as $w_{i}=({1+{\frac{{\hat{r}_{i}^{2}}}{\mu}}})^{-1}$ indicating
the importance of the kurtosis $\nu$ and resultingly $\mu$. It can be observed
that the residuals are gradually downweighted with increasing magnitude during
estimation with $w_{i}=0.5$ for ${\hat{r}_{i}^{2}}=\mu$. Since the residuals
are evaluated considering the state estimate using all the measurements
initially, it is possible that the squared residuals even for the uncorrupted
dimensions become greater than the prefixed $\mu$ downweighting them in the
process. This can lead to performance issues. Therefore, this calls for
adapting $\mu$ by considering the residuals evaluated with clean and corrupted
measurements. Given the limitations, we propose a variant of the standard ROR
method where $\mu$ is adapted during iterations considering the residuals at
each iteration. We call it Extended ROR or simply EROR.

####  III-A2 EROR

In EROR, we propose adaptation of $\mu$ during iterations considering the
updated squared residuals based on the relationship of the $w_{i}$ and $\mu$.
The choice of $\mu$ is done such that weights assigned to residuals span the
maximum portion of the zero to one range. In other words, the largest
residuals need to be pruned with the smallest weights in the weighted least
squared cost function and vice versa. In particular, for
${{\hat{r}_{i}^{2}}}={\hat{r}_{\max}^{2}}$ with $w_{i}\rightarrow 0$ leads to
$\mu\rightarrow 0$ ($\mu=\frac{{\hat{r}_{i}^{2}}}{1/{w_{i}}-1}$). Practically,
$\mu\ll{\hat{r}_{\max}^{2}}$. Similarly for the other extreme
${{\hat{r}_{i}^{2}}}={\hat{r}_{\min}^{2}}$ with $w_{i}\rightarrow 1$ leads to
$\mu\rightarrow\infty$. Practically, $\mu\gg{\hat{r}_{\min}^{2}}$. To cater
for both extremes we propose
$\mu=\mathrm{mean}({\hat{r}_{\max}^{2}},{\hat{r}_{\min}^{2}})$. Also, $\mu$ is
lower bounded by $\chi$ to ensure residuals within a minimum threshold are not
neglected during estimation. The notion of $\chi$ is similar to $\bar{c}^{2}$
as in [11] which is set as the maximum error expected for the inliers. Note
that adaptation of $\mu$ is intuitive which can be viewed as an additional
step in the standard ROR devised using VB. EROR is presented as Algorithm 1.

Initialize $w_{i}=1\ \forall\ i$

while  _the convergence criterion has not met_ do

Variable update:
$\hat{\mathbf{x}}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i}w_{i}{(r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right))}^{2}$

Residual update:
${\hat{r}_{i}^{2}}={\big{(}r_{i}\left(\mathbf{y}_{i},\hat{\mathbf{x}}\right)\big{)}}^{2}$
$\forall\ i$

Parameteric update:
${\hat{r}_{\max}^{2}}=\max_{i}({\hat{r}_{i}^{2}});{\hat{r}_{\min}^{2}}=\min_{i}({\hat{r}_{i}^{2}})\
\mathrm{s.t.}\ i>0$
$\mu=\max(\mathrm{mean}({\hat{r}_{\max}^{2}},{\hat{r}_{\min}^{2}}),\chi)$

Weight update: $w_{i}=\frac{1}{1+({{\hat{r}_{i}^{2}}}/{\mu})}$ $\forall\ i>0$

end while

Algorithm 1 EROR

###  III-B SOR Methods

####  III-B1 Standard SOR

The SOR method, as originally reported [23], assumes $\mathbf{y}_{i}$ to be
scalar but it can be a vector in general. In the original work, an indicator
vector $\bm{\mathcal{I}}\in\mathbb{R}^{m}$ with Bernoulli elements is
introduced to describe outliers in the measurements. In particular,
${{\mathcal{I}}}_{i}=\epsilon$ indicates the occurrence of an outlier in the
ith dimension and ${{\mathcal{I}}}_{i}=1$ is reserved for the no outlier case.
Accordingly, the conditional likelihood to robustify (1) is a multivariate
Gaussian density function [23]

$\displaystyle
p(\mathbf{y}_{i}|\mathbf{x},{\mathcal{I}}_{i})={\mathcal{N}}\Big{(}\mathbf{y}_{i}|\mathbf{h}_{i}(\mathbf{x}),({{\mathcal{I}}}_{i}\bm{\Omega}_{i})^{-1}\Big{)}$
$\displaystyle=\frac{1}{\sqrt{{(2\pi)^{m}{|\bm{\Omega}_{i}^{-1}|}}}}e^{\left({-}0.5{{\mathcal{I}}}_{i}{\left(r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right)\right)}^{2}\right)}{{\mathcal{I}_{i}}}^{0.5}$
(16)

${\mathcal{I}_{i}}\ \forall\ i>0$ is assumed to have the following prior
distribution

$p({{\mathcal{I}}}_{i})=(1-{\theta_{i}})\delta({{{\mathcal{I}}}_{i}}-\epsilon)+{\theta_{i}}\delta({{{\mathcal{I}}}_{i}}-1)$
(17)

where $\theta_{i}$ denotes the prior probability of having no outlier in the
$i$th measurement channel. $\epsilon$ has the role of catering for describing
the anomalous data in effect controlling the covariance of outliers. Using the
Bayes theorem we can write

$p(\mathbf{x},\bm{\mathcal{I}}|\mathbf{y})\propto{p(\mathbf{y}|\bm{\mathcal{I}},\mathbf{x})p(\mathbf{x})p(\bm{\mathcal{I}})}$
(18)

where $\bm{\mathcal{I}}$ denotes the vector with ${\mathcal{I}_{i}}$ its $i$th
element.

As a result, the log-posterior is given as

$\displaystyle\ln(p(\mathbf{x},\bm{\mathcal{I}},b|\mathbf{y}))=\Big{\\{}\sum_{i=1}^{m}\Big{(}-0.5{\mathcal{I}}_{i}{\big{(}r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right)\big{)}}^{2}+0.5\ln({\mathcal{I}}_{i})$
$\displaystyle+\ln\left((1-{\theta_{i}})\delta({{{\mathcal{I}}}_{i}}-\epsilon)+{\theta_{i}}\delta({{{\mathcal{I}}}_{i}}-1)\right)\Big{)}-0.5{\big{(}r_{0}\left(\mathbf{y}_{0},\mathbf{x}\right)\big{)}}^{2}$
$\displaystyle+constant\Big{\\}}$ (19)

For tractable inference we resort to the following VB factorization of the
posterior distribution

$p(\mathbf{x},\bm{\mathcal{I}}|\mathbf{y})\approx
q(\mathbf{x})q(\bm{{\mathcal{I}}})$ (20)

Based on (6) and (19), the state estimate $\hat{\mathbf{x}}$ is updated using
the VB/EM theory as [23]

$\hat{\mathbf{x}}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i=0}^{m}w_{i}{(r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right))}^{2}$
(21)

with $w_{0}=1$ and

$\displaystyle w_{i}$
$\displaystyle=\langle{\mathcal{I}}_{i}\rangle_{q({\mathcal{I}}_{i})}\
\forall\ i>0$ (22)
$\displaystyle=\Omega_{i}+(1-\Omega_{i})\epsilon\approx\Omega_{i}$ (23)

where $\Omega_{i}$ parameterizes the VB posterior marginal
${q({\mathcal{I}}_{i})}$ corresponding to $\theta_{i}$ in
$p({{\mathcal{I}}}_{i})$. $\Omega_{i}$ is updated using (4) and (19) as [23]

$\displaystyle\Omega_{i}$
$\displaystyle=\frac{1}{1+{\sqrt{\epsilon}}(\frac{1}{\theta_{i}}-1){e^{\left(0.5{\hat{r}_{i}^{2}}(1-\epsilon)\right)}}}$
(24)

Since the hyperparameter $\epsilon$ is assumed to be a small positive number
we denote it as $\epsilon=\exp({{-{\rho}}^{2}})$. Assuming a neutral prior for
the occurrence of an outlier in the $i$th dimension i.e. ${\theta_{i}}=0.5$
(as reported originally [23]) and noting that
$\epsilon=\exp({{-{\rho}}^{2}})\approx 0$ we can write

$w_{i}\approx\Omega_{i}\approx\frac{1}{1+{e^{\left(0.5({\hat{r}_{i}^{2}}-\rho^{2})\right)}}}\forall\
i>0$ (25)

### Limitations of standard SOR

Starting with $w_{i}=1\ \forall\ i$, the standard SOR invokes (21) and (25)
iteratively till convergence. The technique has shown good performance in the
filtering context [23] but has limited ability in robust spatial perception
tasks as the standard ROR method. This drawback compromises the performance
especially at high outlier ratios since the standard SOR fails to capture the
outlier characteristics by fixing $\rho^{2}$ (or $\epsilon$) which governs the
covariance of outliers. Experimental evidence verifies this limitation.

Figure 2: $w_{i}$ vs ${\hat{r}_{i}^{2}}-\rho^{2}$ in SOR.

To further appreciate this limitation consider how $w_{i}$ changes with
${\hat{r}_{i}^{2}}-\rho^{2}$ as shown in Fig. 2. It can be observed that the
residuals are gradually downweighted with increasing magnitude during
estimation with $w_{i}=0.5$ for ${\hat{r}_{i}^{2}}=\rho^{2}$. Since the
residuals are evaluated considering the state estimate using all the
measurements initially, it is possible that the squared residuals even for the
uncorrupted dimensions become greater than the prefixed $\rho^{2}$
downweighting them during the inference process. This can lead to performance
issues. Therefore, this calls for adapting $\rho^{2}$ by considering the
residuals evaluated with clean and corrupted measurements.

Given the limitations, we present two methods based on the standard SOR
technique. Firstly, we propose a modification in the SOR method by explicitly
adapting $\rho^{2}$ during iterations considering the residuals at each
iteration. We call it Extended SOR or simply ESOR. Secondly, by modifying the
basic SOR model with a notion of adaptive unknown covariance of outliers, we
propose Adaptive SOR or simply ASOR where the parameters controlling the
covariance of outliers are learnt within the inferential procedure.

####  III-B2 ESOR

In ESOR, we modify $\rho^{2}$ during iterations taking into account the
updated squared residuals. In particular, we propose to select
$\rho^{2}={\sum_{i=0}^{m}}w_{i}{\hat{r}_{i}^{2}}/{\sum_{i=0}^{m}}w_{i}$. In
other words, $\rho^{2}$ is selected as the the effective centroid of points
given as squared residuals considering the weights assigned. With such an
intuitive choice, the probability of declaring an outlier in the $i$th
dimension is 0.5 when ${\hat{r}_{i}^{2}}$ equals the effective mean of squared
residuals. The residuals greater than $\rho$ become the candidates for
downweighing and vice versa during an iteration. Lastly, $\rho^{2}$ is lower
bounded by $\gamma$ to ensure residuals within a minimum threshold are not
neglected during estimation. ESOR is presented as Algorithm 2.

Initialize $w_{i}=1\ \forall\ i$

while  _the convergence criterion has not met_ do

Variable update:
$\hat{\mathbf{x}}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i}w_{i}{(r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right))}^{2}$

Residual update:
${\hat{r}_{i}^{2}}={\big{(}r_{i}\left(\mathbf{y}_{i},\hat{\mathbf{x}}\right)\big{)}}^{2}$
$\forall\ i$

Parameteric update:
$\rho^{2}=\max(\frac{{\sum_{i}}w_{i}{\hat{r}_{i}^{2}}}{{\sum_{i}}w_{i}},\gamma)$

Weight update: $w_{i}=\frac{1}{1+{e^{(0.5({\hat{r}_{i}^{2}}-\rho^{2}))}}}$
$\forall\ i>0$

end while

Algorithm 2 ESOR

####  III-B3 ASOR

For devising ESOR we adapt $\rho^{2}$ (or $\epsilon$) during iterations to
capture the characteristics of outliers. Nevertheless, the choice of selection
of $\rho^{2}$ which controls the covariance of outliers is entirely intuitive.
This can be viewed as an additional step within the standard SOR method, not
falling under the standard VB approach. However, the insights drawn from ESOR
with sound experimental performance suggest the merits of considering or
learning the characteristics of outliers during inference. With these
observations, we now present ASOR which is devised with the standard VB
approach. In contrast to EROR and ESOR, we jointly estimate the covariance
controlling factor along with the state and the weights in ASOR.

The conditional likelihood for designing ASOR remains same as for the standard
SOR given in (16). Building on the insights from EROR and ESOR, we need to
adapt covariances to describe the outliers. In particular, we assume that for
outlier occurrence in the $i$th observation channel i.e.
${{\mathcal{I}}}_{i}\neq 1$ in (16), ${{\mathcal{I}}}_{i}$ obeys a Gamma
probability density being supported on the set of positive real numbers.
Resultingly, we write the hierarchical prior distribution of
${{\mathcal{I}}}_{i}$ given as

$\displaystyle p({{\mathcal{I}}}_{i}|b)$
$\displaystyle=(1-{\theta_{i}})\underbrace{f(a,b){{{\mathcal{I}}}_{i}}^{a-1}e^{-b{{{\mathcal{I}}}_{i}}}}_{\mathcal{G}({{\mathcal{I}}}_{i}|a,b)}+{\theta_{i}}\delta({{{\mathcal{I}}}_{i}}-1)$
(26)

where we assume the two components of $p({{\mathcal{I}}}_{i}|b)$ in (26) as
disjoint with the Gamma density defined as zero for ${{{\mathcal{I}}}_{i}}=1$
without losing anything. This assumption helps in subsequent derivation. The
parameter $b$ is the factor that captures the common effect of outliers in
each observation channel. From the Bayesian theory we know that the conjugate
prior of $b$ is also a Gamma distribution given as [30]

$\displaystyle p(b)$ $\displaystyle=f(A,B){b}^{A-1}e^{-Bb}$ (27)

where $A$ and $B$ are the parameters of this Gamma distribution. We are now in
a position to invoke the Bayes theorem given as

$p(\mathbf{x},\bm{\mathcal{I}},b|\mathbf{y})\propto{p(\mathbf{y}|\bm{\mathcal{I}},\mathbf{x})p(\mathbf{x})p(\bm{\mathcal{I}}|b)p(b)}$
(28)

Resultingly, the log-posterior, which is used in subsequently derivation, is
given as

$\displaystyle\ln(p(\mathbf{x},\bm{\mathcal{I}},b|\mathbf{y}))=\Big{\\{}\sum_{i=1}^{m}\Big{(}-0.5{\mathcal{I}}_{i}{\left(r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right)\right)}^{2}+0.5\ln({\mathcal{I}}_{i})$
$\displaystyle+\ln\left((1-{\theta_{i}})f(a,b){{{\mathcal{I}}}_{i}}^{a-1}e^{-b{{{\mathcal{I}}}_{i}}}+{\theta_{i}}\delta({{{\mathcal{I}}}_{i}}-1)\right)\Big{)}$
$\displaystyle-0.5{\big{(}r_{0}\left(\mathbf{y}_{0},\mathbf{x}\right)\big{)}}^{2}+(A-1)\ln(b)-Bb+constant\Big{\\}}$
(29)

To proceed further we resort to the VB factorization given as

$p(\mathbf{x},\bm{\mathcal{I}},b|\mathbf{y})\approx
q(\mathbf{x})q(\bm{{\mathcal{I}}})q({b})$ (30)

Using the VB/EM theory and with the assumption that
$q(\mathbf{x})=\delta(\mathbf{x}-\hat{\mathbf{x}})$ we obtain the following
using (6) and (29)

$\hat{\mathbf{x}}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i=0}^{m}w_{i}{\left(r_{i}(\mathbf{y}_{i},\mathbf{x})\right)}^{2}$
(31)

where $w_{0}=1$ and

$\displaystyle w_{i}$
$\displaystyle=\langle{\mathcal{I}}_{i}\rangle_{q({\mathcal{I}}_{i})}\
\forall\ i>0$ (32)

Thanks to the notion of VB-conjugacy [27], it turns out that
$q(\mathcal{I}_{i})$ has a same functional form as of
$p({{\mathcal{I}}}_{i}|b)$ in (26). $q(\mathcal{I}_{i})$ is parameterized by
$\alpha$, $\beta_{i}$ and $\Omega_{i}$ corresponding to $a$, $b$ and
$\theta_{i}$ in $p({{\mathcal{I}}}_{i}|b)$ respectively. Resultingly, we can
evaluate the expectation in (32) as

$\displaystyle w_{i}=\Omega_{i}+(1-\Omega_{i})\alpha/\beta_{i}\ \forall\ i>0$
(33)

The VB marginal $q(\bm{\mathcal{I}})$ can be obtained using (4) and (29) as

$\displaystyle q(\bm{\mathcal{I}})\propto$
$\displaystyle\prod_{i=1}^{m}\Big{\\{}\mathcal{I}^{0.5}_{i}e^{-0.5{\hat{r}_{i}^{2}}\mathcal{I}_{i}}((1-{\theta_{i}})f(a,b){{{\mathcal{I}}}_{i}}^{a-1}e^{-\hat{b}{{{\mathcal{I}}}_{i}}}$
$\displaystyle+{\theta_{i}}\delta({{{\mathcal{I}}}_{i}}-1))\Big{\\}}$ (34)

where we have assume a point estimator for $b$ i.e. $q(b)=\delta(b-\hat{b})$
to simplify the arising expectations.

We can further write

$\displaystyle q(\bm{\mathcal{I}})=$
$\displaystyle\prod_{i=1}^{m}\Big{\\{}k_{i}(1-{\theta_{i}})f(a,\hat{b}){{{\mathcal{I}}}_{i}}^{\alpha-1}e^{-\beta_{i}\mathcal{I}_{i}}+$
$\displaystyle
k_{i}{\theta_{i}}e^{-0.5{\hat{r}_{i}^{2}}}\delta({{{\mathcal{I}}}_{i}}-1)\Big{\\}}$
(35)

where $k_{i}$ is the proportionality constant for the $i$th dimension,
$\beta_{i}={0.5{\hat{r}_{i}^{2}}+\hat{b}}$ and $\alpha=a+0.5$.

Proceeding ahead we can write

$\displaystyle q(\bm{\mathcal{I}})$
$\displaystyle=\prod_{i=1}^{m}\overbrace{(1-{\Omega_{i}})\underbrace{f(\alpha,\beta_{i}){{{\mathcal{I}}}_{i}}^{\alpha-1}e^{-\beta_{i}\mathcal{I}_{i}}}_{q^{1}({\mathcal{I}}_{i})}+{{\Omega_{i}}\delta({{{\mathcal{I}}}_{i}}-1)}}^{q(\mathcal{I}_{i})}$

where

${\Omega_{i}}=k_{i}e^{-0.5{\hat{r}_{i}^{2}}}\theta_{i}$ (36)

To determine $k_{i}$, we note that the distribution in (35) should integrate
to $1$. Therefore, the following should hold

$k_{i}{\theta_{i}}e^{-0.5{\hat{r}_{i}^{2}}}+k_{i}(1-{\theta_{i}})\frac{f(a,\hat{b})}{f(\alpha,{\beta_{i}})}=1$
(37)

Leading to

$k_{i}=\frac{1}{{\theta_{i}}e^{-0.5{\hat{r}_{i}^{2}}}+(1-{\theta_{i}})\frac{f(a,\hat{b})}{f(\alpha,{\beta_{i}})}}$
(38)

Resultingly, using (10) and (36), (38) we arrive at

$\Omega_{i}=\frac{1}{1+\zeta\frac{{\hat{b}}^{a}}{{\beta_{i}^{\alpha}}}e^{0.5{\hat{r}_{i}^{2}}}}\
\forall\ i>0$ (39)

where $\zeta=(\frac{1}{\theta_{i}}-1)\frac{\Gamma(\alpha)}{\Gamma(a)}$.

Lastly, in a similar manner using the VB/EM approach we can determine
$q(b)=\delta(b-\hat{b})$ where using (6) and (29)

$\hat{b}=\underset{b}{\operatorname{argmax}}\big{\langle}\mathrm{ln}(p(\hat{\mathbf{x}},\bm{\mathcal{I}},b|\mathbf{y}))\big{\rangle}_{q(\bm{\mathcal{I}})}$
(40)

The expected log-posterior in (40) can be written as

$\displaystyle\big{\langle}\mathrm{ln}(p(\hat{\mathbf{x}},\bm{\mathcal{I}},b|\mathbf{y}))\big{\rangle}_{q(\bm{\mathcal{I}})}=$
$\displaystyle\Big{\\{}{\sum_{i=1}^{m}v_{i}(b)}+{(A-1)}\ln(b){-Bb}$
$\displaystyle+constant\Big{\\}}$ (41)

where

$\displaystyle v_{i}(b)$
$\displaystyle={\langle\ln((1-{\theta_{i}})f(a,b){{{\mathcal{I}}}_{i}}^{a-1}e^{-b\mathcal{I}_{i}}+{\theta_{i}}\delta({{{\mathcal{I}}}_{i}}-1))\rangle_{q({\mathcal{I}}_{i})}}$
(42)

which can further be written as follows considering only the terms dependent
on $b$

$\displaystyle
v_{i}(b)=(1-\Omega_{i})(\ln(f(a,b))-b\langle{{{\mathcal{I}}}_{i}}\rangle_{q^{1}({\mathcal{I}}_{i})})+constant$
(43)

where we assume $q^{1}(\mathcal{I}_{i})$ is defined as zero for
${{{\mathcal{I}}}_{i}}=1$ as the prior Gamma density in (26). Given
$\langle{{{\mathcal{I}}}_{i}}\rangle_{q^{1}({\mathcal{I}}_{i})}={\alpha}/{\beta_{i}}$
and using the expressions in (10) and (43), we can write (41) as

$\displaystyle\big{\langle}\mathrm{ln}(p(\hat{\mathbf{x}},\bm{\mathcal{I}},b|\mathbf{y}))\big{\rangle}_{q(\bm{\mathcal{I}})}=$
$\displaystyle{(\bar{A}-1)}\ln(b)-\bar{B}b+constant$ (44)

where

$\displaystyle\bar{A}$ $\displaystyle=A+\sum_{i=1}^{m}a(1-\Omega_{i})$ (45)
$\displaystyle\bar{B}$
$\displaystyle=B+\sum_{i=1}^{m}(1-\Omega_{i})\frac{\alpha}{\beta_{i}}$ (46)

Maximizing (44) using differentiation, we obtain $\hat{b}$ according to (40)
as

$\hat{b}=\frac{\bar{A}-1}{\bar{B}}\ \ \ \mathrm{s.t.}\ \ \ \bar{A}>1$ (47)

where $\bar{A}>1$ owing to the requirement of positivity of parameter $b$ of
Gamma distribution in (26) being approximated in (47). Also noting that the
parameter $a>0$ for validity of the Gamma distribution in (26), $A>1$ is a
sufficient condition for (47) to hold considering (45). Lastly, note that
since the rate parameter of distribution in (27) is positive i.e. $B>0$ any
numerical errors in (47) are avoided. The resulting method namely ASOR is
given as Algorithm 3.

Initialize $w_{i}=1\ \forall\ i$ and $A,B,a,\hat{b}$, $\theta_{i}\ \forall\ i$

Evaluate $\alpha=a+0.5$ and
$\zeta=(\frac{1}{\theta_{i}}-1)\frac{\Gamma(\alpha)}{\Gamma(a)}$

while  _the convergence criterion has not met_ do

Variable update:
$\hat{\mathbf{x}}=\underset{\mathbf{x}\in\mathcal{X}}{\operatorname{argmin}}\sum_{i}w_{i}{(r_{i}\left(\mathbf{y}_{i},\mathbf{x}\right))}^{2}$

Residual update:
${\hat{r}_{i}^{2}}={\big{(}r_{i}\left(\mathbf{y}_{i},\hat{\mathbf{x}}\right)\big{)}}^{2}$
$\forall\ i$

Parameteric updates:

$\beta_{i}={0.5{\hat{r}_{i}^{2}}+\hat{b}}$

$\Omega_{i}=\frac{1}{1+\zeta\frac{{\hat{b}}^{a}}{{\beta_{i}^{\alpha}}}e^{0.5{\hat{r}_{i}^{2}}}}$

$\hat{b}=\mfrac{A-1+\sum_{i}a(1-\Omega_{i})}{B+\sum_{i}(1-\Omega_{i})\frac{\alpha}{\beta_{i}}}$

Weight update: $w_{i}=\Omega_{i}+(1-\Omega_{i})\alpha/\beta_{i}$ $\forall\
i>0$

end while

Algorithm 3 ASOR

### Remarks

It is interesting to note that the variable update steps in EROR, ESOR and
ASOR is the same as in GNC reflecting their ability to employ non-minimal
solvers during inference. We propose using the change in
${\sum_{i}}w_{i}{\hat{r}_{i}^{2}}$ during consecutive iterations as the
standard convergence criterion. We lastly remark that in EROR and ESOR an
exception to break the iterations can be added for seamless operation when the
sum of the weights gets close to zero which we experienced in certain
applications with very high ratios of outliers.

##  IV Experiments

In this section, we discuss the performance results of the proposed methods
for different spatial perception applications including 3D point cloud
registration (code: MATLAB), mesh registration (code: MATLAB), and pose graph
optimization (PGO) (code: C++) on an Intel i7-8550U processor-based computer
and consider SI units. We consider GNC-GM and GNC-TLS as the benchmark methods
which have shown good results in these applications outperforming methods
including the classical RANSAC and recently introduced ADAPT. For EROR and
ESOR we consider $\chi=\gamma=\bar{c}^{2}$ where $\bar{c}^{2}$, dictating the
maximum error expected for the inliers, is set as specified in the original
work [11]. In ASOR we resort to the choice of initialization which performs
the best across different applications given as
$a=0.5,A=10000,B=1000,\hat{b}=10000,\theta_{i}=0.5\ \forall\ i$. We use the
normalized incremental change of $10^{-5}$ in
$\underset{i}{\sum}w^{i}{\hat{r}_{i}^{2}}$ during consecutive iterations as
the convergence criterion for GNC-TLS, EROR, ESOR and ASOR. For GNC-GM the
convergence criterion remains the same as originally reported.

###  IV-A 3D Point Cloud Registration

Figure 3: Point clouds with correspondences in 3D point cloud registration for
the Bunny dataset [31].

In 3D point cloud registration, we assume that a set of 3D points
$\mathbf{p}_{i}\in\mathbb{R}^{3},i=1,...,m$ undergo a transformation, with
rotation $\mathbf{R}\in\mathrm{SO(3)}$ and translation
$\mathbf{t}\in\mathbb{R}^{3}$, resulting in another set of 3D points
$\mathbf{q}_{i}\in\mathbb{R}^{3},i=1,...,m$. The putative correspondences
$(\mathbf{p}_{i},\mathbf{q}_{i})$ can be potentially infested with outliers.
Fig. 3 depicts how the Bunny point cloud from the Stanford repository [31]
undergoes a random transformation in a point cloud registration setup (blue
lines: inliers, red lines: outliers). The objective is to estimate
$\mathbf{R}$ and $\mathbf{t}$ that best aligns the two point clouds by
minimizing the effect of outliers. The problem can be cast in form of (2)
where the ith residual is the Euclidean distance between $\mathbf{q}_{i}$ and
$\mathbf{R}\mathbf{p}_{i}+\mathbf{t}$. We resort to the renowned Horn’s method
as the non-minimal solver for this case which provides closed form estimates
in the outlier-free case [8].

(a) Rotation error.

(b) Translation error.

(c) Computational time.

Figure 4: Performance of robust estimators for 3D point cloud registration
considering the Bunny dataset [31].

(a) Rotation error.

(b) Translation error.

(c) Computational time.

Figure 5: Performance of robust estimators for mesh registration considering
the motorbike model [32].

(a) RMSE (INTEL).

(b) RMSE (CSAIL).

(c) Computational time (INTEL).

Figure 6: Performance of robust estimators for pose graph optimization
considering Intel and CSAIL datasets.

Using the proposed EROR, ESOR and ASOR we robustify the Horn’s method and
report the results for the Bunny dataset. We downsample the point cloud to
$m=100$ points and restrict it within a $[-0.5,0.5]^{3}$ box before applying a
random rotation $\mathbf{R}\in\mathrm{SO(3)}$ and a random translation
$\mathbf{t}\ (\|\mathbf{t}\|_{2}\leq 3)$. The inliers of the transformed
points are corrupted with independent noise samples drawn from
$\mathcal{N}(0,0.001^{2})$ whereas the outliers are randomly generated being
contained within a sphere of diameter $\sqrt{3}$. 20 Monte Carlo (MC) runs are
used to capture the error statistics for up to $90\%$ outlier contamination
ratio.

Figs. 4 showcase the performance results of our proposed methods in comparison
to the GNC methods. The rotation and translational error statistics as
depicted in Figs. 4(a)-4(b) are very similar in all the cases with errors
generally increasing for all the methods at large outlier ratios. In terms of
computational complexity, the gains of the Bayesian heuristics can be observed
for this case in Fig. 4(c) which is a key evaluation parameter for runtime
performance. ESOR exhibits much faster comparative behavior followed by EROR
and ASOR for the entire range of outlier ratios. We have observed similar
error performance of the methods for other values of $m$ (upto maximum number
of available points). Also we have noticed that with an increase in the inlier
noise magnitude and number of points, ASOR slows down becoming computationally
comparative to the GNC methods. Moreover, we have noted that increasing the
parameter $a$ generally reduces the processing time at the cost of slightly
increased errors at higher outlier ratios.

Other specialized solvers like TEASER [33] also exist for point cloud
registration problems which are certifiably robust and can sustain higher
outlier contamination better. However, it has scalability issues thanks to the
involvement of sluggish semidefinite programming (SDP) based solution for
rotation estimation. An improved version of TEASER namely TEASER++ [18] was
recently introduced that leverages the heuristic, GNC-TLS, in its inferential
pipeline for rotation estimation while still certifying the estimates. Owing
to the well-devised estimation pipeline, GNC-TLS has to deal with a smaller
percentage of outliers making TEASER++ faster and improving its practicality.
Since the error performance of the Bayesian heuristics is similar to the GNC
methods and these are generally found to be faster in various scenarios of the
point cloud registration problem this indicates their utility as standalone
estimators. Moreover, these can potentially be useful in other inferential
pipelines like TEASER++ (while enjoying certifiable performance) but need a
thorough evaluation.

###  IV-B Mesh registration

In the mesh registration problem, the points $\mathbf{p}_{i}$ from the a point
cloud are transformed to general 3D primitives $\mathbf{q}_{i}$ including
points, lines, and/or planes. Fig. 7 shows the result of a random
transformation of a point cloud to the motorbike mesh model from the PASCAL
dataset [32] (blue lines: inliers, red lines: outliers). The aim is to
estimate the $\mathbf{R}$ and $\mathbf{t}$ by minimizing the squared sum of
residuals which represent the corresponding distances. We resort to [9] as the
basic non-minimal solver which has been proposed to find the globally optimal
solution for the outlier-free case.

Figure 7: Mesh and point cloud with correspondences in mesh registration for
the motorbike mesh model [32]

Using the presented EROR, ESOR and ASOR heuristics we robustify the solver and
report the results for the motorbike mesh model. To create point cloud we
randomly sample points on the vertices, edges and faces of the mesh model, and
then apply a random transformation $(\mathbf{R}\in\mathrm{SO(3)}$,
$\mathbf{t}\ (\|\mathbf{t}{\|}_{2}\leq 5))$ and subsequently add independent
noise samples from $\mathcal{N}(0,0.01^{2})$. Considering $100$ point-to-
point, $80$ point-to-line and $80$ point-to-plane correspondences, we create
outliers by random erroneous correspondences. For this case also, 20 MC runs
are carried out to generate the error statistics for up to $90\%$ outlier
contamination ratio. We see a trend similar to the point cloud registration
case as depicted in Figs. 5(a)-5(b). In particular, the performance in terms
of errors is similar for all the algorithms. However, the Bayesian heuristics
exhibit a general computational advantage, except at very high outlier ratios
where the performance becomes comparable. The Bayesian heuristics generally
have similar runtimes with SOR modifications having a general advantage. We
observed similar performance of the methods for other combinations of
correspondences in this case.

During the experiments of point cloud and mesh registration, we have noticed
that ROR and SOR modifications generally get computationally more
advantageous, with estimation quality remaining similar, when the outliers
become larger in comparison to the nominal noise samples.

###  IV-C Pose graph optimization

(a)

(b)

Figure 8: Ground truth paths of datasets considered in pose graph
optimization.

PGO is typically employed for several problems arising in robotic and computer
vision applications like SLAM and structure from motion (SfM) [34]. The
objective is to estimate a set of poses
$(\mathbf{t}_{i},\mathbf{R}_{i}),i=1,...,m$ using pairwise relative
measurements $(\tilde{\mathbf{t}}_{ij},\tilde{\mathbf{R}}_{ij})$. Relative
observations can result in consecutive pose constraints (e.g. from odometry
measurements) or non-successive pose constraints (e.g. from scan matalphang)
also known as loop closures. The residual error for this case is given as

$r\left(\mathbf{R}_{i},t_{i}\right)=\sqrt{\kappa_{ij}\|\mathbf{R}_{j}-\mathbf{R}_{i}\tilde{\mathbf{R}}_{ij}\|_{F}^{2}+\tau_{ij}\|t_{j}-t_{i}-\mathbf{R}_{i}\tilde{t}_{ij}\|_{2}^{2}}$

where $\kappa_{ij}$ and $\tau_{ij}$ encode the measurement noise statistics
and $\|.\|_{F}$ denotes the Frobenius norm. We resort to SE-Sync [10] as the
non-minimal solver for this case and use the Python binding of C++ open-
sourced by the authors. Adopting the same experimentation setup of GNC we
randomly corrupt loop closures and retain odometry observations. For
benchmarking we consider 2D datasets including INTEL and CSAIL which are
available openly (path ground truth plotted in Fig. 8). Since simulations take
much more time as compared to the previous case we carry out 10 MC runs to
obtain the error statistics. Fig. 6(a) showcases the root mean squared (RMSE)
of the trajectory considering the INTEL dataset. The proposed Bayesian
heuristics in this case also exhibit comparative performance to the GNC
methods. At very higher outlier rates ESOR has slightly compromised
performance. In Fig. 6(b) the RMSE for the CSAIL dataset are depicted
reflecting the same pattern. In the PGO examples, GNC-TLS and ASOR outperform
other methods with ASOR generally having the least error for different outlier
ratios. As far as the computational performance is concerned ESOR has the
smallest runtime, followed by ASOR while ROR is the slowest for both cases.
Fig. 6(c) depicts the computational runtime statistics for the INTEL dataset.
CSAIL has similar computational time statistics with ESOR being relatively
much faster as compared to other methods.

Lastly, we also evaluated the Bayesian heuristics using
$\max_{i}({w^{i}{\hat{r}_{i}^{2}}})<\bar{c}^{2}$ as the stopping criteria
which resulted in much faster performance but with slightly degraded error
performance in the considered scenarios of the spatial perception
applications.

##  V Conclusion

We have proposed three Bayesian heuristics: EROR, ESOR and ASOR as nonlinear
estimators for spatial perception problems. Like the existing general-purpose
GNC heuristics, these have the ability to invoke existing non-minimal solvers.
Evaluations in several experiments demonstrate their merits as compared to the
GNC heuristics. In particular, in the 3D point cloud and mesh registration
problems EROR, ESOR and ASOR have similar estimation errors over a wide range
of outlier ratios. However, the Bayesian heuristics have a general advantage
in computational terms. For the PGO setups, we generally find the Bayesian
methods to compete with GNC in estimation quality. The devised ROR and SOR
modifications are found to be the least and most computationally efficient for
this case. Using another suggested criteria can lead to further improvement in
computational terms at expense of estimation quality. In short, the proposed
methods provide general purpose options, in addition to the GNC heuristics, to
robustify the existing non-minimal solvers against outliers in different
spatial perception applications indicating their usefulness. Empirical
evidence suggests that the proposed approaches provide a general edge in
computational terms while remaining competitive in terms of error. The actual
possible gains depend on whether the solvers are used standalone or in an
inferential pipeline for the particular application scenarios and should be
evaluated for the case under consideration before deployment. We believe that
the work can be further extended in different directions by aiming to devise
the heuristics without knowledge of nominal noise statistics. Moreover, these
methods can also be tested within hybrid approaches where the estimates
subsequently get certified for optimality.

## References

  * [1] B. Drost, M. Ulrich, N. Navab, and S. Ilic, “Model globally, match locally: Efficient and robust 3D object recognition,” in _2010 IEEE Computer Society Conference on Computer Vision and Pattern Recognition_ , 2010, pp. 998–1005.
  * [2] D. Scaramuzza and F. Fraundorfer, “Visual odometry [tutorial],” _IEEE Robotics and Automation Magazine_ , vol. 18, no. 4, pp. 80–92, 2011.
  * [3] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” _IEEE Transactions on Robotics_ , vol. 32, no. 6, pp. 1309–1332, 2016.
  * [4] Y.-Q. Liu, F. Jin, K.-F. Dong, J.-L. Song, W.-Q. Mo, and Y.-J. Hui, “Eccentric optimization of multisensor for SLAM-integrated navigation,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 72, pp. 1–8, 2023.
  * [5] A. H. Chughtai, M. Tahir, and M. Uppal, “A robust Bayesian approach for online filtering in the presence of contaminated observations,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 70, pp. 1–15, 2021.
  * [6] P. Antonante, V. Tzoumas, H. Yang, and L. Carlone, “Outlier-robust estimation: Hardness, minimally tuned algorithms, and applications,” _IEEE Transactions on Robotics_ , vol. 38, no. 1, pp. 281–301, 2022.
  * [7] J. Wang, Z. Meng, and L. Wang, “Efficient probabilistic approach to range-only SLAM with a novel likelihood model,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 70, pp. 1–12, 2021.
  * [8] B. K. Horn, “Closed-form solution of absolute orientation using unit quaternions,” _Josa a_ , vol. 4, no. 4, pp. 629–642, 1987.
  * [9] J. Briales and J. Gonzalez-Jimenez, “Convex global 3D registration with Lagrangian duality,” in _2017 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2017, pp. 5612–5621.
  * [10] D. M. Rosen, L. Carlone, A. S. Bandeira, and J. J. Leonard, “Se-sync: A certifiably correct algorithm for synchronization over the special euclidean group,” _The International Journal of Robotics Research_ , vol. 38, no. 2-3, pp. 95–125, 2019.
  * [11] H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non-convexity for robust spatial perception: From non-minimal solvers to global outlier rejection,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 1127–1134, 2020.
  * [12] M. A. Fischler and R. C. Bolles, “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography,” _Communications of the ACM_ , vol. 24, no. 6, pp. 381–395, 1981.
  * [13] V. Tzoumas, P. Antonante, and L. Carlone, “Outlier-robust spatial perception: Hardness, general-purpose algorithms, and guarantees,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2019, pp. 5383–5390.
  * [14] J. L. Schonberger and J.-M. Frahm, “Structure-from-motion revisited,” in _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_ , June 2016.
  * [15] N. Chebrolu, T. Läbe, O. Vysotska, J. Behley, and C. Stachniss, “Adaptive robust kernels for non-linear least squares problems,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 2240–2247, 2021.
  * [16] P.-Y. Lajoie, S. Hu, G. Beltrame, and L. Carlone, “Modeling perceptual aliasing in SLAM via discrete–continuous graphical models,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 1232–1239, 2019.
  * [17] L. Carlone and G. C. Calafiore, “Convex relaxations for pose graph optimization with outliers,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 2, pp. 1160–1167, 2018.
  * [18] H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and certifiable point cloud registration,” _IEEE Transactions on Robotics_ , vol. 37, no. 2, pp. 314–333, 2021.
  * [19] E. M. Ronchetti and P. J. Huber, _Robust statistics_. John Wiley & Sons, 2009.
  * [20] G. Agamennoni, P. Furgale, and R. Siegwart, “Self-tuning m-estimators,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , 2015, pp. 4628–4635.
  * [21] A. Nakabayashi and G. Ueno, “Nonlinear filtering method using a switching error model for outlier-contaminated observations,” _IEEE Transactions on Automatic Control_ , vol. 65, no. 7, pp. 3150–3156, 2020.
  * [22] H. Wang, H. Li, J. Fang, and H. Wang, “Robust Gaussian Kalman filter with outlier detection,” _IEEE Signal Processing Letters_ , vol. 25, no. 8, pp. 1236–1240, 2018.
  * [23] A. H. Chughtai, M. Tahir, and M. Uppal, “Outlier-robust filtering for nonlinear systems with selective observations rejection,” _IEEE Sensors Journal_ , vol. 22, no. 7, pp. 6887–6897, 2022.
  * [24] R. Piché, S. Särkkä, and J. Hartikainen, “Recursive outlier-robust filtering and smoothing for nonlinear systems using the multivariate Student-t distribution,” in _2012 IEEE International Workshop on Machine Learning for Signal Processing_ , 2012, pp. 1–6.
  * [25] M. A. Amaral Turkman, C. D. Paulino, and P. Müller, _Computational Bayesian Statistics: An Introduction_ , ser. Institute of Mathematical Statistics Textbooks. Cambridge University Press, 2019.
  * [26] S. Särkkä and L. Svensson, _Bayesian filtering and smoothing_. Cambridge university press, 2023, vol. 17.
  * [27] V. Šmídl and A. Quinn, _The variational Bayes method in signal processing_. Springer Science & Business Media, 2006.
  * [28] K. P. Murphy, _Machine learning : a probabilistic perspective_. Cambridge, Mass. [u.a.]: MIT Press, 2013.
  * [29] ——, “Conjugate Bayesian analysis of the Gaussian distribution,” _def_ , vol. 1, no. 2$\sigma$2, p. 16, 2007.
  * [30] D. Fink, “A compendium of conjugate priors,” _See http://www. people. cornell. edu/pages/df36/CONJINTRnew% 20TEX. pdf_ , vol. 46, 1997.
  * [31] B. Curless and M. Levoy, “A volumetric method for building complex models from range images,” in _Proceedings of the 23rd annual conference on Computer graphics and interactive techniques_ , 1996, pp. 303–312.
  * [32] Y. Xiang, R. Mottaghi, and S. Savarese, “Beyond PASCAL: A benchmark for 3D object detection in the wild,” in _IEEE Winter Conference on Applications of Computer Vision_ , 2014, pp. 75–82.
  * [33] H. Yang and L. Carlone, “A polynomial-time solution for robust registration with extreme outlier rates,” _Robotics: Science and Systems_ , 2019.
  * [34] F. Bai, T. Vidal-Calleja, and G. Grisetti, “Sparse pose graph optimization in cycle space,” _IEEE Transactions on Robotics_ , vol. 37, no. 5, pp. 1381–1400, 2021.