It System

Increasing Robustness in Self-Localization and Pose Estimation

Ross J. Micheals Terrance E. Boult

VAST Lab, Lehigh University, Bethlehem, PA 18015

abqo04@r.postjobfree.com, abqo04@r.postjobfree.com

Abstract nate systems. In this paper, we will present a new closed-

form quaternion based AO algorithm particularly well-suited

Ideally, an algorithm used for either self localization or for dealing with mismatches and outliers. Through simula-

pose estimation would be both ef cient and robust. Many re- tion, we compare the new methods with previous research.

searchers have based their techniques on the absolute orienta-

2 Representations

tion research of B. K. P. Horn. As will be shown in this paper,

Choosing a representation for the scalar and translational com-

while Horn s method performs well with an additive Gaus-

ponents is simple we use a scalar and a 3D vector respec-

sian noise of large variance, mismatches and outliers have a

tively. Rotations, however, have many different representa-

more profound effect. In this paper, the authors develop a new

tions (Horn reminds us both in [7] and [8] that Korn and Korn

closed-form solution to the absolute orientation problem, fea-

[11] mention ve different methods alone). Aside from Euler

turing techniques speci cally designed for increasing the ro-

angles, the rotation matrixes and quaternions are the represen-

bustness during the critical rotation determination stage. We

tations most often used in the computer vision and graphics

also include a comparative analysis of the various strengths

community (see [3] and [6] for background), but which of the

and weaknesses of both Horn s and the new techniques.

many representations is best suited for the AO problem?

Keywords: absolute orientation, self-localization, pose esti-

Like the research of Horn, we are primarily concerned with

mation

the quaternion representation of rotations. This bypasses the

following disadvantages of using rotation matrixes:

1 Introduction and Background There is no obvious relationship between the individual

At the core of many pose estimations and self-localizations,

elements of a rotation matrix and the axis & angle of rota-

lies a variation of the absolute orientation (AO) problem. His-

tion. This lack of an intuitive understanding makes both

torically, and throughout different elds, the problem takes

comparison and visualization dif cult. Therefore, rotation

on both different constraints and names. But, regardless of

matrixes are also poor candidates for reconciliation via av-

whether the problem to be solved is 3D location parameter

eraging multiple results from subsampling noisy data.

estimation [16], a form of hand-eye calibration, or (the au-

thors favorite nomenclature) the roto-translation problem Traditional rotation matrixes and Euler angles make in-

[15], the essence remains the same. Given two sets of corre- terpolation dif cult. Computer vision, graphics, robotics,

sponding 3D points, what is the transform that relates them? and manufacturing applications often have better results

To the best of the authors knowledge, the rst published when using quaternions [10, 14].

least-square minimization solution to the absolute orientation The normalization required to produce a numerically cor-

problem can be accredited to Fernando Sans` [15]. Horn re-

o rect rotation matrix is computationally expensive. Orthog-

discovered the same method over 14 years later, in 1987 [7]. onalization procedures, such as Gram-Schmidt, introduce

Although Sans` s paper outdates Horn s, the latter is the sig-

o unnecessary opportunities for errors to propagate and can

ni cantly more popular reference. Both authors rst reduce require signi cant amounts of computation.

the problem to an Eigen-decomposition of the same ma-

trix, and then suggest the use of Jacobi s method. Unlike San- Unit quaternions are well-suited for representing rotations

s`, Horn mentions that the Eigenvectors and Eigenvalues of

o for many situations:

the matrix may be found via the roots of a quartic. Since the Because of the direct relationship between the angle & ax-

roots of a quartic may be found in closed form via Ferrari s

is of a rotation and the quaternion that represents it, groups

identities, Horn s completes his closed-form. Horn suggests,

of quaternions are easy to compare and reconcile.

however, that due to the inherent instability of the closed-form

solution for quartics, in implementation, the decomposition Normalization of a quaternion is relatively fast, requiring

should be performed via Jacobi. (See [12] for more history on signi cantly less work than an orthogonalization proce-

the AO problem.) dure.

As de ned in this paper, we consider the absolute orien-

Quaternions do have features that can be disadvantages:

tation problem to be determining, from a collection of corre-

sponding point pairs, the translational, rotational, and scalar- For every rotation, there exists two canonical quaternion

ing correspondence between two different Cartesian coordi-

representations; i.e. a rotation through is equivalent to

one through . Although this may appear to be a dis- Horn [7] provides a more formal treatment of precisely why

advantage, we will later exploit this dual nature of quater- Equations (3) and (4) are desirable estimates in the absence of

nions. mismatches and outliers.

Despite their correlation to the axis and angle of a rota- 3.3 Solving the Rotational Component

tion, the combination of a quaternion s four-dimensional Solving for the rotation is the crux of the new research. For

nature with its unit constraint can be dif cult to visual- the sake of simplicity, in this section we assume that our input

ize. Quaternion visualization has a long history: from data has already been normalized according to the scalar and

1866 desk and table analogy [4] to Hart s [5] more re- translational components.

cent (1994) use of computer graphics to explore the more Consider a set of three-dimensional column vectors (per-

common belt analogy. haps representing points on a rigid body) r,r,, "! ut x8

9

9y

r where r and . Let

!6 v 3 4 v 3 1 v 0 wQv&

70 0 )

Quaternions give no preference to rotations that maintain $ D

c

represent the corresponding set of points after undergoing a

an upright position. Therefore, in the domain of com-

rotation of degrees around the unit axis u (point r rotates v

puter graphics, quaternions are not preferred for interpo- H

G

to s ). v

lating between virtual camera orientations [3].

For convenience, vectors will be freely interchanged with

3 Absolute Orientation Formulae their quaternion counterparts (i.e. r ). U S (@ 0

P

Consider a set of three dimensional column vectors (per-

3.4 Outliers and Mismatches

haps representing points on a rigid body or geolocated land-

A particular advantage of using the aforementioned methods

marks) rr r where r and "26' 3 54' 3 21' 0

70 0 )

%# CB@8

$ 9A 9 is that neither require point matches. In other words, even if

. Let represent the corresponding set of points

D all of the correspondences are incorrect, then the translation-

after undergoing a translation by, a uniform scaling by, E F al and scalar components will not be effected. Determining

and a rotation by degrees about the unit axis u. If u

G H U TSRQI

H GP the rotation, however, requires correspondences, even if they

represents the matrix form of the rotational component as a

XWV8

9A 9 are implicit ones. The issues associated with this indepen-

function of and u, then for all, H

G dence from correspondences will resurface when we consider

' U H SYQ"B ' the metrics to be used for evaluating AO algorithms.

s ur t (1)

GP I F

In the presence of outliers, however, Equations (3) and (4)

Formally, the absolute orientation problem is: Given and can rapidly deteriorate. It is the profound effect of outlier-, recover, t,, and u. Like previous research, we treat the

D F G H s that motivated Fischler and Bolles to develop their land-

absolute orientation in three separate stages: by solving, t, F mark RANSAC (Random Sample Consensus) framework [2].

and (, u) independently. The focus of this research is on the

HG RANSAC instantiates not just one, but many solution mod-

most dif cult component, determining the rotation. There- els through the use of repeated subsampling. By seeking a

fore, we will only brie y summarize the common methods for majority of points consistent with their models, Fishler and

nding the scalar and translational components. Bolles were able to use RANSAC to increase the robustness

of a classic photogrammetric localization problem. Later, our

3.1 Solving the Translational Component

simulations will show how outliers may cause a similarly cor-

To determine the translational component, t, we look to the

rupting effect.

centroids. Given the respective centroids of each point set,

t is simply the difference between the two vectors. Simply 3.5 The New Closed-Form

calculate the centroids For the sake of deriving the new form, we assume that our

input data ( and ) is noiseless, complete, and correctly

D

' ed'B # a` ' ed'B # a` (2)

r r and s s

cb cb matched; in later sections we examine the effects of noise.

If is the unit quaternion

then the translational component t can be approximated by

simply

U H Y T T Rh A u (5)

U GP F U GP P

!` `

t s r (3)

it can be shown that the post-rotation vector, s, is uniquely '

determined by the quaternion multiplication

3.2 Solving the Scalar Component

In the multi-stage approach, obtaining an estimate for the s- U ' S P aU ' S P

s r (6)

calar component is also relatively trivial. Suppose that given

a point set, we sum all of the distances from each point to the

where r is a quaternion with a scalar component of zero

U ' RP

centroid of the set. If our point set remains rigid, we expect

and vector component r, and is the conjugate of quater-

'

this total to be invariant under the effects of translation and ro-

nion .

tation. Therefore, we look to the ratio of the average distance

Fully expanding (,, and ) algebraically yields E-

'4 '1 '6

to the centroid for an estimate of : F quations (7) (8) of Figure 1. From these equations we can see

#f

s ` that the components of the sought-after quaternion can not be

r r

h 5d#' b CF

g

g (4)

expressed as a direct linear combination of the input points.

qrpg ` X ed' b s s

g

i

However, if we consider the product of two of the quaternion Regardless, brie y consider a naive implementation of E-

components, we can establish a linear relationship. quation (10). If we assume that a dot product requires three

Suppose we choose just three pairs of corresponding points multiplications and one addition, a cross product requires six

from and : r s, r s, and r s . We can then multiplications and three additions; then an implementation

e T P ! d P D

U

U ef Tf P

U h

build the system of Equation (10) of Figure 1. with no partial evaluation can nd an estimate of the com-

These ten equations fully constrain the rotation between ponent with just 45 multiplications, 25 additions, and 1 divi-

and . It is no surprise that at least three points were needed. sion just 71 FLOPS. A highly optimized version that solves

D 8

The inclusion of the unit quaternion identity may not for all of the quadratic components can be performed in 123

gg

be as obvious, however. In fact, it was the inclusion of the unit FLOPS and 37 assignments. (See [12] for implementation de-

quaternion constraint that allows Maple, with some creative tails.) An optimized method that calculates all ten components

coaxing, to nd closed-form solutions for all ten component (which may not always be necessary) can be accomplished

combinations. with 266 FLOPS.

Although it is not the main focus of this paper, it should be

3.6 Increasing Stability and Degenerate Cases

noted that this system can easily be extended to include the de-

Clearly, as the volume of the parallelepiped spanned by r,

termination of a translational component. The three addition-

r, and r approaches zero (all three points and the origin are

f

al degrees of freedom can be constrained with a fourth point

coplanar), the solution degenerates. However, there is a sim-

e8

g

pair. Despite its larger size, closed-form solutions for all

ple way to increase the stability of the new form. If r r r

8 g df ~

unknowns ( quaternion products and degrees of transla-

is greater than s s s, then use the formulations as writ-

"f T T ~

tion) can be found from the new system. Details are provided

ten. However if s s s is greater, then the arguments to

f ~

in [12].

each component function should be s s s r r r in- Uf f P

After signi cant algebraic manipulation of Maple s out-

stead of r r r s s s . The conjugate of this result is

Uf f P

put, we nd that all products of two components of a (quater-

the sought-after estimate.

nion) rotation can be expressed as a linear combination of

While three point pairs are suf cient for the non-translation

triple product ratios. In Equations (12) (16), the componen-

case, four pairs of non-coplanar points are required for both

t products are expressed as functions of the original input

translation and rotation.

points.

! " ! ! "j" "j" ! "

l k k i l h k h i h 3.6.1 Resolving the Overdetermined System

The six component products,,,,,

! !

l i Since not all ten components are required to calculate the

are all of the form of Equation (16) where and are

m

n

sought-after quaternion, we are left with many possible re-

elements of and . For instance, if and

sp3p3 d

$ r q o vum

n t

w@m i h

, then the component considered is, substituting construction techniques. We can now use the dual nature of

yxn

o !"" "!

{h z

i quaternions as an advantage; any one component can be arbi-

the matrix where appropriate. Note that .

trarily xed as positive, and the sign of other components can

3.5.1 Computational Expense

be adjusted accordingly. The resulting rotation is not effect-

A direct implementation of the above equations can hardly be

ed. Since it has a direct correlation to the angle of rotation, it

considered an ef cient one. Speci cally, an actual implemen- e

h

makes the most intuitive sense to x the scalar component

tation should not include any matrix multiplications the

positive.

matrixes of Equation (11) all reduce to element swaps and/or

During early experimentation, it was discovered that out of

negations and have been included for notational compactness

8 k i h l

all the products,,,, and are the least susceptible

only. Common subexpression elimination also yields signi -

to noise. Using the stable products as an anchor, and the less

cant speedup.

stable products for determining the proper sign, the rotation

As computing power continues its exponential growth a-

can be reconstructed with the algorithm shown in Figure 2.

long Moore s price-performance curve, the number of com-

In some cases, the size of the input set is minimal. In [9],

putations required by an algorithm is less important than in

only a handful of points (three to ve) are used for localizing a

previous years. Even a typical desktop PC can run proverbial

mobile platform using geolocated landmarks. Using a greater

circles around the most powerful workstations of yesteryear.

number of points, however, has the potential for increasing

Horn s absolute orientation algorithm is a perfect example of

many algorithms accuracy.

how inexpensive cycles can improve algorithms. As disclosed

Fortunately, there are many ways to generalize the absolute

in Section 1, even though the closed-form algorithm as de-

orientation problem to points. For instance, one may seek

scribed in [7] can be implemented in pure form, it usually is

the rotation that:

not. The high availability of mathematical libraries that rapid-

ly perform iterative procedures have made it possible to sub- minimizes the sum of the squared distances between the

stitute potentially unstable algorithms with more robust ones. pre and post-rotated points

Still, there is no substitute for high-ef ciency algorithms; maximizes the alignment of the data, treated as vectors,

embedded systems, robotics, RANSAC-based algorithms, and

before and after the rotation

real-time systems all depend on rapid turn-around. Unless an

minimizes the sum of squared errors in any linear formu-

algorithm is particularly compact, RANSAC algorithms espe-

lation that produces a solution

cially can not afford to repeat an iterative numerical method

hundreds or thousands of times.

of triple products. Triple products are denoted with angle brackets.

post-rotation points as a linear combination of products of quaternion components. Equations 11 16: Solutions to the system as ratios

Figure 1. Equations 7 9: Fully expanded expressions for the elements of r after a quaternion rotation about . Equation 10: The

ef T d ~ rrr

(16)

s rr sr r srr

}z }z }z

f s s ~ f ~ f s R~ }

~

f rrr

(15) "f s d ~ "f T sT ~ df T d R~ "f ~ l

s rr sr r srr rrr

l z jl z jl z

l l l

~

f

rrr

(14) "f s T ~ ef T d ~ "f T d R~ "f ~ k

s rr sr r srr rrr

k z !k z k z

k k k

rrr

(13) f ~ f ef ~ T ~ f R~ f ~ i

s rr sr r srr rrr

iei z i ei z i "i z

rrr

(12)

~ h

f ~ f "f T f ~ ~ f

rrs rsr

srr rrr

~

u

u8 ei z l l k

8 !k z u 8 ei z

u 8

8

8

(11) l k i

8 h z 8 h z 8 8 {h z

8

8

8

8 z 8 z

l jl !k z k i "i

8

8 8 8 8 8

1

6 6 6 6 1 j 4 j j 4 j

4

s

4 s 4 s 4 s 6 sj 1 sj 1 j

6 j

Q1 s 1 s 1 s 6 sj sjw j

1 s sj

1

1

Q6 u6 6 j 4 j

(10) 6 j 4 j

6

4 4 4

j 1 j 1 jj

6 jj

4 4

Q1 1 1 6 j

j 4 j

1 6 jj

Q6 1 1

! u6 6 !

!j 4 j

6 j 4 j

4 4 4

6 j 1 j 1 !j

6 !j

4

3

Q1 1 1 6 j

4 j 4 j

3 3 1 6 !j

0 0 0 0 #

6 e

(9) 0

k h i i l 0 h l

i h k l i i T l k k l i 0 k l 0

4#

(8) l h 0

! "Ti !je"l !d"ek

i h 0 l l 0 k i 0 0

k Tk 0 i k 0 h Tk B

! "Ti T l Tk 0

1#

(7) l h 0

! "dk T k h 0 l i 0 k i 0 0

k i 0 i i 0

!jedl " !dl k l i 0 h i B

4 Absolute Orientation Algorithms

Algorithm Basic Reconstruct-3

Now that the basic formulae have been introduced, we use

Input: Six vectors, two groups of three, each respectively rep-

them as a base for developing new algorithms. Since in Horn s

resenting points before r s t and after r s t a rotation

e R R

solution there is a direct map from his formulation to an algo-

through unkn

Contact this candidate