Modifications to Pose Estimation Algorithmsthat Allow for Near-Planar Feature Points

Samuel A. Schweighart1, Piero Miotto2, Matthew Hale3 and Leena Singh4

Charles Stark Draper Laboratory, Cambridge, MA, 02472, USA

In the process of developing terrain relative navigation algorithms, different poseestimation algorithms were researched, tested and evaluated. The two best algorithms thatfit our purposes were by Fiore, and by Ansar and Daniilidis. However, these algorithmsrequired modifications for our application. Specifically, Fiore’s algorithm failed to handlefeature points that are nearly planar. A modification to the algorithm presented hereinallows for the seamless handling of planar, near-planar, and non-planar feature pointswithout any additional computational workload. Ansar and Daniilidis’ algorithmsporadically produced large errors in the presence of noise. Our modifications minimize thistendency with only a slight increase in the computational workload.

I. IntroductionN the process of developing terrain relative navigation (TRN) algorithms for planetary landing, several pose

estimation algorithms were investigated. These algorithms take the identified features on the surface of the planet,

and the corresponding features on the image and determine the position and attitude of the vehicle. Most pose

estimation algorithm attempt to determine the camera matrix. Some methods are iterative and produce the ‘best’

solution, while others are linear and fast, but don’t produce as accurate results. Two methods, one by Fiore [1] and a

second by Ansar and Daniilidis [2], approach the problem in a different way. Neither algorithms attempt to find the

camera matrix but instead find the ‘depths’ of the feature points. With the depth information, the features in the 2D

camera image are turned into 3D points in the camera’s frame of reference. With a simple absolute orientation

algorithm, it is now possible to recover the position and attitude of the vehicle [3].

While implementing the two algorithms, problems developed. The algorithm given in Fiore’s paper works very

well if the feature points do not lie in a plane. (If the feature points do lie in a plane, a second algorithm is presented

that handles that specific case.) However, the algorithm does not work well when the points are ‘near-planar’. Since

planetary features look almost planar from orbit, but not quite, they fall in this range where the algorithm breaks

down. This paper address that problem and provides a modification to the algorithm that seamlessly handles non-

1 Terrafugia Inc.2 C. S. Draper Laboratory3 US Air Force.4 C. S. Draper Laboratory

I

AIAA Guidance, Navigation and Control Conference and Exhibit18 - 21 August 2008, Honolulu, Hawaii

AIAA 2008-7478

Copyright © 2008 by the American Institute of Aeronautics and Astronautics, Inc. All rights reserved.

planar to planar points while maintaining the high accuracy of the solution without any additional computational

workload.

The algorithm by Ansar/Daniilidis is not affected if the feature points lie in a plane, are near-planar or not in a

plane at all. However, when implementing the algorithm, sporadic ‘outliers’ or solutions with large error were

noticed. Analysis into the algorithm determined the problem and the modification are presented herein. This

modification removes these outliers and improves the overall accuracy of the algorithm for only a slight increase in

computational workload.

II. Fiore Algorithm

In 2001, Paul Fiore published ‘Efficient Linear Solution of Exterior Orientation’ in which he presented an

algorithm for solving the pose estimation problem (also known as the exterior orientation problem). This algorithm

has very good accuracy and speed when compared with current algorithms. Fiore accomplished this by establishing

a set of linear equations which are solved for the depth information of the feature points (up to scale) instead of the

camera matrix directly. The main benefit of this method is that it is non-iterative, linear, with matrices on the order

of n X n. This makes it very computationally efficient even for many feature points. One drawback is that the

algorithm, because of its linear nature, requires a minimum of 6 points. The main problem encountered with the

algorithm is that if the feature points are nearly planar, the algorithm breaks down in the presence of noise. Fiore

presents a modified version of the algorithm to handle the completely planar case, but neither algorithm is successful

when the points are almost in a plane. The following section will present a different look at the derivation of the

Fiore method and the modification to the algorithm that will allow for the seamless handling of planar, near-planar

and non-planar points.

It should be first noted, that an attempt to preserve the same notation between Fiore and this paper was made,

(and between Ansar/Daniilidis and our paper), but it was not possible to make all three consistent. Therefore

{ }XxXAFcs ii ,,,,,, 33

rrrrin this paper correspond to{ }DaxPsl ii

T ,,,,,,rrr

α in Fiore’s paper, respectively.

A. A New Derivation of the Algorithm

The algorithm attempts to solve the following equation,

( )refrefi

camref

camii pxcRXs

vvv−= (1)

where Tii

cami vuX ),(=

vare the positions of the points in the image, cam

refR rotation matrix between the camera and

reference frame, Tiii

refi zyxx ),,(=r

are the 3D positions of the points in the reference frame, refpr

is the location

of the camera, c is an unknown scalar, and is are the distances from the image plane to the points. Equation 1 can

easily be manipulated to the following form, where F is what is commonly known as the ‘camera matrix’.

( )( )14

431

|1 x

refi

xF

refcamref

camrefi

i

ixpRRcv

us

⋅−⋅⋅=

v

444 3444 21v

(2)

The equation is re-written so that the vector sr

is isolated.

33

34

33

32

31

222

111

2

1

..11

FAc

FFFF

zyxzyx

css r

MMMMM≡

=

22

24

23

22

21

2

1

..1

1

2

2

2

2

2

2

1

1

1

1

1

1

FAc

FFFF

css

vx

vx

vx

vz

vy

vx

r

MMMMM≡

=

11

14

13

12

11

2

1

..1

1

2

2

2

2

2

2

1

1

1

1

1

1

FAc

FFFF

css

ux

ux

ux

uz

uy

ux

r

MMMMM≡

=

(3)

From equation 3, we quickly get to the heart of the algorithm. sr

lies in the intersection of the three column spaces

iA , i.e. )( 321 AAAs IIr∈ . Unless the feature points are in a critical configuration [1,2], there is only one

solution or intersection. This intersection is found by recognizing that the intersection lies in the compliment

(nullspace) of the union of the compliment (nullspace) of iA , i.e. )( 321 AAAs UUr∈ . Define

≡3W nullspace )( 3TA , such that the columns of 3W and the columns of 3A completely span nℜ . 1W and 2W can

also be created by taking the nullspace of 21 and AA but it is more efficient to do the following. Let jiw , be the

components of 3W ,

=

=

−

−

−

−

nnnnn

n

nnnnn

n

vwvw

vwvwW

uwuw

uwuwW

4,1,

14,111,1

2

4,1,

14,111,1

1L

MOM

L

LMOM

L

(4)

This allows for only one nullspace operation to be needed and avoids singularity conditions if 0or =ii vu . The

solution sr

lies in the nullspace of the union of iW , i.e. )( 321 WWWs UUr∈ . If we define L as the union of iW

0

3

2

1

=⋅

≡⋅ s

WWW

sLT

T

T

rr(5)

then sr

is found by simply taking the nullspace of L. Since a solution exists, L is guaranteed to have a nullspace of at

least 1 if there is no noise. In the presence of noise, L is probably full rank and the SVD of L can be taken to find the

solution. The matrix L is (3N-12) X N. If N is 6, then L is square. If L is less than 6, then there is a nullspace of at

least rank 2 and the solution is not isolated. This sets the minimum number of points to be 6.

To prevent the need of taking an SVD of a 4 X N matrix and then a (3N-12) X N matrix to obtain the solution,

Fiore simplifies the problem in two ways. First, instead of solving for sr

directly, 3Fr

is found. This is accomplished

by using the relationship 33.FAsrr = (up to a scale factor). Since s

ris now lies completely in the space spanned

by 3A , we don’t need to force it to lie in the nullspace of 3W . Equation 5 is now written as

0332

13 =⋅

≡ FA

WW

FC T

T rr(6)

C is (2N-8) X 4. To save computational costs, the nullspace of C is found by looking at the eigenvector of CCT

with the smallest eigenvalue. Fiore also provides a method of calculating CCT using a QR procedure instead of

calculating the nullspace of 3A . Therefore, a solution is found with a QR procedure and an eigenvector calculation

instead of two SVD calculations which is stated to save computational resources. Both methods produce identical

results (within numerical error). Once 3Fr

is found, the depths (up to scale) can be found from 33.FAsrr = .

B. Near-Planar Points

We now move on to the case of ‘near-planar’ points. Where there is no noise on the system, the algorithm works

all the up to the point where the points become planar. However, if there is noise on the system, then the algorithm

begins to have significant errors when the points are near-planar. The following section will look more closely at

why the algorithm fails and present modifications that allow it to provide accurate solutions in this range.

If the points aren’t completely in a plane, but close, then the rank of iA is ‘almost’ 3. Define iA~

as the matrix

which contains the orthonormal basis of the space spanned by the columns of iA . iA~

is N X 4 and can be created by

taking the first 4 columns of the V matrix of the SVD of TiA ( )(SVD T

iT AUDV = ). Let the first column of

iA~

correspond to the largest diagonal of the D matrix, and the 4th column corresponding to the smallest. The

solution can now be written as )~~~

( 321 AAAs IIr∈ and ii FAs

~~ rr = .

As the points become planar, the 4th column of iA~

, becomes less significant to the solution sr

. This is because

the 4th column of iA~

becomes more orthogonal to sr

. This is evident by the fact that the fourth component of iF~r

goes to zero, and the associated diagonal component of the D matrix goes to zero as the points become planar.

Essentially, the solution sr

tends to lie more and more in the intersection of the spaces spanned by the first 3

columns of iA~

and less in the 4th.

Is the fact that the 4th column vector become more orthogonal to the solution, the reason why the algorithm fails

in the presence of noise? The answer is no. The presence of noise causes the direction of each column vector to

change. This will cause the column spaces of iA~

to no longer intersect, and thus the ‘closest’ intersection must be

chosen as the solution. The first three column vectors of iA~

do not change significantly, but the 4th column vector of

iA~

can. Because the 4th column vector is essentially orthogonal to the solution, it’s controlled more by the noise

than the actual solution, and in the presence of noise can vary significantly. The intersection, and solution, still lies

primarily in the first 3 columns of iA~

. The presence of a nearly orthogonal 4th column in a random direction, will

not significantly change the location of this intersection. However, since the solution is the ‘closest’ intersection and

found by a minimum least squares method, the 4th column vector regardless of its orthogonality will have some

impact on the final solution albeit small.

The only way that the 4th columns of iA could cause difficulties is if created a 2nd intersection. Let’s look at the

extreme case where the points are planar and the rank of iA is 3. The direction of the 4th column of iA~

is only a

result of the noise of the system and can be considered random in direction (while remaining in the nullspace of

iA ). For the 4th column of 1

~A to cause problems, it must intersect the columns in 2

~A . That intersection must also

intersect the columns of 3

~A . Because the direction of the 4th column of iA

~is essentially random, the possibility

that there is a second intersection in all three column spaces is very small. Even with this remote possibility, one can

easily check to verify that only one intersection is created by taking the SVD of the L matrix in equation 5,

)(SVD LUDV T = . If there is only one near-zero diagonal in the D matrix, then there is only one intersection,

the solution. If there is more than one near-zero solution, it is probably due to the fact that the points are in or near a

degenerate configuration or that the noise has randomly created a 2nd intersection, but the former is more likely.

There is nothing inherent in the fact that the points are becoming planar, even in the presence of noise, that will

cause the solution of )( 321 AAAs IIr∈ to fail. It is for this reason, the same algorithm can be used for both the

planar and non-planar case.

The reason the Fiore algorithm fails for the near-planar and planar case is in the creation the C matrix. See

Equation 6, reproduced below. Because 3A becomes rank deficient as the points become planar, two possible

solutions for 03 =FCr

exist.

0332

13 =⋅

≡ FA

WW

FC T

T rr(6)

The first solution solves 033 =FAr

, while the second solves for the true solution sFArr

=33. . Since in the presence

of noise, the true solution doesn’t exactly satisfy sFArr

=33. , either solution looks like the correct one. If Equation

6 is solved using the SVD method, )(CSVDUDV T = , then the two possible solutions are associated with the

two near-zero diagonals in the D matrix. Since either solution can have the smallest diagonal, the true solution is

lost.

C. Algorithm Modification

The modification to Fiore’s algorithm that allows it to handle the near-planar and even the planar case without

large errors or scale sensitivity is to either solve equation 5 directly, or to make 3A not rank deficient. The latter is

easily done by replacing 3A with 3

~A in equation 6, and the solution simply becomes 33

~FAsrr = . Because 3

~A is

full rank, there is no solution to 0~

33 =FAr

, and thus only one solution to 03 =FCr

. This modification does not add

any extra computational burden since 3

~A is already being calculated to make the TWW matrix. The final solution

(including Fiore’s simplification for finding CCT ) is

33

~)(

~AXXWWACC TTTT ⊗= (8)

where ⊗ is componentwise multiplication and

TT AAIWW 33

~~−= and ),...,,,( 321 NXXXXX

vvvv= (9)

D. Planar Points

When the points are completely planar, the rank of iA is 3. It makes sense to create iA~

and just remove the 4th

column. Since it is orthogonal to the solution, it adds no information, and only adds error in the presence of noise.

When the column vector is removed from iA~

, it should be added to iW . The L matrix is now (3N-9) X N.

Therefore only 5 points are needed in this situation.

This is the Planar algorithm listed in Fiore’s paper. As will be seen later in the paper, for planar points the

removal of the 4th column helps the accuracy of the solution. It seems logical to extend the planar algorithm for

near-planar points. Unlike the modified algorithm, extending the planar algorithm for use with near-planar points

will eventually fail as the points become planar. However, if the points are very near-planar, the planar algorithm

can have slightly improved accuracy. To use the planar algorithm for very near-planar points, one MUST rescale

the feature points or the planar algorithm will have significant errors, even for very near-planar points.

This is due to the fact that column vectors in iA~

are ‘ranked’ in regard to how well they span the nullspace. If a

vector is in the nullspace, it gets a zero. (The ranking is the diagonals of the D matrix resulting from the SVD of

iA .) Let the feature points be scaled by constant β so that 3A is now

=′

MMMM11

222

111

3 zyxzyx

A ββββββ

(10)

Remember that the equation for a plane is given by 0=+++ dczbyax . When the points are in a plane, the

nullspace is simply Tdcba ),,,( . If the points are near planar, then 0≠+++≡ dczbyaxγ . The magnitude

of γ , is directly related to its ranking or how ‘close’ Tdcba ),,,( is to the nullspace. If the feature points are badly

scaled, dczbyax +++≡ )(βγ , then γ can be large and Tdcba ),,,( is not found.

The planar algorithm turns out to be fairly sensitive to this effect. In fact, anytime the planar algorithm is used,

the points should be scaled properly. This is accomplished by setting

∑=

=n

i

refix

n

1

vβ (11)

It turns out that the original algorithm is also sensitive to scaling effects when the points are near-planar. This is

due to the fact that the same vector Tdcba ),,,( , is the solution to 033 =FAr

that was causing the algorithm to

fail in the near-planar case. By artificially increasing β , one can increase the accuracy of the original algorithm.

However, as the points become more planar, β must increase even more, to the point the problem is very poorly

numerically conditioned.

The modified algorithm is not affected by scale.

E. Weighed Algorithm

In the planar algorithm, because the 4th column is orthogonal to the solution, it is removed from iA~

and added to

iW . In the modified algorithm, the 4th column is kept in iA~

and not added to iW . Is it possible to ‘blend’ the two

algorithms and provide better accuracy? The answer is yes. First, the 4th column of iA~

is kept in iA~

. It is also

added to iW but is scaled by a scalar 10 ≤≤ ω . When ,1=ω the weighted algorithm is the planar method.

When 0=ω , it is the modified method. Note that this method incorporates the planar algorithm, it is critical to

rescale the feature points.

An analytic expression for the weighting factor has not yet been derived. The weighting factor is a function of

how planar the points are (ratio of the max and min diagonals in the D matrix from the SVD of 3A ), the image

noise (ε is the one sigma pixel error), and the location the camera. The following equation works well with the

unknown constant χ determined experimentally.

−=

1,1

4,4,1max(1D

D

εχω ) (12)

F. Comparisons

The following figure compares the four different algorithms: Fiore’s original and planar algorithm, the modified

algorithm, and the weighted algorithm. For this comparison, a camera is placed at a location 1km above the surface.

It has a FOV of approximately 60 degrees, and is looking 30 degrees off nadir. This produces a footprint on the

surface of about 2km X 2km. The feature points are randomly distributed about the footprint, and the height of the

feature points is constrained to a maximum height, h. h essentially controls how planar the points are. The camera is

assumed to be a pin-hole camera with a 1 Mega pixel image. We assume a 1-sigma error of 1 pixel. 1000 different

sets of feature points are generated for every h, and each algorithm is evaluated to determine sr

. For the weighted

algorithm 1.0=χ . An absolute orientation algorithm [3] is used to calculate the estimated position of the camera.

The average error for each h is plotted in Figure 1.

From Figure 1, we see that the modified algorithm, and the original have the same similar statistics when the

points are non-planar. As the points become closer to a plane, (h becomes smaller), the error in Fiore’s algorithm

increases while the error for the modified algorithm remains constant even when points become planar.

Fiore’s planar algorithm performs better than the modified algorithm when the points are planar or very-near

planar. The weighted algorithm successfully blends the two algorithms together and provides increased accuracy for

the very near-planar points.

G. Conclusions

In this paper, modifications to Fiore’s algorithm were presented. The modified algorithm performs better than

the original algorithm when the points are planar and near-planar. It is unaffected by scale or how planar the points

are, all for no additional computational load. The original planar algorithm does perform slightly better than the

modified algorithm when the points are in a plane or very near-planar. A weighted algorithm blends the two together

based on a weighting function. Which algorithm should be used is dependent on the application. If non-planar, near-

planar, and planar points are to be imaged, then it makes sense to use the modified algorithm since this one

algorithm will handle all three cases seamlessly. However, if the increased accuracy provided by the planar

algorithm is necessary, and a robust weighting function is determined, the weighted algorithm can be used.

III. Ansar/Daniilidis

In 2003, Ansar and Daniilidis published Linear Pose Estimation from Points or Lines which proposed an

effective method for solving the pose estimation problem when non-iterative and non-initialized methods are

preferred. Conceptually, this method forms quadratic constraints based upon the geometry of the problem,

linearizes the constraints, and solves them as a system of equations. The geometric constraints allow for solution of

the depth information sr

. The Ansar/Daniilidis algorithm states that it is better in accuracy than Fiore, and works

for as little as 4 feature points. It is also invariant to whether or not the feature points lie in a plane. The downside

to the Ansar/Daniilidis algorithm is that it is relatively slow, especially for many points. The matrices involved

scale on the order of 3n .

Position Error in km

Figure 1: Algorithm Comparison

Original Alg.Modified Alg.Planar Alg.Weighted Alg.

h in km

Position Error in km

h in km

The following section briefly goes over the Ansar/Daniilidis method in order to explain the modifications and

additions to the algorithm. Please refer to [2] for a complete and well written explanation of the method. Once

again we have had to change notation in our paper. Therefore { }iXcsrr

,, in this paper correspond to{ }ptrr

,, ρ in

Ansar/Daniilidis’ paper, respectively.

A. Algorithm Overview

Looking again at equation 1, Ansar and Daniilidis also attempt to solve for the depth si of each imaged point .

( )refrefi

camref

camii pxcRXs

vvv−= (1)

Instead of using equation 1 as the basis for their method, they instead look at known distance, ijd between the

feature points refj

refiij xxd

vv −= . Since distance is independent of coordinate frame, the orientation of the camera

does not need to be known. The distance from the camera to each feature point can be defined as ii Xsr

. The

distance between two features can now be written in terms of the image points and the unknown depths. The

constraint equation is given as

( ) ( ) jTijij

Tjji

Tiiiijj

T

iijjij XXssXXsXXsXsXsXsXscdvvvvvvvvvv

2222 −+=−−= (13)

where c is the unknown scale factor and is set to one later in the algorithm. Note that equation 13 is a quadratic

system, and we introduce a notion consistent with [2] where jiji sss =, . This system of constraint equations can be

rewritten in matrix form:

0=⋅ ssM (14)

where Tnnnn csssssss ),,,,,,,( ,1,1,13,12,1 LL −= and M contains the nmX , , nnX , , and 2

,nmd information

from the constraint equations. The solution to equation 14 lies in the nullspace of M, defined as V. Unfortunately,

the dimension of the nullspace is not one, and therefore the solution ss is a linear combination of the columns in

the nullspace.

λv

⋅= Vss (15)

Fortunately, there an additional constraint that can be imposed onto equation 15. The components of the ss vector

are not independent of each other.

4,23,1423143214,32,1 ssssssssssss === (16)

These constraints are developed for all combinations of all n depth terms. This system of constraint equations can

be rearranged and written in matrix from

0=⋅ λλK (17)

where ( )L2,11,1 λλλλ = . Equation 17 can be solved by finding the nullspace of K. This time, the nullspace

only has a dimension of one and λλ terms can be solved for directly. Back substitution yields the desired depth

information:

sssVss =⇒⋅=⇒= vvλλλλ (18)

If desired, the sr

can be correctly scaled by dividing by c (the last component of sr

). However, this is not necessary

for the exterior orientation problem as it solves for scale.

In general, Ansar and Daniilidis are very thorough in their algorithm development in [2]. However, in our

implementation of the Ansar/Daniilidis Method several modifications and additions were made. This section will

explain the motivation for these changes and detail the changes themselves.

A. Constraint equation for matrix K

The first modification was in the constraint matrix K given in equation 17 above. Equation 15 can be written in

terms of the column vectors of V.

( )

=

+

+

1

2

1

121

n

nvvvss

λ

λλ

M

rL

rr(19)

Expanding the column vector ivv

, let the component of each vector be labeled by the corresponding rows of the

depth vector si,j

( ) ( ) ( ) ( ) ( ) ( )( )Tinniiinniii vvvvvvv ,,,2,2,1,1,,1,3,1,2,1 LLv

−= (20)

Let { }LKJI ,,, be defined as any permutation of { }lkji ,,, . Substituting equation 20 into equation 16, the

following constraint equation can be developed:

( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( ) 0

1

1

1

1 ,,,,,,,,,

,,,,,,,,,

1

1,,,,,,,,,

,,,,

=

−

+−

+−

=−

∑ ∑

∑+

=

+

+=

+

=

n

a

n

ab aLKbJIalkbjiba

bLKaJIblkajiba

n

aaLKaJIalkajiaa

LKJIlkji

vvvv

vvvv

vvvv

ssss

λ

λ

λ (21)

This equation is slightly different than the corresponding equation proposed by Ansar and Daniilidis. Equation

21 can now be re-written into the form of equation 17. Ansar and Daniilidis assert that only the ( )2

12 −nn constraints

of the form 0,,,, =− kijikjii ssss are essential to forming equation 17 rather than all permutations of { }lkji ,,, .

This was interpreted as meaning

≤≠≠ kjkiji ;; , which yields the desired number of constraint equations

and produces a valid solution. However, this produces some duplicate equations. For example, { } }3,3,1{,, =kji

will produce the same equation as{ } }1,1,3{,, =kji . Removal of these extra equations results in

2

)1( 2 nn − equations. If we incorporate the remaining permutations of { }lkji ,,, where lkji ≠≠≠ , then there are

8

)2)(1( 2 +−− nnnn equations. For n>6, this is a significant increase in number of equations. Removal of the duplicate

equations, and addition of the remaining permutations provide for a marginal increase in the accuracy of the

solution. Comparisons of the different K matrices are shown later.

B. Solving ( ) iiii sign ,,1 λλλ =

The second modification dealt with the outlier generation. Looking more carefully at equation 14, the method of

determining iλ from λλ is given by Ansar and Daniilidis as

( ) iiii sign ,,1

1,11

λλλ

λλ

⋅=

=(22)

This method arbitrarily forces the sign on 1λ to be positive and determines the sign on the remaining iλ to be

defined by the sign of i,1λ Although this method results in a uniform sign ambiguity for the iλ , the ambiguity can

be removed since the terms in ss must be positive. In the ideal case, this method provides no difficulties.

In the non-ideal case with noise added to the data, this method is found to sporadically generate errors. Noise on

the data causes the value of ii ,λ to vary away from its ‘true’ value. Since iλ is real, by definition, ii ,λ must be

greater than zero. If the true value of ii ,λ is near zero, then in the presence of noise ii ,λ can become negative. The

quick solution is to just assign

0if0 , <= iii λλ (23)

However, this method while preventing a solution from becoming imaginary, still causes errors.

Another error that sporadically produces large errors in the final solution is more subtle. Analysis revealed that

these outliers occurred when 01 ≈λ . Remember that the sign of the remaining iλ are based off of the sign of i,1λ .

If 01 ≈λ , then i,1λ is also small. Since noise on the image points causes the value of i,1λ to vary away from its

‘true’ value, it is possible that when 01 ≈λ , i,1λ could change sign. Therefore the solution for each iλ could have

a sign error even though iλ itself is not near zero. The quick fix to this problem is instead of using 1λ to determine

the signs, use the largest iλ (determined by finding the largest ii ,λ ) to determine the sign of the remaining iλ .

( ) iiiki

iiik

sign

k

,,

, )(max

λλλ

λ

⋅=

⇒=

(24)

Instead of just taking the square root of the ii ,λ terms, we looked at different methods of recovering iλ . The

best method appears to be

∑∑+

= =

−1

1 1

2, )(min

n

j

j

ijiji λλλ (25)

However, solving this solution is iterative and took away from the speed of the algorithm. Another method is to

incorporate all of the ji ,λ terms into the solution and use a minimum least square solution to find the best fit for

each iλ . For each combination of },{ ji

( ) jjjiiijiji sign ,,, 2 λλλλλλλ ++⋅+=+ (26)

which can be rewritten as

( )( )( )

( )

++⋅+

++⋅+++⋅+++⋅+

=

++++++++

+

1,11,11,111

3,33,11,131

2,22,11,121

1,11,11,111

1

2

1

2

2

2

2

20000

010100110002

nnnnnnnn

n

sign

sign

sign

sign

λλλλλ

λλλλλλλλλλλλλλλ

λ

λλ

MMMMM

LLL

(27)

The two problems that were encountered above can also encountered here, just in different form. For example if

0≈+ ji λλ , then the term inside the square root could become negative in the presence of noise. In this case, the

square root is once again set to zero. However, since we are using a minimum least squares, this does not directly

set any variable to zero.

In order to determine the ( )jisign λλ + , one only needs to know the sign of iλ and jλ , and the relative

magnitude between the two. The sign of each iλ is determined in the same way as above; the largest iλ is found by

looking at relative magnitudes of ii,λ . If the largest iλ is defined as kλ , then ( )iki signsign ,)( λλ = . The

relative magnitudes can be found by looking at ii,λ .

( ) ( ) ( ) )( ,,,, jjjkiiikji signsignsignsign λλλλλλ +=+ (28)

C. Solving ( ) iiii sssigns ,,1=

The same algorithms can be used to determine the individual is from the jis , terms rather than to exclusively

use the information from the iis , terms. Note that the depth information terms are uniformly positive and of

relatively large magnitude, so noise on the data does not cause extreme outliers for the depth information terms.

D. Comparisons

The same configuration used to analyze the modified Fiore algorithm is used to evaluate the modifications to the

Ansar/Daniilidis algorithm. Since Ansar/Daniilidis is not affect by points lying in a plane, it is not instructive to vary

the height h of the feature point’s bounding box. Therefore, h = 0.05 km is chosen. The results of over 1000

different cases are presented in the following table. Algorithms written in Mathematica and are not necessarily

optimized for speed. Timing values are presented for comparison and are within a factor of 2 when compared to

[2].

K matrix Permutations. {I,J,K,L}

{ }kjkiji ≤≠≠ ;;Ansar/Daniilidis

{ }kjkiji ≤≠≠ ;;Duplicates Removed

All PermutationsDuplicates Removed

Eqn. 22 & 23 6.75m240ms

6.37m200ms

5.92m460ms

Eqn 26 4.69m260ms

4.51m220ms

4.05m480ms

iλ and is found

using methods from

Eqn 25 3.07m500ms

3.03m480ms

2.80m750ms

From Table 1, we can see the following trends. First, regardless how iλ and is were determined, removing the

duplicate rows from the K matrix increased the accuracy of the algorithm and reduced the computational time it took

to execute. Adding all permutations of {I,J,K,L} increased the accuracy of the solution, but took 2-3 times longer to

execute (for 7 points). As the number of points increases, it will take exponentially longer. In terms of determining

iλ and is from ji ,λ and jis , , solving equation 25 produces the best results, but again at the cost of an increased

computational time. However, solving equation 26 produces about a 30% improvement with only an extra

computational time of about 10%. Overall, which algorithm that is chosen depends on the situation, but in terms of

accuracy vs. computational time, solving equation 26 with the duplicate rows removed from the K matrix appears to

be the best algorithm.

For comparison, the results from Fiore are also presented.

Fiore Modified Fiore Planar Modified Planar Weighted21.2m1.4ms

10.5m1.4ms

35.3m1.6ms

34.5m1.6ms

10.4m1.6ms

IV. Conclusions

In this paper, two algorithms developed by Fiore and by Ansar and Daniilidis were presented and the associated

difficulties were shown. The Fiore algorithm did not handle near planar points. A modified algorithm was

presented that provided a seamless solution when the points lied in a plane, near a plane, or were completely non-

planar. A modification to the Fiore Planar algorithm allowed for improved solutions when the points were very

nearly planar. However, this algorithm must be used with caution on near-planar points as the error grows quickly

as the points become non-planar. The algorithm by Ansar and Daniilidis had difficulties in recovering the

iλ and is terms in the presence of noise which led to outliers. The modification to the algorithm presented in this

paper fixed the generation of outliers and provided a more accurate solution without a significant increase in

computational work.

Overall, both methods do a good job of pose estimation, but the modified Fiore method has the advantages of

speed and lower memory requirements. The method by Ansar/Daniilidis, while more accurate, is slow and does not

handle many feature points. That being said, in the case of a reduced number of features, (4 or 5), when Fiore’s

method does not work, the computational and memory burden may be acceptable for the use of the modified Ansar/

Daniilidis method.

References

1Fiore, Paul D.; “Efficient Linear Solution of Exterior Orientation” IEEE Transactions on Pattern Analysis and Machine

Intelligence,” Vol. 23, No. 2, Feb 2001, pp. 140–148.

2Ansar, Adnad; Daniilidis, Kostas; “Linear Pose Estimation from Points or Lines”, IEEE Transactions on Pattern Analysis

and Machine Intelligence,” Vol. 25, No. 5, May 2003, pp. 578–589.

3Thorn, B.K.P.; “Closed Form Solution of Absolute Orientation Using Unit Quaternions,” J. of Optical Soc. Am. A, Vol. 5,

1988, pp., 1127-1135.