Graduated Non-Convexity for Robust Spatial Perception:

From Non-Minimal Solvers to Global Outlier Rejection

The MIT Faculty has made this article openly available. Please share
how this access benefits you. Your story matters.

As Published 10.1109/LRA.2020.2965893

Publisher Institute of Electrical and Electronics Engineers (IEEE)

Version Author's final manuscript

Citable link https://hdl.handle.net/1721.1/136232

Terms of Use Creative Commons Attribution-Noncommercial-Share Alike

Detailed Terms http://creativecommons.org/licenses/by-nc-sa/4.0/

 This paper has been accepted for publication in the IEEE Robotics and Automation Letters.
Please cite the paper as: H. Yang, P. Antonante, V. Tzoumas, and L. Carlone,
“Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection”, 1

IEEE Robotics and Automation Letters (RA-L), 2020.

Graduated Non-Convexity for Robust Spatial Perception:

From Non-Minimal Solvers to Global Outlier Rejection
Heng Yang, Pasquale Antonante, Vasileios Tzoumas, Luca Carlone

Abstract—Semidefinite Programming (SDP) and Sums-of-Squ- measurement, quantifying the mismatch between the expected
ares (SOS) relaxations have led to certifiably optimal non-minimal measurement at an estimate x and the actual measurement yi .
solvers for several robotics and computer vision problems. How- In typical robotics and computer vision applications, the least
ever, most non-minimal solvers rely on least squares formulations,
and, as a result, are brittle against outliers. While a standard squares optimization (1) is difficult to solve globally, due to the
approach to regain robustness against outliers is to use robust nonlinearity of the residual errors and the nonconvexity of the
cost functions, the latter typically introduce other non-convexities, domain X . Despite these challenges, the research community
preventing the use of existing non-minimal solvers. In this paper,
arXiv:1909.08605v4 [cs.CV] 11 Jun 2020

has developed closed-form solutions and globally optimal

we enable the simultaneous use of non-minimal solvers and robust solvers for many such problems. Specifically, while closed-
estimation by providing a general-purpose approach for robust
global estimation, which can be applied to any problem where a form solutions are rare [3], Semidefinite Programming (SDP)
non-minimal solver is available for the outlier-free case. To this and Sums-of-Squares (SOS) [4] relaxations have been recently
end, we leverage the Black-Rangarajan duality between robust shown to be a powerful tool to obtain certifiably optimal solu-
estimation and outlier processes (which has been traditionally tions to relevant instances of problem (1), ranging from pose
applied to early vision problems), and show that graduated non- graph optimization [5], [6], rotation averaging [7], anisotropic
convexity (GNC) can be used in conjunction with non-minimal
solvers to compute robust solutions, without requiring an initial registration [8], two-view geometry [9], and PnP [10]. The
guess. Although GNC’s global optimality cannot be guaranteed, resulting techniques are commonly referred to as non-minimal
we demonstrate the empirical robustness of the resulting robust solvers, to contrast them against methods that solve prob-
non-minimal solvers in applications, including point cloud and lem (1) using only a small (minimal) subset of measurements
mesh registration, pose graph optimization, and image-based yi (see related work in Section II).
object pose estimation (also called shape alignment). Our solvers
are robust to 70-80% of outliers, outperform RANSAC, are more Unfortunately, in the presence of outliers, problem (1)’s
accurate than specialized local solvers, and faster than specialized solution provides a poor estimate for x. This limits the
global solvers. We also propose the first certifiably optimal non- applicability of existing non-minimal solvers, since they can
minimal solver for shape alignment using SOS relaxation. be applied only after the outliers have been removed. Yet, the
Index Terms—Graduated non-convexity, outlier rejection, ro- theory of robust estimation suggests regaining robustness by
bust estimation, spatial perception, global optimization. substituting the quadratic cost in the least squares problem (1)
with a robust cost ρ(·):
I. I NTRODUCTION N
X
min ρ(r(yi , x)). (2)
OBUST estimation is a crucial tool for robotics and
R computer vision, being concerned with the estimation
of unknown quantities (e.g., the state of a robot, or of
x∈X
i=1

For instance, ρ(·) can be a Huber loss, a truncated least

variables describing the external world) from noisy and po- squares cost, or a Geman-McClure cost [11]. To date, the
tentially corrupted measurements. Corrupted measurements application of non-minimal solvers to eq. (2) has been limited.
(i.e., outliers) can be caused by sensor malfunction, but are Some of the non-minimal solvers designed for (1) can be
more commonly associated with incorrect data association extended to include a convex robust cost function, such as
and model misspecification [1], [2]. the Huber loss [12]; however, it is known that convex losses
In the outlier-free case, common estimation problems are have a low breakdown point and are still sensitive to gross
formulated as a least squares optimization: outliers [13]. In rare cases, the literature provides robust
non-minimal solvers that achieve global solutions to specific
N
X instances of problem (2), such as robust registration [2], [14].
min r2 (yi , x), (1) However, these solvers cannot be easily extended to other
x∈X
i=1
estimation problems, and they rely on solving large SDPs,
where x is the variable we want to estimate (e.g., the pose which is currently impractical for large-scale problems.
of an unknown object); X is the domain of x (e.g., the set Contributions. In this paper, we aim to reconcile non-
of 3D poses); yi (i = 1, . . . , N ) are given measurements minimal solvers and robust estimation, by providing a general-
(e.g., pixel observations of points belonging to the object); purpose1 algorithm to solve problem (2) without requiring
and the function r(yi , x) is the residual error for the i-th an initial guess. We achieve this by combining non-minimal
solvers with an approach known as graduated non-convexity
This work was partially funded by ARL DCIST CRA W911NF-17-2- (GNC). In contrast, standard algorithms for problem (2) rely
0181, ONR RAIDER N00014-18-1-2828, and Lincoln Laboratory’s Resilient
Perception in Degraded Environments program. on iterative optimization to refine a given initial guess, which
The authors are with the Laboratory for Information & Decision Sys-
tems (LIDS), Massachusetts Institute of Technology, Cambridge, USA, 1 For a given problem (2), we only assume the existence of a non-minimal
{hankyang,antonap,vtzoumas,lcarlone}@mit.edu solver for the corresponding outlier-free problem (1).
2

causes the result to be brittle when the quality of the guess is guarantees. Briales and Gonzalez-Jimenez [8] solve G-REG
poor. Particularly, we propose three contributions. using SDP. Carlone et al. [5], [23] use Lagrangian duality and
First, we revisit the Black-Rangarajan duality [11] between SDP relaxations for PGO. Rosen et al. [6] develop SE-Sync, a
robust estimation and outlier processes. We also revisit the use fast solver for the relaxation in [5], [23]. Mangelson et al. [24]
of graduated non-convexity (GNC) as a general tool to solve apply the sparse bounded-degree variant of SOS relaxation,
a non-convex optimization problem without an initial guess. also for PGO. Currently, there are no certifiably optimal non-
While Black-Rangarajan duality and GNC have been used in minimal solvers for shape alignment; Zhou et al. [15] propose
early vision problems, such as stereo reconstruction, image a convex relaxation to obtain an approximate solution for SA.
restoration and segmentation, and optical flow, we show that
combining them with non-minimal solvers allows solving spa-
B. Robust Estimation
tial perception problems, ranging from mesh registration, pose
graph optimization, and image-based object pose estimation. Global Methods. We refer to a robust method as global, if it
Our second contribution is to tailor Black-Rangarajan du- does not require an initial guess. Several global methods adopt
ality and GNC to the Geman-McClure and truncated least the framework of consensus maximization [25], [26], which
squares costs. We show how to optimize these functions by al- looks for an estimate that maximizes the number of measure-
ternating two steps: a variable update, which solves a weighted ments that are explained within a prescribed estimation error.
least squares problem using non-minimal solvers; and a weight Consensus maximization is NP-hard [25], [27], and related
update, which updates the outlier process in closed form. work investigates both approximations and exact algorithms.
Our approach requires a non-minimal solver, but currently RANSAC [28] is a widely used heuristic, applicable when a
there is no such solver for image-based object pose estimation minimal solver exists. But RANSAC provides no optimality
(also known as shape alignment). Our third contribution is to guarantees, and its running time grows exponentially with the
present a novel non-minimal solver for shape alignment. While outlier ratio [29]. Tzoumas et al. [27] develop the general-
related techniques propose approximate relaxations [15], we purpose Adaptive Trimming (ADAPT) algorithm, that has linear
provide a certifiably optimal solution using SOS relaxation. running time, and, instead, is applicable when a non-minimal
We demonstrate our robust non-minimal solvers on point solver exists. Mangelson et al. [30] propose a graph-theoretic
cloud registration (P-REG), mesh registration (as known as gen- method to prune outliers in PGO. Exact solutions for consensus
eralized registration, G-REG), pose graph optimization (PGO), maximization are based on BnB [26]: see [31] for the Wahba
and shape alignment (SA). Our solvers are robust to 70- problem, and [29] for P-REG.
80% outliers, outperform RANSAC, are more accurate than Another framework for global methods is M-estimation,
specialized local solvers, and faster than global solvers. which resorts to robust cost functions. Enqvist et al. [32] use a
truncated least squares (TLS) cost, and propose an approach
II. R ELATED W ORK that scales, however, exponentially with the dimension of the
parameter space. More recently, SDP relaxations have also
A. Outlier-free Estimation been used to optimize robust costs. Carlone and Calafiore [12]
Minimal Solvers. Minimal solvers use the smallest number develop convex relaxations for PGO with `1 -norm and Huber
of measurements necessary to estimate x. Examples include loss functions. Lajoie et al. [13] adopt a TLS cost for PGO.
the 2-point solver for the Wahba problem (a rotation-only Yang and Carlone [2], [14] develop an SDP relaxation for
variant of P-REG) [16], the 3-point solver for P-REG [3], the Wahba and P-REG problem, also adopting a TLS cost.
and the 12-point solver [17] for G-REG with point-to-plane Currently, the poor scalability of the state-of-the-art SDP
correspondences. Notably, the approach [18] enables minimal solvers limits these algorithms to only small-size problems.
solvers for a growing number of estimation problems. Finally, graduated non-convexity (GNC) methods have also
Non-minimal Solvers. Minimal solvers do not leverage been employed to optimize robust costs [33]. Rather than
data redundancy, and, consequently, can be sensitive to mea- directly optimizing a non-convex robust cost, these methods
surement noise. Therefore, non-minimal solvers have been sequentially optimize a sequence of surrogate functions, which
developed. Typically, non-minimal solvers assume Gaussian start from a convex approximation of the original cost, but then
measurement noise, which results in a least squares opti- gradually become non-convex, converging eventually to the
mization framework. In some cases, the resulting optimization original cost. Despite GNC’s success in early computer vision
problems can be solved in closed form, e.g., Horn’s method [3] applications [11], [34], its broad applicability has remain
for P-REG, or PLICP [19] for G-REG. Generally, however, the limited due to the lack of non-minimal solvers. Indeed, only
resulting optimization problems are hard and only locally op- a few specialized methods for spatial perception have used
timal solutions can be obtained [20]. For global optimization, GNC. Particularly, Zhou et al. [35] develop a method for P-
researchers have developed exponential-time methods, such as REG, using Horn’s or Arun’s methods [3], [36].
Branch and Bound (BnB). Hartley and Kahl [21] introduce a Local Methods. In contrast to global methods, local meth-
BnB search over the rotation space to globally solve several ods require an initial guess. In the context of M-estimation,
vision problems. Olsson et al. [22] develop optimal solutions these methods iteratively optimize a robust cost function till
for G-REG. Recently, Semidefinite Programing (SDP) and they converge to a local minimum [37]. Zach et al. [38]
Sums of Squares (SOS) relaxations [4] have been used to include auxiliary variables and propose an iterative optimiza-
develop polynomial-time algorithms with certifiable optimality tion approach that alternates updates on the estimates and the
YANG et al.: GRADUATED NON-CONVEXITY FOR ROBUST SPATIAL PERCEPTION 3

auxiliary variables; the approach still requires an initial guess. (a) (b)
Bouaziz et al. [39] propose robust variants of the iterative clos-
est point algorithm for P-REG. Sünderhauf and Protzel [40],
Olson and Agarwal [41], Agarwal et al. [42], Pfingsthorn and
Birk [43] propose local methods for PGO. Wang et al. [44]
investigate local methods for shape alignment.

III. B LACK -R ANGARAJAN D UALITY AND G RADUATED

N ON - CONVEXITY
Fig. 1. Graduated Non-Convexity (GNC) with control parameter µ for (a)
We review the Black-Rangarajan duality [11], and a tool for Geman McClure (GM) and (b) Truncated Least Squares (TLS) costs.
global optimization known as graduated non-convexity [33].
gradually changing µ (i.e., gradually increasing the amount
of non-convexity) till the original non-convex function is
A. Black-Rangarajan Duality recovered. The solution obtained at each iteration is used as
This section revisits the Black-Rangarajan duality between the initial guess for the subsequent iteration.
robust estimation and outlier process [11]. This theory is less Let us shed some light on GNC with two examples.
known in robotics, and its applications have been mostly
Example 1 (Geman McClure (GM) and GNC). The Geman-
targeting early vision problems, with few notable exceptions.
McClure function is a popular (non-convex) robust cost. The
Lemma 1 (Black-Rangarajan Duality √ [11]). Given a robust following equation shows the GM function (left) and the
.
cost function ρ(·), define φ(z) = ρ( z). If φ(z) satisfies surrogate function including a control parameter µ (right):
limz→0 φ0 (z) = 1, limz→∞ φ0 (z) = 0, and φ00 (z) < 0, then
. c̄2 r2 µc̄2 r2
the robust estimation problem (2) is equivalent to ρ(r) = =⇒ ρµ (r) = , (4)
c̄2 + r2 µc̄2 + r2
N
where c̄ is a given parameter that determines the shape of the
X
wi r2 (yi , x) + Φρ (wi ) ,
 
min (3)
x∈X ,wi ∈[0,1]
i=1 Geman McClure function ρ(r).
The surrogate function ρµ (r) (shown in Fig. 1(a)) is such
where wi ∈ [0, 1] (i = 1, . . . , N ) are slack variables (or
that: (i) ρµ (r) becomes convex for large µ (in the limit of µ →
weights) associated to each measurement yi , and the function
∞, ρµ (r) becomes quadratic), and (ii) ρµ (r) recovers ρ(r)
Φρ (wi ) (the so called outlier process) defines a penalty on the
when µ = 1. GNC minimizes the function ρ(r) by repeatedly
weight wi . The expression of Φρ (wi ) depends on the choice
minimizing the function ρµ (r) for decreasing values of µ.
of robust cost function ρ(·).
Example 2 (Truncated Least Squares (TLS) and GNC). The
The conditions on ρ(·) are satisfied by all common choices
truncated least squares function is defined as:
of robust costs [11]. Besides presenting this fundamental (
result, asserting the equivalence between the outlier process (3) r2 if r2 ∈[0,c̄2 ]
and the robust estimation (2), Black and Rangarajan provide a ρ(r) = 2 , (5)
c̄ if r2 ∈[c̄2 ,+∞)
procedure (see Fig. 10 in [11]) to compute Φρ (wi ) and show
that common robust cost functions admit a simple analytical where c̄ is a given truncation threshold. The GNC surrogate
expression for Φρ (wi ). Interestingly, the outlier process (3) function with control parameter µ is:
has been often used in robotics and SLAM [42], [40], without 
2 µ
r if r2 ∈[0, µ+1 c̄2 ]
acknowledging the connection with robust estimation, and 
√
with a heuristic design of the penalty terms Φρ (wi ). ρµ (r) = 2c̄|r| µ(µ+1)−µ(c̄ +r ) if r ∈[ µ+1 c̄ , µ c̄ ] . (6)
2 2 2 µ 2 µ+1 2

Despite the elegance of Lemma 1, Problem (3) remains hard 2
c̄ if r2 ∈[ µ+1 2
µ c̄ ,+∞)
to solve, due to its non-convexity. Approaches in robotics
(e.g., [42], [40]) apply local optimization from an initial guess, By inspection, one can verify ρµ (r) is convex for µ approach-
resulting in brittle solutions (see [12] and Section V). ing zero (ρ00µ (r) = −2µ → 0) and retrieves ρ(r) in (5) for
µ → +∞. An illustration of ρµ (r) is given in Fig. 1(b).
B. Graduated Non-Convexity (GNC)
Graduated non-convexity (GNC) is a popular approach for IV. T HE GNC A LGORITHM FOR ROBUST E STIMATION
the optimization of a generic non-convex cost function ρ(·) We present an algorithm that combines GNC, Black-
and has been used in several endeavors, including vision [33] Rangarajan duality, and non-minimal solvers to solve the
and machine learning [45] (see [46] for more applications). robust estimation problem (2) without an initial guess.
The basic idea of GNC is to introduce a surrogate cost ρµ (·),
governed by a control parameter µ, such that (i) for a certain
value of µ, the function ρµ (·) is convex, and (ii) in the limit A. Overview: GNC Algorithm with Non-minimal Solvers
(typically for µ going to 1 or infinity) one recovers the original We start by providing an overview of the proposed al-
(non-convex) ρ(·). Then GNC computes a solution to the non- gorithm, then we delve into technical details and tailor the
convex problem by starting from its convex surrogate and approach to two specific robust cost functions (Section IV-B).
4

Instead of optimizing directly the robust estimation prob- B. The GNC Algorithm with GM and TLS Costs
lem (2), we use GNC and, at each outer iteration, we fix a Here we tailor the GNC algorithm to two cost functions,
µ and optimize: the Geman McClure and the Truncated Least Squares costs,
N provide expressions for the penalty term Φρµ , and discuss how
X
min ρµ (r(yi , x)). (7) to solve the weight update step. The proofs of the following
x∈X propositions are given in the supplementary material [50].
i=1

Since non-minimal solvers cannot be used directly to solve (7), Proposition 3 (GNC-Geman McClure (GNC-GM)). Consider
we use the Black-Rangarajan duality and rewrite (7) using the the Geman-McClure function and its GNC surrogate with
corresponding outlier process: control parameter µ, as introduced in Example 1. Then, the
minimization of the surrogate function ρµ (·) is equivalent to
N
X the outlier process with penalty term chosen as:
wi r2 (yi , x) + Φρµ (wi ) .
 
min (8) √
x∈X ,wi ∈[0,1]
i=1 Φρµ (wi ) = µc̄2 ( wi − 1)2 . (11)
.
As discussed in Section IV-B and [11], it is easy to compute Moreover, defining the residual r̂i2 = r2 (yi , x(t) ), the weight
the penalty terms Φρµ (·) even for the surrogate function ρµ (·). update at iteration t can be solved in closed form as:
Finally, we solve (8) by alternating optimization, where at 2
µc̄2

each inner iteration we first optimize over the x (with fixed (t)
wi = . (12)
wi ), and then we optimize over wi , i = 1, . . . , N (with fixed r̂i2 + µc̄2
x). In particular, at inner iteration t, we perform the following:
Proposition 4 (GNC-Truncated Least Squares (GNC-TLS)).
1) Variable update: minimize (8) with respect to x with Consider the truncated least squares function and its GNC
(t−1)
fixed weights wi : surrogate with control parameter µ, as introduced in Exam-
ple 2. Then, the minimization of the surrogate function ρµ (·)
N
X (t−1) 2 is equivalent to the outlier process with penalty term:
x(t) = arg min wi r (yi , x), (9)
x∈X i=1 µ(1 − wi ) 2
Φρµ (wi ) = c̄ . (13)
µ + wi
where we dropped the second term in (8) which does not
depend on x. Problem (9) is simply a weighted version .
Moreover, defining the residual r̂i2 = r2 (yi , x(t) ), the weight
of the outlier-free problem (1), hence it can be solved update at iteration t can be solved in closed form as:
globally using certifiably optimal non-minimal solvers.  h i
2 µ+1 2
2) Weight update: minimize (8) with respect to wi (i =  0 if r̂i ∈ c̄ , +∞
h µ


1, . . . , N ) with fixed x(t) :
 p i
(t) µ
wi = r̂c̄i µ(µ + 1) − µ if r̂i2 ∈ µ+1 c̄2 , µ+1 c̄ 2
(14)
 h iµ
N h  µ
1 if r̂i2 ∈ 0, µ+1 c̄2 .
X i 
w(t) = arg min wi r2 (yi , x(t) ) + Φρµ (wi ) , (10)
wi ∈[0,1] i=1
Remark 5 (Implementation Details). For GNC-GM, we start
where r2 (yi , x(t) ) is a constant for fixed x, and the with a convex surrogate (µ → ∞) and decrease µ till we
2 .
expression of Φρµ (·) depends on the choice of robust recover the original cost (µ → 1). In practice, calling rmax =
2 (0)
cost function. As we discuss in Section IV-B, the weight maxi (r (yi , x )) the maximum residual after the first vari-
2
update can be typically solved in closed form. able update, we initialize µ = 2rmax /c̄2 , update µ ← µ/1.4
at each outer iteration, and stop when µ decreases below 1.
The process is then repeated for changing values of µ, where For GNC-TLS, we start with a convex surrogate (µ → 0) and
each change of µ increases the amount of non-convexity. increase the µ till we recover the original cost (µ → ∞). In
Remark 2 (Teaching an Old Dog New Tricks). While the practice, we initialize µ = c̄2 /(2rmax
2
− c̄2 ), update µ ← 1.4µ
combination of GNC and Black-Rangarajan duality has been at each outer
PNiteration, and stop when the sum of the weighted
investigated in related works [11], [35], its applicability has residuals i=1 wi r̂i2 converges. For each outer iteration we
been limited by the lack of global solvers for the variable perform a single variable and weight update. At the first
(0)
update in (9). For instance, [35] focuses on a specific problem iteration, all weights are set to 1 (wi = 1, i = 1, . . . , N ). For
(point cloud registration) where (9) can be solved in closed both robust functions, we set the parameter c̄ to the maximum
form [36], [3], while [11] focuses on a Markov Random Field error expected for the inliers, see Remarks 1-2 in [14].
formulation for which global solvers and heuristics exist [47].
One of the main insights behind our approach is that modern V. A PPLICATIONS AND E XPERIMENTS
non-minimal solvers (developed over the last 5 years) allow
solving (9) globally for a broader class of problems, including We showcase our robust non-minimal solvers in three spatial
spatial perception problems such as SLAM, mesh registration, perception applications: point cloud and mesh registration
and object localization from images. (Section V-A), pose graph optimization (Section V-B), and
shape alignment (Section V-C).
 YANG et al.: GRADUATED NON-CONVEXITY FOR ROBUST SPATIAL PERCEPTION 5

(a) TEASER GNC-TLS RANSAC (b) TEASER GNC-TLS RANSAC (c) ADAPT GNC-TLS GNC-GM RANSAC
ADAPT GNC-GM ADAPT GNC-GM
103
102 100

Translation Error [m]

Rotation Error [deg]

# of Iterations
101
10-1

100 102
-2
10

10-1
10-3
60 70 80 90 95 60 70 80 90 95 60 70 80 90 95
Outlier Rate (%) Outlier Rate (%) Outlier Rate (%)

Fig. 2. Point Cloud Registration. Performance of GNC-GM and GNC-TLS compared with state-of-the-art techniques on the Bunny dataset [48] for increasing
outliers. (a) rotation error; (b) translation error; (c) number of iterations until convergence. Statistics are computed over 20 Monte Carlo runs.
(a) ADAPT GNC-TLS GNC-GM RANSAC (b) ADAPT GNC-TLS GNC-GM RANSAC (c) ADAPT GNC-TLS GNC-GM RANSAC
4
10
102
0
10
Translation Error [m]
Rotation Error [deg]

10-1 103

# of Iterations
0
10

10-2
102
-3
10
10-2

10 20 30 40 50 60 70 80 90 10 20 30 40 50 60 70 80 90 10 20 30 40 50 60 70 80 90
Outlier Rate (%) Outlier Rate (%) Outlier Rate (%)

(d)
(e) (f)

Fig. 3. Mesh Registration. Performance of GNC-GM and GNC-TLS compared with state-of-the-art techniques on mesh registration on the PASCAL+ “car-2”
dataset [49] for increasing outliers. Quantitative results: (a) rotation error; (b) translation error; (c) number of iterations until convergence. Qualitative results:
(d) point cloud and mesh with putative correspondences (70% outliers; green: inliers, red: outliers); (e) successful registration using GNC-TLS; (f) incorrect
registration using RANSAC. Statistics are computed over 20 Monte Carlo runs.

A. 3D Point Cloud and Mesh Registration Point Cloud Registration Results. We use the Bunny
dataset from the Stanford 3D Scanning Repository [48]. We
Setup. In generalized 3D registration, given a set of 3D first scale the Bunny point cloud to be inside a unit cube, and
points ai ∈ R3 , i = 1, . . . , N , and a set of primitives Pi , then at each Monte Carlo run we apply a random rotation and
i = 1, . . . , N (being points, lines and/or planes) with putative translation to get a transformed copy of the Bunny. N = 100
correspondences ai ↔ Pi (potentially including outliers), the correspondences are randomly chosen, where we add zero-
goal is to find the best rotation R ∈ SO(3) and translation mean Gaussian noise with standard deviation σ = 0.01 to
t ∈ R3 that align the point cloud to the 3D primitives. The the inliers, while corrupt the outliers with randomly generated
residual error is r(R, t) = d(Pi , Rai + t), where d(·) denotes points as in [2]. We benchmark the performance of GNC-
the distance between a primitive Pi and a point ai after the GM and GNC-TLS against (i) RANSAC with 1000 maximum
transformation (t, R) is applied. The formulation can also iterations and 99% confidence using Horn’s 3-point minimal
accommodate weighted distances to account for heterogeneous solver [3], plus refinement using the maximum consensus set
and anisotropic measurement noise. In the outlier-free case, of inliers, (ii) ADAPT [27], and (iii) TEASER [2].
Horn’s method [3] gives a closed-form solution when all the
3D primitives are points and the noise is isotropic, and [8] de- Fig. 2 reports the statistics for 60%-95% outlier rates (all
velops a certifiably optimal relaxation when the 3D primitives methods work well below 60%). Fig. 2(a)-(b) show the rotation
include points, lines, and planes and the noise is anisotropic. and translation errors for increasing outliers. RANSAC, GNC-
We now show that, using the GNC-GM and GNC-TLS solvers, GM, GNC-TLS, and ADAPT all break at 90% outliers and achieve
we can efficiently robustify these non-minimal solvers. We similar estimation accuracy at outlier ratio below 90%. TEASER
benchmark our algorithms against state-of-the-art techniques is a specialized robust global solver and outperforms all other
in point cloud registration and mesh registration. techniques; unfortunately, it currently does not scale to large
 6

(a) ADAPT GNC-GM DCS (100) (b) ADAPT GNC-GM DCS (100) (c) ADAPT GNC-GM DCS (100)
GNC-TLS PCM (5%) G2O GNC-TLS PCM (5%) G2O GNC-TLS PCM (5%) G2O
40 3
10
12
Average Trajectory Error [m]

Average Trajectory Error [m]

10 30 2

Solution Time [s]

10
8
20
6 10 1
4 10

2 10 0

0 0

10 20 30 40 50 60 70 80 90 10 20 30 40 50 60 70 80 90 10 20 30 40 50 60 70 80 90
Outlier Rate (%) Outlier Rate (%) Outlier Rate (%)

Fig. 4. Pose Graph Optimization. Performance of GNC-GM and GNC-TLS compared with state-of-the-art techniques for increasing outliers. Average
trajectory error for (a) INTEL dataset and (b) CSAIL dataset; (c) solution time on the CSAIL dataset. Statistics are computed over 10 Monte Carlo runs.
problem instances (>5min runtime) and does not extend to where κij and τij are known parameters describing the mea-
other registration problems (e.g., mesh registration); Fig. 2(c) surement noise distribution, and k·kF denotes the Frobenious
plots the number of inner iterations used by RANSAC, GNC- norm. SE-Sync [6] is a fast non-minimal solver for PGO.
GM, GNC-TLS, and ADAPT (TEASER has no outlier iterations). PGO Results. We test GNC-GM and GNC-TLS on two stan-
With respect to the outlier rates, the number of iterations dard benchmarking datasets: INTEL and CSAIL, described in [6].
grows very fast for RANSAC, grows linearly for ADAPT and In each dataset, we preserve the odometry measurements,
is almost constant for GNC-GM and GNC-TLS. At 80% outliers, but at each Monte Carlo run we spoil loop closures with
the average runtimes for RANSAC, GNC-GM, and GNC-TLS are random outliers. To create outliers, we sample random pairs
218, 22 and 23ms, respectively, showing that GNC can be much of poses and add a random measurement between them.
faster than RANSAC. For point cloud registration, GNC-GM is We benchmark GNC-GM and GNC-TLS against (i) g2o [20],
essentially the same as Fast Global Registration [35], while (ii) dynamic covariance scaling (DCS) [42], (iii) pairwise
below we show that the use of non-minimal solvers allows consistent measurement set maximization (PCM) [30], and (iv)
extending GNC to other spatial perception applications. ADAPT [27]. DCS and PCM are fairly sensitive to the choice of
Mesh Registration Results. We apply GNC-GM and GNC- parameters: we tested parameters Φ = {1, 10, 100} for DCS,
TLS to register a point cloud to a mesh, using the non-minimal and thresholds τ = {5%, 10%, 15%} for PCM, and, for the
solver [8]. We use the “car-2” mesh model from the PASCAL+ sake of clarity, we only reported the choice of parameters
dataset [49]. At each Monte Carlo run, we generate a point ensuring the best performance.
cloud from the mesh by randomly sampling points lying on the Fig. 4(a) shows the average trajectory errors for the INTEL
vertices, edges and faces of the mesh model, and then apply dataset. g2o is not a robust solver and performs poorly across
a random transformation and add Gaussian noise with σ = the spectrum. DCS and PCM are specialized robust local
0.05. We establish 40 point-to-point, 80 point-to-line and 80 solvers, but their errors gradually increase with the percentage
point-to-plane correspondences, and create outliers by adding of outliers. GNC-GM, GNC-TLS, and ADAPT are insensitive to up
incorrect point to point/line/plane correspondences (Fig. 3(d)). to 40% outliers and preserve an acceptable performance till
We benchmark GNC-GM and GNC-TLS against (i) RANSAC with 70 − 80% of outliers; GNC-TLS dominates the others. Fig. 4(b)
10,000 maximum iterations and 99% confidence using the 12- reports the results on the CSAIL dataset. Also in this case,
point minimal solver in [17] and (ii) ADAPT [27]. GNC-TLS dominates the other techniques and is robust to 90%
Fig. 3(a)-(c) show the errors and iterations for each tech- outliers. Fig. 4(c) reports the CPU times required by the
nique. GNC-GM, GNC-TLS, and ADAPT are robust against 80% techniques to produce a solution on CSAIL; in this case, all
outliers, while RANSAC breaks at 50% outliers. The number of techniques are implemented in C++.
iterations of ADAPT grows linearly with the number of outliers,
while GNC-GM’s and GNC-TLS’s iterations remain constant. C. Shape Alignment
Fig. 3(e) shows a successful registration with GNC-TLS and Setup. In shape alignment, given 2D features zi ∈ R2 , i =
Fig. 3(f) shows an incorrect registration from RANSAC, both 1, . . . , N , on a single image and 3D points Bi ∈ R3 , i =
obtained in a test with 70% outliers. 1, . . . , N , of an object with putative correspondences zi ↔ Bi
(potentially including outliers), the goal is to find the best scale
s > 0, rotation R, and translation t of the object, that projects
B. Pose Graph Optimization
the 3D shape to the 2D image under weak perspective projec-
Setup. PGO is one of the most common estimation engines tion. The residual function is r(s, R, t) = kzi − sΠRBi − tk,
for SLAM [1]. PGO estimates a set of poses (ti , Ri ), i = where Π ∈ R2×3 is the weak perspective projection matrix
1, . . . , n (typically sampled along the robot trajectory) from (equal to the first two rows of a 3×3 identity matrix). Note that
pairwise relative pose measurements (t̃ij , R̃ij ) (potentially t is a 2D translation, but under weak perspective projection
corrupted with outliers). The residual error is the distance one can extrapolate a 3D translation (i.e., recover the distance
between the expected relative pose and the measured one: of the camera to the object) using the scale s.
q A Non-minimal Solver for Shape Alignment. The liter-
r({Ri , ti }) = κij kRj − Ri R̃ij k2F +τij ktj − ti − Ri t̃ij k22 , ature is missing a global solver for shape alignment, even in
 YANG et al.: GRADUATED NON-CONVEXITY FOR ROBUST SPATIAL PERCEPTION 7

(a) ADAPT GNC-GM Zhou (b) ADAPT GNC-GM Zhou (c) ADAPT GNC-TLS GNC-GM RANSAC
GNC-TLS RANSAC GNC-TLS RANSAC
100
102
102
80

Translation Error [m]

Rotation Error [deg]

101

# of Iterations
60
1
10 100 40

20
10-1

100 0

10 20 30 40 50 60 70 80 10 20 30 40 50 60 70 80 10 20 30 40 50 60 70 80
Outlier Rate (%) Outlier Rate (%) Outlier Rate (%)

(d) (e) (f)

Fig. 5. Shape Alignment. Performance of GNC-GM and GNC-TLS compared with state-of-the-art techniques on the FG3DCar dataset [51] for increasing
outliers. Quantitative results: (a) rotation error; (b) translation error; (c) number of iterations. Qualitative results with 70% outliers: (d) successful pose
estimation by GNC-GM; (e) failed pose estimation by RANSAC; (f) failed pose estimation by Zhou’s method [15]. Yellow thick line: 3D skeleton of the car
model; circle: 3D points on the car model; square: detected 2D features on the image; green: inlier correspondences; red: outlier correspondences.
the outlier-free case. Therefore, we start by proposing a non- dataset [51], where we use the ground-truth 3D shape model as
minimal solver for (weighted) outlier-free shape alignment: B and the ground-truth 2D landmarks as z. To generate out-
PN 2 liers for each image, we set random incorrect correspondences
min 2 i=1 wi kzi − sΠRBi − tk , (15) between 3D points and 2D features. We benchmark GNC-GM
s>0,R∈SO(3),t∈R
and GNC-TLS against (i) Zhou’s method [15], (ii) RANSAC with
where wi are constant weights. The next proposition states
100 maximum iterations and 99% confidence using a 4-point
that the global minimizer of problem (15) can be obtained by
minimal solver (we use our SOS solver as minimal solver),
solving a quaternion-based unconstrained optimization.
and (iii) ADAPT [27].
Proposition 6 (Quaternion-based Shape Alignment). Define Fig 5(a)-(c) show translation errors, rotation errors, and
. √
the non-unit quaternion v = sq ∈ R4 , where q is the number of iterations for all compared techniques. Statistics
unit-quaternion corresponding to R and s is the unknown are computed over all 600 images in the FG3DCar dataset.
.
scale. Moreover, if v = [v1 , v2 , v3 , v4 ]T , define [v]2 = The performance of Zhou’s method degrades quickly with
[v12 , v22 , v32 , v42 , v1 v2 , v1 v3 , v1 v4 , v2 v3 , v2 v4 , v3 v4 ]T ∈ R10 as increasing outliers. RANSAC breaks at 60% outliers. GNC-GM,
the vector of degree-2 monomials in v. Then the globally GNC-TLS, and ADAPT are robust against 70% outliers, while
optimal solutions (s? , R? , t? ) of problem (15) can be obtained GNC-GM and GNC-TLS require a roughly constant number of
from the solution v ? of the following optimization: iterations. Qualitative results for GNC-GM, RANSAC, and Zhou’s
. approach are given in Fig. 5(d)-(f), respectively.
min4 f (v) = [v]T T
2 Q[v]2 − 2g [v]2 + h, (16)
v∈R
VI. C ONCLUSIONS
where Q ∈ R10×10 , g ∈ R10 , and h > 0 are known quantities,
whose expression is given in the supplementary material [50]. We proposed a general-purpose approach for robust estima-
tion that leverages modern non-minimal solvers. The approach
Problem (16) requires minimizing a degree-4 polynomial allows extending the applicability of Black-Rangarajan duality
f (v) in 4 variables; to this end, we apply SOS relaxation and and Graduated Non-convexity (GNC) to several spatial per-
relax (16) to the following SOS optimization: ception problems, ranging from mesh registration and shape
min −γ , s.t. f (v) − γ is SOS, (17) alignment to pose graph optimization. We believe the proposed
v∈R4 ,γ∈R approach can be a valid replacement for RANSAC. While
which can be readily converted to an SDP and solved with RANSAC requires a minimal solver, our GNC approach requires
certifiable optimality [4]. We use the GloptiPoly 3 [52] package a non-minimal solver. Our approach is deterministic, resilient
in Matlab to solve problem (17) and found that empirically to a large number of outliers, and significantly faster than
the relaxation is always exact. Solving the SDP takes about specialized solvers. As a further contribution, we presented a
80 ms on a desktop computer. non-minimal solver for shape alignment. Future work includes
Shape Alignment Results. We test the performance of GNC- investigating a priori and a posterior conditions that guarantee
GM and GNC-TLS, together with our SOS solver on the FG3DCar convergence of GNC to globally optimal solutions.
8

Supplementary Material (i) Translation-free Shape Alignment. To this end, we

develop the objective function of (A22):
VII. P ROOF OF P ROPOSITION 3 PN
The outlier process (11) is derived by following the Black- i=1 wi kzi − sΠRBi − tk2
PN
Rangarajan procedure in Fig. 10 of [11]. Therefore, we here = i=1wi tT t − 2wi (ziT − sBiT RT ΠT )t
prove the weight update rule in eq. (12). To this end, we derive PN
+ i=1 kzi − sΠRBi k2 , (A24)
the gradient gi of the objective function with outlier process
√
Φρµ (wi ) = µc̄2 ( wi − 1)2 in eq. (10) with respect to wi : whose partial derivative w.r.t. t is:
 
2 2 1
gi = r̂i + µc̄ 1 − √ . (A18) N N N
!
wi X X X
2 wi t − 2 wi zi − sΠR wi Bi . (A25)
From eq. (A18) we observe that if wi → 0, then gi → −∞; i=1 i=1 i=1
and if wi = 1, then gi = r̂i2 ≥ 0. These facts, combined
with the monotonicity of gi w.r.t. wi , ensure that there exists By setting the derivative (A25) to zero, we derive t? in closed
a unique wi? such that the gradient gi vanishes: form as:
2
µc̄2

wi? = . (A19) t? = z̄w − s? ΠR? B̄w , (A26)
r̂i2 + µc̄2
This vanishing point is the global minimizer of (10). where
PN PN
VIII. P ROOF OF P ROPOSITION 4 i=1 wi zi i=1 wi Bi
z̄w = PN , B̄w = P N
. (A27)
The outlier process (13) is derived by following the Black- i=1 wi i=1 wi
Rangarajan procedure in Fig. 10 of [11]. Therefore, we here
By inserting eq. (A26) back to the original problem (A22), we
prove the weight update rule in eq. (14). To this end, we derive
obtain an optimization only involving the scale and rotation:
the gradient gi of the objective function with outlier process
Φρµ (wi ) = µ(1−w i)
µ+wi in eq. (10) with respect to wi : N
X
µ(µ + 1) 2 min kz̃i − sΠRB̃i k2 , (A28)
gi = r̂i2 − c̄ . (A20) s>0,R∈SO(3)
i=1
(µ + wi )2
From eq. (A20) we observe that if wi = 0, then gi = r̂i2 − where:
µ+1 2 2 µ 2
µ c̄ ; and if wi = 1, then gi = r̂i − µ+1 c̄ . Therefore, the √ √
?
global minimizer wi can be obtained by setting the gradient z̃i = wi (zi − z̄w ), B̃i = wi (Bi − B̄w ). (A29)
gi to zero, leading to:
 h i (ii) Quaternion-based Translation-free Problem. We now
0

 if r̂i2 ∈ µ+1
µ c̄2 , +∞ show that the translation-free shape alignment problem (A28)
 p h i
µ
wi? = r̂c̄i µ(µ + 1) − µ if r̂i2 ∈ µ+1 c̄2 , µ+1 c̄ 2
.(A21) can be converted to the quaternion-based unconstrained shape
 h iµ alignment problem (A23). To this end, we denote q ∈
µ
 2 2
1 if r̂i ∈ 0, µ+1 c̄

R4 , kqk= 1 as the unit-quaternion corresponding to the ro-
tation matrix R. It can be verified by inspection that R is
IX. P ROOF OF P ROPOSITION 6 a linear function of [q]2 ∈ R10 , the vector of all degree-2
In the main document, we claim that the optimal solutions monomials of q:
(s? , R? , t? ) for the shape alignment problem
.
PN 2
R = R(q) = mat(A[q]2 ), (A30)
min 2 i=1 wi kzi − sΠRBi − tk (A22)
s>0,R∈SO(3),t∈R
where A ∈ R9×10 is a constant matrix, and mat(·) converts a
can be obtained from the optimal solution v ? of the
vector to a matrix with proper dimension (in this case a 3 × 3
quaternion-based unconstrained optimization:
matrix). The expression of the matrix A is as follows:
.
min4 f (v) = [v]T T
2 Q[v]2 − 2g [v]2 + h. (A23)  
v∈R 1 −1 −1 1 0 0 0 0 0 0
Here we prove this proposition, and provide a formula to  0 0 0 0 2 0 0 0 0 2 
compute Q ∈ R10×10 , g ∈ R10 and h, as well as construct
 
 0 0 0 0 0 2 0 0 −2 0 
(s? , R? , t? ) from v ? . Particularly, we complete the proof in
 

 0 0 0 0 2 0 0 0 0 −2 

two steps: (i) we marginalize out the translation t and convert A = 
 −1 1 −1 1 0 0 0 0 0 0  .(A31)

problem (A22) into an equivalent translation-free problem; (ii) 
 0 0 0 0 0 0 2 2 0 0 

we reparametrize the rotation matrix using unit-quaternion and 
 0 0 0 0 0 2 0 0 2 0 

then arrive at the unconstrained optimization in (A23). The two  0 0 0 0 0 0 −2 2 0 0 
steps follow in more detail below: −1 −1 1 1 0 0 0 0 0 0
YANG et al.: GRADUATED NON-CONVEXITY FOR ROBUST SPATIAL PERCEPTION 9

Using the notation in (A30) and developing the squares in the [5] L. Carlone, G. Calafiore, C. Tommolillo, and F. Dellaert, “Planar pose
objective function of problem (A28) (denoting r = vec (R)): graph optimization: Duality, optimal solutions, and verification,” IEEE
Trans. Robotics, vol. 32, no. 3, pp. 545–565, 2016.
N
X [6] D. Rosen, L. Carlone, A. Bandeira, and J. Leonard, “SE-Sync: a certi-
fiably correct algorithm for synchronization over the Special Euclidean
kz̃i − sΠRB̃i k2 group,” Intl. J. of Robotics Research, 2018.
i=1 [7] A. Eriksson, C. Olsson, F. Kahl, and T.-J. Chin, “Rotation averaging and
N
X strong duality,” IEEE Conf. on Computer Vision and Pattern Recognition
= kz̃i k2 −2s z̃iT ΠRB̃i + s2 B̃iT RT ΠT ΠRB̃i (CVPR), 2018.
[8] J. Briales and J. Gonzalez-Jimenez, “Convex Global 3D Registration
i=1
with Lagrangian Duality,” in IEEE Conf. on Computer Vision and
N
X     Pattern Recognition (CVPR), 2017.
= kz̃i k2 −2s tr B̃i ziT ΠR + s2 tr B̃i B̃iT RT ΠT ΠR [9] J. Briales, L. Kneip, and J. Gonzalez-Jimenez, “A certifiably globally
i=1 optimal solution to the non-minimal relative pose problem,” in IEEE
N Conf. on Computer Vision and Pattern Recognition (CVPR), June 2018.
X  T [10] S. Agostinho, J. Gomes, and A. Del Bue, “CvxPnPL: A unified convex
= kz̃i k2 −2s vec ΠT z̃i B̃iT r + s2 r T (B̃i B̃iT ⊗ ΠT Π)r solution to the absolute pose estimation problem from point and line
i=1 correspondences,” arXiv preprint arXiv:1907.10545, 2019.
N
X [11] M. J. Black and A. Rangarajan, “On the unification of line processes,
= kz̃i k2 −2s fiT A[q]2 + s2 [q]T T outlier rejection, and robust statistics with applications in early vision,”
2 A Fi A[q]2 Intl. J. of Computer Vision, vol. 19, no. 1, pp. 57–91, 1996.
i=1 [12] L. Carlone and G. Calafiore, “Convex relaxations for pose graph
N
X optimization with outliers,” IEEE Robotics and Automation Letters (RA-
= kzi k2 −2s giT [q]2 + s2 [q]T
2 Qi [q]2
L), vol. 3, no. 2, pp. 1160–1167, 2018.
[13] P. Lajoie, S. Hu, G. Beltrame, and L. Carlone, “Modeling perceptual
i=1
aliasing in SLAM via discrete-continuous graphical models,” IEEE
= s2 [q]T T
2 Q[q]2 − 2s g [q]2 + h, (A32) Robotics and Automation Letters (RA-L), 2019.
[14] H. Yang and L. Carlone, “A quaternion-based certifiably optimal solution
where the Q, g, and h can be computed by: to the Wahba problem with outliers,” in Intl. Conf. on Computer Vision
 P  (ICCV), 2019, arxiv preprint: 1905.12536.
T N T T [15] X. Zhou, M. Zhu, S. Leonardos, and K. Daniilidis, “Sparse represen-
Q = A B̃ i B̃ i ⊗ Π Π A;
P i=1 


  tation for 3D shape estimation: A convex relaxation approach,” IEEE
T N T T (A33) Trans. Pattern Anal. Machine Intell., vol. 39, no. 8, pp. 1648–1661,
g=A i=1 vec Π z̃i B̃i ;

 PN 2017.
2
h = i=1 kz̃i k . [16] F. L. Markley and J. L. Crassidis, Fundamentals of spacecraft attitude

determination and control. Springer, 2014, vol. 33.
. √ [17] K. Khoshelham, “Closed-form solutions for estimating a rigid motion
Now, since s > 0, we define v = sq and it is straightforward
from plane correspondences extracted from point clouds,” ISPRS Journal
to verify that [v]2 = s[q]2 , and therefore the objective function of Photogrammetry and Remote Sensing, vol. 114, pp. 78 – 91, 2016.
in (A32) is equal to the objective function of problem (A23). [18] Z. Kukelova, “Algebraic methods in computer vision,” Ph.D. disserta-
. √ tion, Czech Technical University in Prague, 2013.
In addition, because v = sq, there is no constraint on v, be-
[19] A. Censi, “An ICP variant using a point-to-line metric,” in IEEE Intl.
cause the unit-norm constraint of the quaternion q disappears. Conf. on Robotics and Automation (ICRA), Pasadena, CA, May 2008.
After we solve problem (A23), we can recover the optimal [20] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard,
solution (s? , R? , t? ) of the original constrained optimiza- “g2o: A general framework for graph optimization,” in Proc. of the IEEE
Int. Conf. on Robotics and Automation (ICRA), May 2011.
tion (A22) from the optimal solution v ? of problem (A23) [21] R. Hartley and F. Kahl, “Global optimization through rotation space
using the following formula: search,” Intl. J. of Computer Vision, vol. 82, no. 1, pp. 64–79, 2009.
 [22] C. Olsson, F. Kahl, and M. Oskarsson, “Branch-and-bound methods for
? ? 2
s = kv k ; 

 euclidean registration problems,” IEEE Trans. Pattern Anal. Machine
? Intell., vol. 31, no. 5, pp. 783–794, 2009.
R? = R kvv? k ; (A34) [23] L. Carlone, D. Rosen, G. Calafiore, J. Leonard, and F. Dellaert,

t? = z̄ − s? ΠR? B̄ .
 “Lagrangian duality in 3D SLAM: Verification techniques and optimal
w w solutions,” in IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems
(IROS), 2015, pp. 125–132.
In summary, to solve the outlier-free (weighted) shape align- [24] J. G. Mangelson, J. Liu, R. M. Eustice, and R. Vasudevan, “Guaranteed
ment problem (A22), we first calculate Q, g, h using globally optimal planar pose graph and landmark SLAM via sparse-
bounded sums-of-squares programming,” in IEEE Intl. Conf. on Robotics
eq. (A33), solve the unconstrained optimization (A23) (using and Automation (ICRA). IEEE, 2019, pp. 9306–9312.
SOS relaxation as discussed in the main document), and then [25] T.-J. Chin, Z. Cai, and F. Neumann, “Robust fitting in computer vision:
recover the optimal scale, rotation, translation using eq. (A34). Easy or hard?” in European Conf. on Computer Vision (ECCV), 2018.
[26] T. J. Chin and D. Suter, “The maximum consensus problem: recent
algorithmic advances,” Synthesis Lectures on Computer Vision, vol. 7,
R EFERENCES no. 2, pp. 1–194, 2017.
[27] V. Tzoumas, P. Antonante, and L. Carlone, “Outlier-robust spatial
[1] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, perception: Hardness, general-purpose algorithms, and guarantees,” in
I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), 2019,
localization and mapping: Toward the robust-perception age,” IEEE extended arxiv version: 1903.11683.
Trans. Robotics, vol. 32, no. 6, pp. 1309–1332, 2016. [28] M. Fischler and R. Bolles, “Random sample consensus: a paradigm
[2] H. Yang and L. Carlone, “A polynomial-time solution for robust reg- for model fitting with application to image analysis and automated
istration with extreme outlier rates,” in Robotics: Science and Systems cartography,” Commun. ACM, vol. 24, pp. 381–395, 1981.
(RSS), 2019. [29] Á. Parra Bustos and T. J. Chin, “Guaranteed outlier removal for point
[3] B. K. P. Horn, “Closed-form solution of absolute orientation using unit cloud registration with correspondences,” IEEE Trans. Pattern Anal.
quaternions,” J. Opt. Soc. Amer., vol. 4, no. 4, pp. 629–642, Apr 1987. Machine Intell., vol. 40, no. 12, pp. 2868–2882, 2018.
[4] G. Blekherman, P. A. Parrilo, and R. R. Thomas, Semidefinite optimiza- [30] J. G. Mangelson, D. Dominic, R. M. Eustice, and R. Vasudevan,
tion and convex algebraic geometry. SIAM, 2012. “Pairwise consistent measurement set maximization for robust multi-
10

robot map merging,” in IEEE Intl. Conf. on Robotics and Automation

(ICRA), 2018, pp. 2916–2923.
[31] J. C. Bazin, Y. Seo, and M. Pollefeys, “Globally optimal consensus
set maximization through rotation search,” in Asian Conference on
Computer Vision. Springer, 2012, pp. 539–551.
[32] O. Enqvist, E. Ask, F. Kahl, and K. Åström, “Robust fitting for mul-
tiple view geometry,” in European Conf. on Computer Vision (ECCV).
Springer, 2012, pp. 738–751.
[33] A. Blake and A. Zisserman, Visual reconstruction. MIT Press, 1987.
[34] S. Gold, A. Rangarajan, C. Lu, S. Pappu, and E. Mjolsness, “New
algorithms for 2D and 3D point matching,” Pattern Recognition, vol. 31,
no. 8, pp. 1019–1031, 1998.
[35] Q. Zhou, J. Park, and V. Koltun, “Fast global registration,” in European
Conf. on Computer Vision (ECCV). Springer, 2016, pp. 766–782.
[36] K. Arun, T. Huang, and S. Blostein, “Least-squares fitting of two 3-D
point sets,” IEEE Trans. Pattern Anal. Machine Intell., vol. 9, no. 5, pp.
698–700, sept. 1987.
[37] M. Bosse, G. Agamennoni, and I. Gilitschenski, “Robust estimation and
applications in robotics,” Foundations and Trends in Robotics, vol. 4,
no. 4, pp. 225–269, 2016.
[38] C. Zach, “Robust bundle adjustment revisited,” in European Conf. on
Computer Vision (ECCV), 2014, pp. 772–787.
[39] S. Bouaziz, A. Tagliasacchi, and M. Pauly, “Sparse iterative closest
point,” in ACM Symp. Geom. Process. Eurographics Association, 2013,
pp. 113–123.
[40] N. Sünderhauf and P. Protzel, “Switchable constraints for robust pose
graph SLAM,” in IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems
(IROS), 2012.
[41] E. Olson and P. Agarwal, “Inference on networks of mixtures for robust
robot mapping,” in Robotics: Science and Systems (RSS), July 2012.
[42] P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard,
“Robust map optimization using dynamic covariance scaling,” in IEEE
Intl. Conf. on Robotics and Automation (ICRA), 2013.
[43] M. Pfingsthorn and A. Birk, “Generalized graph SLAM: Solving local
and global ambiguities through multimodal and hyperedge constraints,”
Intl. J. of Robotics Research, vol. 35, no. 6, pp. 601–630, 2016.
[44] C. Wang, Y. Wang, Z. Lin, A. L. Yuille, and W. Gao, “Robust estimation
of 3D human poses from a single image,” in IEEE Conf. on Computer
Vision and Pattern Recognition (CVPR), 2014, pp. 2361–2368.
[45] K. Rose, “Deterministic annealing for clustering, compression, classifi-
cation, regression, and related optimization problems,” Proceedings of
the IEEE, vol. 86, no. 11, pp. 2210–2239, 1998.
[46] H. Mobahi and J. W. Fisher, “On the link between gaussian homotopy
continuation and convex envelopes,” in International Workshop on En-
ergy Minimization Methods in Computer Vision and Pattern Recognition.
Springer, 2015, pp. 43–56.
[47] S. Hu and L. Carlone, “Accelerated inference in Markov Random Fields
via smooth Riemannian optimization,” IEEE Robotics and Automation
Letters (RA-L), 2019.
[48] B. Curless and M. Levoy, “A volumetric method for building complex
models from range images,” in SIGGRAPH, 1996, pp. 303–312.
[49] Y. Xiang, R. Mottaghi, and S. Savarese, “Beyond PASCAL: A bench-
mark for 3d object detection in the wild,” in IEEE Winter Conference
on Applications of Computer Vision. IEEE, 2014, pp. 75–82.
[50] H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non-
convexity for robust spatial perception: From non-minimal solvers to
global outlier rejection,” arXiv preprint arXiv:1909.08605, 2019.
[51] Y.-L. Lin, V. I. Morariu, W. H. Hsu, and L. S. Davis, “Jointly optimizing
3D model fitting and fine-grained classification,” in European Conf. on
Computer Vision (ECCV), 2014.
[52] D. Henrion, J.-B. Lasserre, and J. Löfberg, “GloptiPoly 3: moments,
optimization and semidefinite programming,” Optim. Methods. Softw.,
vol. 24, no. 4-5, pp. 761–779, 2009.