Go-ICP: Solving 3D Registration Efficiently and Globally Optimally
Jiaolong Yang
1,2
, Hongdong Li
2
, Yunde Jia
1
1
Beijing Lab of Intelligent Information Technology, Beijing Institute of Technology
2
Australian National University and NICTA Australia
{yangjiaolong,jiayunde}@bit.edu.cn, hongdong.li@anu.edu.au
Abstract
Registration is a fundamental task in computer vision.
The Iterative Closest Point (ICP) algorithm is one of the
widely-used methods for solving the registration problem.
Based on local iteration, ICP is however well-known to
suffer from local minima. Its performance critically relies
on the quality of initialization, and only local optimality is
guaranteed. This paper provides the very first globally op-
timal solution to Euclidean registration of two 3D pointsets
or two 3D surfaces under the L
2
error. Our method is built
upon ICP, but combines it with a branch-and-bound (BnB)
scheme which searches the 3D motion space SE(3) effi-
ciently. By exploiting the special structure of the underlying
geometry, we derive novel upper and lower bounds for the
ICP error function. The integration of local ICP and global
BnB enables the new method to run efficiently in practice,
and its optimality is exactly guaranteed. We also discuss
extensions, addressing the issue of outlier robustness.
1. Introduction
The Iterative Closest Point (ICP) [5, 10, 35] is a well-
known algorithm for registering two point sets (in 2D or
3D) under Euclidean transformation. It has been successful-
ly applied to solving numerous real-world applications. The
concept of ICP is simple and intuitive. It alternates between
estimating geometric transformation (rotation and transla-
tion), and estimating the point-wise correspondences. Part-
ly due to its conceptual simplicity, as well as its good per-
formance in practice, ICP is one of the most popular algo-
rithms for registration, widely used in computer vision, and
beyond computer vision.
ICP is however also well-known for its suffering from
the issue of local minima, due to the local iterative pro-
cedure it adopts. Being an iterative method, ICP requires
a good initialization to start, without which the algorithm
may easily get trapped into local minima. If this situation
happens, the solution found by ICP may be far away from
the true (optimal) solution, leading to erroneous estimation.
More seriously, ICP itself has no way to tell whether or not
it has been trapped into a local minimum. Despite that this
drawback of local-minima is generally well-known, rela-
tively few papers have tackled this issue explicitly.
This paper is, to the best of our knowledge, the very first
that proposes a truly globally optimal solution to ICP type
Euclidean registration in 3D. It provides guaranteed opti-
mality without the need for a good initialization. In fact,
our new method always produces the exact and globally op-
timal solution (up to any desired accuracy), starting from
any initialization.
We call our new algorithm the Globally Optimal ICP (or
Go-ICP in short), because it largely resembles the computa-
tional structure of a standard ICP. It still relies on the search
of closest-points at each iteration. Moreover, a standard (lo-
cal) ICP is employed as a subroutine in our new algorithm.
By exploiting special structure of the underlying geometry
of SE(3), and with the help of local ICP, our Go-ICP algo-
rithm works rather efficiently. We have conducted extensive
tests on both synthetic data and real data; satisfactory result-
s (both in terms of theoretical optimality and computational
efficiency) are obtained for all tests.
Although Go-ICP is specifically designed for 3D Eu-
clidean registration since we take advantage of the geometry
of SE(3), the same techniques used in this paper may be in-
spiring for other cases as well (e.g. 2D or affine). Moreover,
confining to 3D should not be considered as a limitation, as
the 3D case is arguably the most useful case for registra-
tion. Our error metric used by Go-ICP follows strictly that
of ICP, namely, minimizing the L
2
norm of the vector of
residuals. However, with small effort we can extend it to
other metrics such as the L
1
norm, Least Median Squares
and other variants of ICP as well.
1
2. Related Work
There has been a large volume of work published on ICP,
preventing us from giving a comprehensive list. Below we
only list a few most relevant works, that either aimed to
achieve optimal ICP or, more generally, addressed optimal-
ity in Euclidean registration. For other papers, the reader
is referred to a recent survey [8] or [31] and the references
therein.
To alleviate the local minima issue, previous work has at-
tempted to enlarge the basin of convergence by smoothing
out the objective function. Good performance has been ob-
served from Fitzgibbon’s LM-ICP [13] with robust kernels.
Probability density based techniques [18, 33, 27, 6] have
been used to model the points with Gaussian Mixture Mod-
els. Although improved robustness can be archived, the op-
timization procedures they adopted are still local search. Ef-
forts have been devoted to various heuristic stochastic op-
timizations, e.g. particle swarm optimization [34] and par-
ticle filtering [32], to help the registration jump out of lo-
cal minima. While these methods provide improved re-
sults, none of them maintains a deterministic and exact
optimality. Another class of methods adopt the heuris-
tic hypothesis-and-test idea. Examples include Hough
Transform, RANSAC and alignment-based object recogni-
tion [17]. They work well in cluttered scenes (e.g. the 4PC-
S [1]), but the heuristic nature renders their results not ex-
actly optimal.
Registration methods that come with guaranteed opti-
mality were published in the past, though in a smaller num-
ber. For example, in 2D cases, branch-and-bound (BnB)
has been used for image pattern matching [7, 26, 29]. A
truncated L
2
optimization for optimal geometric fitting is
recently addressed in [2]. However, most of these methods
are focused on the much simpler 2D case. Extending them
to 3D and SE(3) is a non-trivial task.
Li and Hartley [23] presented a rotation-search method
for 3D-3D registration. While being globally optimal,
their method makes unrealistic assumption such as the t-
wo pointsets are of equal size and there is only pure rota-
tion. Branch-and-bound based Euclidean registration was
investigated in Olsson et al. [28] for cases with known cor-
respondences. Enqvist et al. [12] converted the registration
problem to graph vertex cover and provided an optimal so-
lution. Applications of BnB to other vision geometry prob-
lems may be found in [22, 20].
Methods that make use of local invariant shape descrip-
tors (e.g. spin image [19], shape contexts [4], EGI [24])
are mostly heuristic and do not address the optimality is-
sue. One exception is the work of Gelfand et al. [14] in
which they proposed a globally optimal solution on top of
the local descriptors. Their idea is based on pair-wise dis-
tance consistency similar to [12]. Their optimization is ap-
plied to a relatively small number of local descriptors rather
than whole point clouds. In contrast, our method (to be
described in this paper) requires no local descriptors and
directly works on raw point clouds.
In this paper, we solve the 3D Euclidean registration
problem with global optimality guarantee. Our method is
related to the idea of SO(3) space search, as proposed in
[15, 16] and extended in (e.g. [30, 3]). Most of the exist-
ing work along this line are based on the L
∞
-norm mini-
mization. For the case of L
2
-norm global optimization (e.g.
which is of interest to this paper), few results are known to
us.
3. The 3D Registration Problem
The standard ICP algorithm solves an L
2
-error mini-
mization problem, defined as follows.
Let two 3D pointsets X = {x
i
}, i = 1, ..., M and
Y = {y
j
}, j = 1, ..., N, where x
i
, y
j
∈ R
3
are point co-
ordinates, be the data pointset and model pointset respec-
tively. The aim is to estimate a rigid motion with rotation
R ∈ SO(3) and translation t ∈ R
3
, which minimizes the
following L
2
error E:
E(R, t) =
M
X
i=1
e
i
(R, t)
2
=
M
X
i=1
kRx
i
+ t − y
j
∗
k
2
(1)
where e
i
(R, t) is the per-point residual error for x
i
. The
point y
j
∗
∈ Y is denoted as the optimal correspondence of
x
i
, which in the context of ICP is the closest point to the
transformed x
i
in Y, i.e.
j
∗
= argmin
j∈{1,..,N }
kRx
i
+ t − y
j
k. (2)
Given initial transformation R and t, the ICP algorithm
iteratively solves the above minimization via alternating be-
tween estimating the transformation in Eq. (1), and finding
the closest-point matches by Eq. (2). Due to such iterative
nature, ICP can only guarantee the convergence to a local
minimum.
4. Method Overview
In this work, we seek a truly globally-optimal solution
to 3D registration. We choose to use branch-and-bound to
solve the global optimization problem. Our method is sum-
marized as follows.
Use BnB to search the space of SE(3)
Whenever a better solution is found, call ICP (ini-
tialized at this solution) to refine (reduce) the objec-
tive function value. Use ICP’s result as an updated
upper bound to continue the above BnB search.
Until convergence.
2
While the idea of using BnB is straightforward, it is non-
trivial to apply it for the case of rigid 3D registration. Al-
though existing BnB approaches work successfully for 2D
registration, extending the success to 3D has been much
challenging (see e.g., [16, 12, 23, 12]).
In order to apply BnB to 3D registration, one must first
answer the following questions: (i) how to parameterize and
branch the domain of 3D motions, (ii) how to efficiently find
an upper bound and lower bound.
Domain parametrization. Recall that our goal is to min-
imize the error E in Eq. (1) over the domain of all feasible
3D motions (i.e. the group of SE(3), defined by SE(3) =
SO(3) × R
3
). Each member of SE(3) can be minimally
parameterized by six parameters. The angle-axis represen-
tation is used in this paper to encode rotation, and we use
R
r
to denote the rotation matrix with its angle-axis repre-
sentation to be r, i.e. R
r
= exp([ r ]
×
) where exp(·) is the
matrix exponential and [ ·]
×
denotes the skew-symmetric
matrix representation. With this representation, the entire
space formed by all 3D rotations can be compactly repre-
sented as a solid radius-π ball in 3D. For ease of manipula-
tion, we use the minimum cube [−π, π]
3
that encloses the
π-ball as the rotation domain. For the translation part, we
assume the optimal translation must lie within a bounded
cube [−ξ, ξ]
3
which may be readily set by choosing a big
number as ξ. During BnB searches, initial cubes will be
subdivided into smaller sub-cubes C
r
, C
t
using the octree
data-structure and the process is repeated.
Bounding functions. We will present the derivation of
our new bounding functions (for the ICP L
2
-norm metric) in
the next section. Worth mentioning here is a unique feature
of the proposed BnB method. That is, we employ, as a sub-
routine, the conventional ICP algorithm in the BnB search
computation. This way, our method enjoys both the effi-
ciency provided by the local ICP search, and the optimality
guaranteed by the BnB search.
5. Derive New Bounding Functions
As for any BnB method, finding quality bounds is the key
to success. In our method, we need to find the bounds of the
particular type of L
2
-norm error function used in ICP within
a domain C
r
× C
t
. Next, we will introduce the concept
of uncertainty radius as a mathematical preparation, then
derive our new bounds based on it.
5.1. Uncertainty radius
The intuition behind the concept of uncertainty radius
is: we want to examine, if we perturb a 3D rigid motion
with rotation r ∈ C
r
and/or translation t ∈ C
t
applied to a
3D point x, what the uncertainty region of the transformed
point will be. We aim to find a ball enclosing such an un-
certainty region. Our first result is as follows.
X
Y
Z
∠
max
(a) Rotation uncertainty radius (b) Translation uncertainty radius
Figure 1. Uncertainty radii at a point. Left: rotation uncertainty
ball for C
r
(in red) with center R
r
0
x (blue dot) and radius γ
r
.
Right: translation uncertainty ball for C
t
(in red) with center x +
t
0
(blue dot) and radius γ
t
. In both diagrams, the uncertainty balls
enclose the range of R
r
x or x + t (in green).
Result 1. (Rotation uncertainty radius) Given a 3D point
x. For a rotation cube C
r
of half side-length σ
r
with r
0
as
the center, examining the maximum distance from R
r
x to
R
r
0
x, we have
kR
r
x − R
r
0
xk6 2 sin(min(
√
3σ
r
/2, π/2))kxk
.
=γ
r
. (3)
Proof. kR
r
x − R
r
0
xk
=2 sin(∠(R
r
x, R
r
0
x)/2)kxk
62 sin(min(∠(R
r
, R
r
0
)/2, π/2))kxk
62 sin(min(kr − r
0
k/2, π/2))kxk
62 sin(min(
√
3σ
r
/2, π/2))kxk.
The first, and the second inequalities above, are based on
Lemma 1, Lemma 2 of paper [16], respectively. For con-
venience, we summarize both Lemmas in a (single) result
shown below.
Result 2. For any vector x, two rotations R
r
and R
r
0
, with
r and r
0
as their angle-axis representations, then we have
∠(R
r
x, R
r
0
x) 6 ∠(R
r
, R
r
0
) 6 kr − r
0
k. (4)
The second inequality in Eq. (4) means that, the angular
distance between two rotations in the underlying manifold,
is less than their vector distance in the angle-axis represen-
tation. We call γ
r
the rotation uncertainty radius. Similarly,
we can derive a translation uncertainty radius γ
t
, for a trans-
lation cube C
t
with half side-length σ
t
centered at t
0
:
k(x + t) − (x + t
0
)k = kt − t
0
k 6
√
3σ
t
.
= γ
t
. (5)
See Fig. 1 for illustrations of γ
r
and γ
t
. Both uncertainty
radii are used in deriving the lower bound for our method.
Note that γ
r
is point-dependent, therefore γ
r
i
refers to the
rotation uncertainty radius at x
i
.
3
Figure 2. An illustrative figure for the obtained lower bound. It is
clear that a ≤ b ≤ c while a = e
i
and c = e
i
(R
r
, t). See text for
details.
5.2. Bounding the L
2
error
Given a 3D data point x
i
, a rotation cube C
r
centered at
r
0
and a translation cube C
t
centered at t
0
, an upper bound
for the per-pixel residual error e
i
(R, t) within the cubes can
be trivially found as
e
i
.
= e
i
(R
r
0
, t
0
) > min
∀(r,t)∈(C
r
×C
t
)
e
i
(R
r
, t). (6)
Finding a suitable lower bound for this L
2
residual er-
ror appears to be a harder task, especially considering the
domain is in SE(3). However, below we will show how a
lower bound can be found, using the concept of uncertainty
radius.
The point y
j
∗
∈ Y is closest to (R
r
x
i
+ t) as in Eq. (2).
Let y
j
∗
0
be the closest point to R
r
0
x
i
+ t
0
. Observe that,
∀(r, t) ∈ (C
r
× C
t
),
e
i
(R
r
, t)
=kR
r
x
i
+t−y
j
∗
k
=k(R
r
0
x
i
+t
0
−y
j
∗
)+ (R
r
x
i
−R
r
0
x
i
)+(t−t
0
)k
> kR
r
0
x
i
+t
0
−y
j
∗
k−(kR
r
x
i
−R
r
0
x
i
k+kt−t
0
k) (7)
> kR
r
0
x
i
+t
0
−y
j
∗
k−(γ
r
i
+γ
t
) (8)
> kR
r
0
x
i
+t
0
−y
j
∗
0
k−(γ
r
i
+γ
t
) (9)
=e
i
(R
r
0
, t
0
)−(γ
r
i
+γ
t
),
where Eq. (7) follows from the (reverse) triangle inequali-
ty, Eq. (8) is based on the uncertainty radii in Eq. (3) and
Eq. (5), and Eq. (9) is based on the closest-point definition.
Note that, y
j
∗
is not a fixed point, but changes dynamically
as a function of (r, t) as defined in Eq. (2). Such a closest-
point mechanism is consistent with standard ICP.
Now we have reached a lower bound of the per-point
residual for C
r
× C
t
as
e
i
.
=max(e
i
(R
r
0
, t
0
)−(γ
r
i
+γ
t
), 0)6 min
∀(r,t)∈(C
r
×C
t
)
e
i
(R
r
, t).
(10)
The geometric explanation of this lower bound is as fol-
lows. Since y
j
∗
0
is the closest point to the center R
r
0
x
i
+t
0
of the uncertainty ball with radius γ = γ
r
i
+γ
t
, it is also the
closest point to (the surface of) the ball and e
i
is the closest
distance between pointset Y and the ball. Thus no matter
where the transformed data point R
r
x
i
+ t lies inside the
ball, its closest distance to pointset Y will be no less than
e
i
. See Fig. 2 for an illustration.
Summing up the squared upper bounds and lower bounds
of the per-point residuals in Eq. (6) and Eq. (10) for all the
M points, we get the important result below.
Result 3. (Bounds of the L
2
error) For a 3D motion domain
C
r
×C
t
centered at (r
0
, t
0
) with uncertainty radii γ
r
i
s and
γ
t
, the upper bound E and the lower bound E of the optimal
L
2
registration error E
∗
can be chosen as
E
.
=
M
X
i=1
e
i
2
=
M
X
i=1
e
i
(R
r
0
,t
0
)
2
, (11)
E
.
=
M
X
i=1
e
i
2
=
M
X
i=1
max(e
i
(R
r
0
,t
0
)−(γ
r
i
+γ
t
), 0)
2
.(12)
This result gives both the upper bound and lower bound
of the registration error, based on which we developed our
Go-ICP algorithm.
6. The Go-ICP Algorithm
6.1. Nested BnBs
Instead of searching the 6D space of SE(3) with a
single BnB which would be less efficient, we propose to
use a nested BnB search structure. An outer BnB search-
es the rotation space of SO(3), while solving the bounds
and corresponding optimal translation by calling an in-
ner translational BnB. The bounds for both BnB algo-
rithms can be readily derived according to Sec. 5.2 and
will be briefly described as follows. In the outer rota-
tion BnB, for a cube C
r
the bounds of the registration er-
ror can be chosen as E
r
= min
∀t∈C
t
P
i
e
i
(R
r
0
, t)
2
and
E
r
= min
∀t∈C
t
P
i
max(e
i
(R
r
0
, t) − γ
r
i
, 0)
2
where C
t
is the initial translation cube. To solve E
r
with the inner
translation BnB, the bounds for a translation cube C
t
can
be chosen as E
t
=
P
i
max(e
i
(R
r
0
, t
0
) − γ
r
i
, 0)
2
and
E
t
=
P
i
max(e
i
(R
r
0
, t
0
) − (γ
r
i
+ γ
t
), 0)
2
. By setting
all the rotation uncertainty radii γ
r
i
to be zero in E
t
and E
t
above, we get the translation BnB to solve E
r
.
A detailed description is given in Algorithm 1 (Go-
ICP – the Main Algorithm) and Algorithm 2 (the Trans-
lation BnB). The nested BnB structure can be clearly
seen: the outer BnB in Algorithm 1 calls the inner BnB
in Algorithm 2.
Search strategy and stop criterion. In both BnBs, we
perform best-first-search strategy. Specifically, each of the
BnBs maintains a priority queue Q
r
, Q
t
, respectively. The
priority of the cubes in the queue is opposite to their lower
bounds, which means that the BnBs always explore the cube
4
Algorithm 1: Go-ICP – the Main Algorithm: BnB
search for optimal registration in SE(3)
Input: Data and model points; threshold ; initial cubes C
r
, C
t
.
Output: Globally minimal error E
∗
and corresponding r
∗
, t
∗
.
1 Put C
r
into priority queue Q
r
. Set E
∗
= +∞.
2 loop
3 Read out a cube with lowest lower-bound E
r
from Q
r
.
4 Quit the loop if E
∗
− E
r
< .
5 Divide the cube into 8 sub-cubes.
6 foreach sub-cube C
r
do
7 Compute E
r
for C
r
and corresponding optimal t by
calling Algorithm 2 with r
0
, zero uncertainty radius, E
∗
.
8 if E
r
< E
∗
then
9 Run ICP with initialization of (r
0
, t).
10 Update E
∗
, r
∗
, and t
∗
with the results of ICP.
11 end
12 Compute E
r
for C
r
by calling Algorithm 2 with r
0
, γ
r
and E
∗
.
13 Discard C
r
if E
r
> E
∗
; otherwise put it into Q
r
.
14 end
15 end
with smallest lower bound in the queue. Once the difference
between so-far-the-best error E
∗
and the lower bound E of
current cube is less than a threshold , the BnB stops.
Proof of convergence. The convergence for both of the
algorithms is provable. All we need to do is to show that, as
the algorithm iterates, the gap between the lower bound and
the upper bound converges uniformly to zero. This is easy
to see, as when the side-lengths of all cubes asymptotically
diminish to zero, the gap between the two bounds, i.e. the
uncertainty radii in Eq. (3) and Eq. (5), will be zero too.
Initial error for inner BnB. Here we give some details
regarding the initial error setting in Line 1 of Algorithm 2.
To speed up the computation of inner BnB, we set the initial
E
∗
t
to be E
∗
without loss of globally optimal registration
based on the following insight. If E
r
= E
∗
t
≥E
∗
, then E
∗
will not be updated. If E
r
= E
∗
t
≥ E
∗
, then C
r
contains
no better solution. In other words, if the error E
∗
t
returned
by the inner BnB is greater than or equal to so-far-the-best
error E
∗
in the outer BnB, it makes no contribution.
6.2. Integration with local ICP
Lines 9-10 of Algorithm 1 describe our upper-bound re-
finement procedure based on a standard local ICP. This pro-
cedure is done as follows. Whenever the outer BnB finds
a cube C
r
which has an upper-bound lower than the so-far-
the-best function value, it will then call the conventional
ICP to start over, from the center of C
r
and corresponding
t
∗
as the new initial transformation. This helps ICP jump
out of the previous local minima. Once ICP converges, it
will arrive at a new local minimum which has a lower func-
tion value. This new local minimum is used to update the
upper bound.
Algorithm 2: BnB search for optimal translation given
rotation
Input: Data and model points; threshold ; initial cube C
t
; rotation
r
0
; rotation uncertainty radii γ
r
, so far the best error E
∗
.
Output: Minimal error E
∗
t
and corresponding t
∗
.
1 Put C
t
into priority queue Q
t
. Set E
∗
t
= E
∗
.
2 loop
3 Read out a cube with lowest lower-bound E
t
from Q
t
.
4 Quit the loop if E
∗
t
− E
t
< .
5 Divide the cube into 8 sub-cubes.
6 foreach sub-cube C
t
do
7 Compute E
t
for C
t
with r
0
,t
0
and γ
r
.
8 if E
t
< E
∗
t
then
9 Update E
∗
t
= E
t
, t
∗
= t
0
.
10 end
11 Compute E
t
for C
t
with r
0
,t
0
,γ
r
,γ
t
.
12 Discard C
t
if E
t
> E
∗
t
; otherwise put it into Q
t
.
13 end
14 end
ICP
ICP
ICP
BnB
BnB
Figure 3. Left: BnB and ICP collaboratively update the upper
bounds during the search process. Right: with the guidance of
BnB, ICP only explores un-discarded promising cubes with small
lower bounds marked up by BnB.
Figure 3 (left) illustrates the collaborative relationship
between ICP and BnB. Under the guidance of the global
BnB, the local ICP seems to have a strong “sense of direc-
tion”. Instead of exploring the domain blindly, ICP con-
verges into local minima one by one with each local mini-
mum having lower error than the previous one, and reaches
the global minimum in the end. Since ICP monotonically
decreases current-best error E
∗
(cf. [5]), all points (transfor-
mation parameter r, t) in its search path should have error
lower than E
∗
, which means that lower bounds of the cubes
containing these points should be lower than E
∗
. Thus the
search path of the local ICP is entirely confined to the un-
discarded, promising cubes with small lower bounds, as il-
lustrated in Fig. 3 (right).
This way, both the global BnB search and the local ICP
search are intimately integrated in our method. The former
not only helps the latter to jump out of local minima, but al-
so provides a guidance for the latter’s next search; the latter
accelerates the former’s convergence by refining the upper
bound, hence improves the overall efficiency.
5
