Automatica 49 (2013) 3112–3119

Contents lists available at ScienceDirect

Automatica

journal homepage: www.elsevier.com/locate/automatica

Brief paper

The nonlinearity structure of point feature SLAM problems withspherical covariance matrices✩

Heng Wang a,1, Shoudong Huang b, Udo Frese c, Gamini Dissanayake b

a College of Electronic Information and Control Engineering, Beijing University of Technology, Beijing, 100124, Chinab Centre for Autonomous Systems, Faculty of Engineering and Information Technology, University of Technology, Sydney, Australiac German Research Center for Artificial Intelligence (DFKI) in Bremen, Germany

a r t i c l e i n f o

Article history:Received 4 July 2012Received in revised form25 March 2013Accepted 1 July 2013Available online 12 August 2013

Keywords:SLAMLeast squaresOne-dimensional optimization problemMinima analysis

a b s t r a c t

This paper proves that the optimization problem of one-step point feature Simultaneous Localizationand Mapping (SLAM) is equivalent to a nonlinear optimization problem of a single variable when theassociated uncertainties can be described using spherical covariance matrices. Furthermore, it is proventhat this optimization problem has at most two minima. The necessary and sufficient conditions for theexistence of one or two minima are derived in a form that can be easily evaluated using observation andodometry data. It is demonstrated that more than one minimum exists only when the observation andodometry data are extremely inconsistent with each other. A numerical algorithm based on bisection isproposed for solving the one-dimensional nonlinear optimization problem. It is shown that the approachextends to joining of twomaps, thus can be used to obtain an approximate solution to the complete SLAMproblem through map joining.

© 2013 Elsevier Ltd. All rights reserved.

1. Introduction

Simultaneous Localization and Mapping (SLAM) is the problemof building a map of the environment using a mobile robot andat the same time estimating the location of the robot withinthe map (Bailey & Durrant-Whyte, 2006; Dissanayake, Newman,Clark, Durrant-Whyte, & Csorba, 2001). SLAM is a fundamentalproblem for mobile robot navigation and has been investigated byrobotic researchers for more than ten years (Cadena & Neira, 2010;Durrant-Whyte & Bailey, 2006; Frese, 2006; Huang & Dissanayake,2007; Kummerle et al., 2009; Tardos, Neira, Newman, & Leonard,2002).

Althoughmany efficient SLAMalgorithmshave beendeveloped,and the sparseness of the information matrix in different SLAMformulations is now well understood and exploited thoroughly(e.g. Agrawal & Konolige, 2008; Dellaert & Kaess, 2006; Huang,Wang, & Dissanayake, 2008), the underlying structure of the

✩ This work has been supported in part by the Funds of National Science ofChina (Grant No. 61004061, 61210306067, 61034008), Australian Research CouncilDiscovery project (DP120102786), German Research Foundation (DFG) under grantSFB/TR 8 Spatial Cognition and the National Science Foundation for DistinguishedYoung Scholar of China (Grant No. 61225016). The material in this paper wasnot presented at any conference. This paper was recommended for publication inrevised form by Associate Editor Kok Lay Teo under the direction of Editor Ian R.Petersen.

E-mail addresses: wangheng19801980@126.com (H. Wang),Shoudong.Huang@uts.edu.au (S. Huang), udo.frese@dfki.de (U. Frese),Gamini.Dissanayake@uts.edu.au (G. Dissanayake).1 Tel.: +86 10 67396217; fax: +86 10 67396125.

0005-1098/$ – see front matter© 2013 Elsevier Ltd. All rights reserved.http://dx.doi.org/10.1016/j.automatica.2013.07.025

nonlinearity in SLAM has not been fully understood as yet. Forexample, Olson, Leonard, and Teller (2006) demonstrates thatpose graph SLAM, formulated as a high dimensional nonlinearoptimization problem, can be solved using stochastic gradientdescent (SGD) by simply dealing with each of the constraintsindividually. Recently, a more efficient SLAM algorithm, the so-called tree based network optimizer (TORO), was proposed inGrisetti, Rizzini, Stachniss, Olson, and Burgard (2008) where a treestructure is used on top of the SGD approach. Surprisingly, verylarge scale SLAM problems can be very efficiently solved in thisway without the need for good initial values, especially when thecovariance matrices of the relative poses are close to spherical(Grisetti, Stachniss, & Burgard, 2009). Some initial attempts toobtain a closed-form solution to pose graph SLAM have beenmadein Rizzini (2009), and in Carlone, Aragues, Castellanos, and Bona(2011) andMartinez, Morales, Mandow, and Garcia-Cerezo (2010),a linear approximation approach for pose graph SLAM is proposed.

For the point feature based SLAM problem, our initial investiga-tion has also shown some interesting phenomenon when a simpleGauss–Newton algorithm is applied to solve the SLAM as an opti-mization problem (Huang, Lai, Frese, & Dissanayake, 2010). For theVictoria Park dataset (Guivant & Nebot, 2001), the algorithm canconverge to the globally optimal solution with random initial val-ues 80% of the time.2 However, this ‘‘magic’’ convergence happens

2 Here the preprocessed data is used where data association is provided. Thepreprocessed data is available on OpenSLAM website: http://openslam.org/ underproject 2D I-SLSJF.

H. Wang et al. / Automatica 49 (2013) 3112–3119 3113

only when the covariances of observations and odometries are setto be spherical matrices but not when the original covariance ma-trices are used.

Although very high dimensional nonlinear optimization prob-lems typically have many local minima, the above convergence re-sults indicate that thismay not be the case for SLAMproblems. Thismotivates us to analyze the structure of the nonlinearity and thenumber of local minima involved in SLAM. The main contributionof this paper is the theoretical insight into the nonlinearity struc-ture of SLAM problems and a demonstration that a one-step SLAMproblem is equivalent to a nonlinear optimization problem in asingle variable. Furthermore, a numerical algorithm based on bi-section is proposed to solve the one-dimensional optimizationproblem. It is also shown that this algorithm can be used for joiningtwo local maps by approximating the local map covariance matri-ces with spherical ones, such that an approximate solution to thecomplete SLAM problem through sequential map joining can beobtained. This, however, is meant as an application of our theoreti-cal results, not as a full new SLAM algorithm. This paper is based onour preliminary work on feature based SLAM (Huang,Wang, Frese,& Dissanayake, 2012) and pose graph SLAM (Wang, Hu, Huang,& Dissanayake, 2012). The key improvements in this paper overHuang et al. (2012) andWang et al. (2012) include: (1) a new defi-nition of spherical covariance matrices, a new compact way to ex-press the high dimensional optimization problems, and a new andmore intuitive way to prove that the high dimensional problemis equivalent to a one-dimensional problem; (2) the wrap of 2πperiodicity of angles is taken into account when analyzing the op-timization problem, which is more practical; (3) a bisection algo-rithm for solving the one-dimensional problem for feature basedSLAM is given; and (4) more intuitive explanations of the resultsare presented.

The paper is organized as follows. Section 2 states the leastsquares SLAM formulation. In Section 3, the definition of sphericalmatrices is introduced, and an alternative SLAM formulation isderived when covariance matrices are spherical. In Section 4,it is shown that the one-step SLAM problem is equivalent to anonlinear optimization problem in a single variable. Section 5provides an analysis of the number of minima for one-step SLAM.In Section 6, a numerical algorithm based on bisection is proposedto obtain the globally optimal solution to one-step SLAM. InSection 7, experiments are performed to validate the analysisand the proposed numerical algorithm. Section 8 concludes thepaper and the Appendix presents the proof of a central, but moretechnical theorem related to the minima analysis.Notations: Throughout the paper, the superscript T and −1 standfor the transposition and the inverse of a matrix, respectively; C >0means thatmatrix C is real symmetric and positive definite; I andIn represent the identity matrix with appropriate dimension andthe identity matrix with dimension n, respectively; 0 representsthe zero matrix with a compatible dimension; and

|e|2C = eTC−1e (1)

denotes the weighted norm of vector e with a given covariancematrix C . wrap(θ) is a function that converts an arbitrary angleinto the equivalent angle in the range [−π, π), e.g., if θ =

3π2 , then

wrap(θ) = −π2 . The symbol diag(A1, . . . , An) denotes a diagonal

matrix whose elements are A1, . . . , An.

2. Least squares SLAM problem

2.1. Point feature based SLAM problem

The input to the SLAM problem contains two kinds of informa-tion. Odometry provides the geometric relationship between two

Fig. 1. The SLAM problem with 5 poses, 3 features and 5 observations, the initialpose r0 is fixed to (0, 0, 0).

consecutive poses. Observations provide the relative position ofobserved features with respect to the pose from which the obser-vations aremade. The full least squares SLAM formulation (Dellaert& Kaess, 2006) uses the odometry and observation information toestimate all the robot poses and all the feature positions.

In this paper, we use Z ij ∈ R2×1 to denote the observation made

from pose ri to feature fj (observed x and y coordinates of the fea-ture). We use Oi−1

i ∈ R3×1 (1 ≤ i ≤ p) to denote the odometryinformation between pose ri−1 and pose ri (relative x, y positionand orientation angle), PZ ij ∈ R2×2 > 0 and POi ∈ R3×3 > 0 are thecorresponding covariance matrices of the observation and odome-try noises. Here the noises are assumed to be zero-mean Gaussian.Fig. 1 presents a simple SLAM problem, where there are 4 odome-tries and 5 observations.

2.2. Least squares SLAM formulation

Suppose a number of 2D point features f1, f2, . . . , fn in theenvironments are observed from a sequence of 2D robot posesr0, r1, r2, . . . , rp. The first robot pose (pose r0) is chosen as theorigin of the global coordinate frame.

We use Xfj = [xfj , yfj ]T to denote the x, y position of feature fj.

Xri = [xri , yri ]T denotes the x, y position of robot pose ri while φri

denotes the orientation of pose ri. Rri is the rotation matrix of poseri given by

Rri = R(φri) =

cosφri − sinφrisinφri cosφri

. (2)

The least squares SLAM formulation uses the odometry and obser-vation information to estimate the state vector containing all therobot poses and all the feature positions

X = [XTf1 , . . . , X

Tfn , X

Tr1 , φr1 , . . . , X

Trp , φrp ]

T

and the SLAM problem is to minimize

F(X) =

pi=1

|Oi−1i − HOi(X)|2POi

+

i,j

|Z ij − HZ ij (X)|2P

Zij

. (3)

Note here that we have used the notation in (1) to simplify the ex-pression.

In (3), HOi(X) and HZ ij (X) are the corresponding functions relat-ing Oi−1

i and Z ij to the state X . The odometry is a function of two

poses [XTri−1

, φri−1 ]T and [XT

ri , φri ]T and is given by

HOi(X) =

RTri−1

(Xri − Xri−1)

φri − φri−1

.

3114 H. Wang et al. / Automatica 49 (2013) 3112–3119

For example, in Fig. 1, the function of odometry between r1 and r2 is

HO2(X) =

RTr1(Xr2 − Xr1)

φr2 − φr1

=

(xr2 − xr1) cos(φr1) + (yr2 − yr1) sin(φr1)

−(xr2 − xr1) sin(φr1) + (yr2 − yr1) cos(φr1)φr2 − φr1

.

The observation is a function of one pose (XTri , φri)

T and one featureposition Xfj which is given by

HZ ij (X) = RTri(Xfj − Xri).

In Fig. 1, the function of the observation of f2 by r1 is

HZ12 (X) = RTr1(Xf2 − Xr1)

=

(xf2 − xr1) cos(φr1) + (yf2 − yr1) sin(φr1)

−(xf2 − xr1) sin(φr1) + (yf2 − yr1) cos(φr1)

.

In particular, since φr0 = 0 and Xr0 =0 0

T , the odometry func-tion from robot r0 to r1 is given by

HO1(X) =

Xr1φr1

and the observation function from robot r0 to fj is given by

HZ0j (X) = Xfj .

The ‘‘one-step SLAM’’ problem is a special case of (3) where thenumber of robot poses is two, i.e. p = 1.

3. Alternative formulation with spherical covariances

Note thatHOi(X) andHZ ij (X) in (3) are all nonlinear functions. Inthis section, we will show that there is an alternative formulationof F(X) which is much simpler if the covariance matrices PZ ij andPOi are all spherical.

3.1. Definition of spherical matrices

We first introduce the definition of spherical matrices.

Definition 1. A matrix A ∈ R2×2 is called spherical if it commuteswith R(φ) in (2) for every φ, i.e. AR(φ) = R(φ)A for every φ. Amatrix B ∈ R3×3 is called spherical if it has the format of B =

diag(A, a) where A ∈ R2×2 is spherical and a is a real number.

Intuitively, such a spherical matrix treats every direction in theplane in the same way, so it does not make a difference whethera vector is rotated before or after the application of the matrix.

Remark 1. A2×2 positive definite sphericalmatrix has the formatA = diag(a, a)with a > 0. A 3×3positive definite sphericalmatrixhas the format A = diag(a, a, b) with a > 0 and b > 0.

We will also introduce a more general definition of sphericalmatrices which will be used in the following sections of this paper.

Definition 2. Let R̄2n(φ) ∈ R2n×2n be a block-diagonal matrix withφ-rotationmatrices, i.e., R(φ), in every 2×2 block on the diagonal.Matrix A ∈ R2m×2n is called spherical if it commutes with in-planerotation, i.e. AR̄2n(φ) = R̄2m(φ)A for every φ.

As an example, form = 3, n = 2,

A ∈ R6×4=

I2 00 I2I2 −I2

is a spherical matrix, since we have AR̄4(φ) = R̄6(φ)A for everyφ where R̄6(φ) = diag(R(φ), R(φ), R(φ)), R̄4(φ) = diag(R(φ),R(φ)).

Definition 2 makes sense for matrices operating on stacked2D vectors with alternating x and y coordinates. Intuitively, thesematrices treat all directions the same, it does notmake a differencewhether all 2D vectors in a stacked vector rotated before or afterthe application of the matrix. Note that all 2D components mustbe rotated by the same angle. A non-trivial example is the matrix13I2

13I2

13I2

, which maps 3 points to their center. It makes

no differencewhether the points are rotated or the resulting centeris rotated.

Remark 2. The sum, difference, product, inverse, transpose ofspherical matrices are also spherical matrices. If P > 0 is sphericaland R is a rotation matrix, then for any x, Rx and x have the sameweighted norm as shown below:

|Rx|2P = xTRTP−1Rx = xTRTRP−1x = xTP−1x = |x|2P

where P−1 is also spherical.

3.2. Alternative formulation

From Remark 2, when the covariance PZ ij is a 2 × 2 sphericalpositive definite matrix, we have

|Z ij − HZ ij (X)|2P

Zij

= |Z ij − RT

ri(Xfj − Xri)|2PZij

= |RriZij − (Xfj − Xri)|

2PZij

. (4)

Similarly, when the covariance POi is a 3 × 3 spherical positivedefinite matrix with the format

POi = diag(POxyi, pOφ

i)

where POxyi

is a 2 × 2 spherical positive definite matrix, theodometry term

|Oi−1i − HOi(X)|2POi

with Oi−1i =

Oi−1ixy

Oi−1iφ

becomes

|Oi−1ixy − RT

ri−1(Xri − Xri−1)|

2POxyi

+ p−1Oφi[wrap(Oi−1

iφ− φri + φri−1)]

2

= |Rri−1Oi−1ixy − (Xri − Xri−1)|

2POxyi

+ p−1Oφi[wrap(Oi−1

iφ− φri + φri−1)]

2.

Note that the difference between any two angles iswithin [−π, π),so we wrap the error in orientations.

Now the objective function in least squares SLAM formulationcan be written as

F(X) =

i,j

|RriZij − (Xfj − Xri)|

2PZij

+

pi=1

|Rri−1Oi−1ixy − (Xri − Xri−1)|

2POxyi

+

pi=1

p−1Oφi[wrap(Oi−1

iφ− φri + φri−1)]

2

where φr0 = 0, Rr0 = I, Xr0 =0 0

T .

H. Wang et al. / Automatica 49 (2013) 3112–3119 3115

Fig. 2. One-step SLAM problem with n features.

4. One-step SLAM problem

As pointed out in Cadena and Neira (2010) and Huang, Wang,and Dissanayake (2008), all SLAM problems can be decomposedto that of joining two maps, which is similar to a one-step SLAMproblem. To understand the nonlinearity of the general SLAMproblem, it is essential to analyze the simple case of one-stepn-feature SLAM which is shown in Fig. 2.

4.1. A compact form of the objective function

For one-step SLAM with spherical covariance matrices, assumethat the n features are observed from both pose r0 and pose r1, thenthe objective function in least squares SLAM formulation is

F(X) =

nj=1

|Z0j − Xfj |

2PZ0j

+

nj=1

|Rr1Z1j − (Xfj − Xr1)|

2PZ1j

+ |O01xy − Xr1 |

2POxy1

+ p−1Oφ1(wrap(O0

1φ− φr1))

2.

To simplify the notations, we use φ to denote φr1 , zφ to denoteO01φ

, pφ to denote pOφ1. Then the objective function of the spherical

one-step SLAM problem is

F(X) =

nj=1

|Z0j − Xfj |

2PZ0j

+

nj=1

|Rr1Z1j − (Xfj − Xr1)|

2PZ1j

+ |O01xy − Xr1 |

2POxy1

+ p−1φ (wrap(zφ − φ))2. (5)

This function can be further written in a compact form as

F1(XL, φ) = |AXL − z0 − R̄(φ)z1|2C + p−1φ (wrap(zφ − φ))2 (6)

where R̄(φ) is defined in Definition 2 (for brevity, the dimensionsubscripts of R̄(φ) will be omitted later), and

A =

I2 . . . 0 0...

. . ....

...0 . . . I2 00 . . . 0 I2I2 . . . 0 −I2...

. . ....

...0 . . . I2 −I2

, XL =

Xf1...XfnXr1

,

z0 =

Z01...

Z0n

O01xy

0

z1 =

0 Z1

1 · · · Z1n

T (7)

C = diag(PZ01 , . . . , PZ0n , POxy1, PZ11 , . . . , PZ1n ). (8)

Remark 3. In this new compact form, XL represents the ‘‘linear’’part of the state vector X; z0 contains measurements takenfrom pose r0 and the translation part of the odometry; z1containsmeasurements taken frompose r1, which are then rotatedaccording to the orientation of pose r1; C is a block-diagonalmatrixmade from the 2 × 2 covariance matrices.

Definition 3. In this paper, the function

F0(XL, φ) = |AXL − z0 − R̄(φ)z1|2C (9)

is called the objective function of a spherical one-step SLAMproblem without an orientation link. The function (6) is called theobjective function of a spherical one-step SLAM problem with anorientation link. Note that here both A and C are spherical matricesaccording to Definition 2.

Using this compact form of SLAMobjective functions, we can easilyprove that the one-step SLAM problem is equivalent to a one-dimensional optimization problem. In fact, for any fixed φ, theproblem is linear, and the optimal XL tominimize F0(XL, φ) in (9) orminimize F1(XL, φ) in (6) can be obtained in a closed-form whichhas a simple dependence on φ. Thus the optimization problembecomes a one-dimensional optimization problem with respect tothe single variable φ.

4.2. Spherical one-step SLAM

The following lemma is essential for later developments.

Lemma 1. For a linear least squares problem |AX −b|2C , its minimumcan be obtained by

minX

|AX − b|2C = |b|2Q

where Q−1= C−1

−C−1A(ATC−1A)−1ATC−1, and the solution is alsothe global minimum.

Proof. Note that the optimal solution to minX |AX − b|2C is

X∗= (ATC−1A)−1ATC−1b;

substituting X∗ into |AX − b|2C , we can get |b|2Q . This completes theproof. �

Theorem 1. For a spherical one-step n-feature SLAM problem, theoptimal solution to the following optimization problem

minXL,φ

Fχ (XL, φ)

where

Fχ (XL, φ) = |AXL − z0 − R̄(φ)z1|2C + χp−1φ (wrap(zφ − φ))2

can be obtained by firstly solving the following one dimensionoptimization problem

minφ

fχ (φ) (10)

to obtain φ∗, then solving X∗

L by

X∗

L = (ATC−1A)−1ATC−1(z0 + R̄(φ∗)z1) (11)

where

fχ (φ) = c0 − 2a0 cos(φ − φ0) + χp−1φ (wrap(zφ − φ))2

and c0, a0, φ0 are constants given by

c0 = zT0Q−1z0 + zT1Q

−1z1 (12)

a0 =

(zT0Q−1z1)2 + (zT0Q−1z⊥

1 )2, (13)

φ0 = atan2(−zT0Q−1z⊥

1 , −zT0Q−1z1) (14)

3116 H. Wang et al. / Automatica 49 (2013) 3112–3119

and z⊥

1 = R̄

π2

z1; χ ∈ {0, 1}, if χ = 0, there is no orientation link,

else if χ = 1, there is an orientation link.

Proof. Let

f0(φ) = minXL

F0(XL, φ) = minXL

|AXL − z0 − R̄(φ)z1|2C

applying Lemma1, andobserving thatQ is again a sphericalmatrix,we have

f0(φ) = |z0 + R̄(φ)z1|2Q

=z0 + R̄(φ)z1

TQ−1

z0 + R̄(φ)z1

= zT0Q−1z0 + 2zT0Q

−1R̄(φ)z1 + zT1 R̄(φ)TQ−1R̄(φ)z1= c0 + 2zT0Q

−1(z1 cosφ + z⊥

1 sinφ)

= c0 + (2zT0Q−1z1) cosφ + (2zT0Q

−1z⊥

1 ) sinφ

= c0 − 2

−zT0Q−1z1

−zT0Q−1z⊥

1

T cosφsinφ

= c0 − 2a0 cos(φ − φ0)

where c0, a0, φ0 are given by (12)–(14). Thus, φ can be obtainedby solving the optimization problem (10), then from the proof ofLemma 1, X∗

L can be obtained by (11) where φ∗ is the solution to(10), this completes the proof. �

Remark 4. It should be noted that the spherical covariancematrices play a key role in developing the results in Theorem 1.If the covariance matrix is non-spherical, the matrix Q will be afunction of φ and the function f0 will be far more complicated. Thenon-trivial insight in Theorem 1 is that f0(φ) has such a simpleform, only a shifted cosine function in φ.

Remark 5. The spherical one-step n-feature SLAM problem with-out odometry is similar to the problem of finding the relativetransformation between two coordinate frames given two corre-sponding point sets (Arun, Huang, & Blostein, 1987), both of whichare formulated as a problem of determining a rotation matrix. Thedifference between the approach proposed in this paper and thatof Arun et al. (1987) is that a numerical algorithm through usingsingular value decomposition (SVD) is proposed to solve the 3Dproblem in Arun et al. (1987), while a closed-form solution to thesimpler 2D case is obtained in this paper, i.e., the rotation matrixR(φ0) can be obtained from formula (2) where φ0 is given by (14).

5. Minima analysis of the spherical one-step SLAM

In this section, we analyze the minima characteristic of theobjective function of the one-step SLAM problem. By virtue ofTheorem 1, the problem is essentially a one-dimensional problemand when there is no orientation link, the one-dimensionalfunction f0(φ) is periodic with a single minimum of φ0. With theadditional orientation link (i.e., χ = 1 in Theorem 1), f1(φ) in (10)is still periodic because of the wrap function. So it is enough toperform the analysis within any interval with the length of 2π .Without loss of generality, we consider interval φ ∈ [zφ − π, zφ +

π). Within this interval, the wrap is not needed and we have thefollowing theorem which analyzes the minima of the one-stepSLAM problem with an orientation link.

Theorem 2. Let c0 ≥ 0, a0 > 0, φ0 ∈ [−π, π) be constants definedin (12)–(14), pφ > 0, zφ ∈ [−π, π) are given constants, then thefunction

f1(φ) = c0 − 2a0 cos(φ − φ0) + p−1φ (zφ − φ)2 (15)

Fig. 3. Possible situations of having twominima by satisfying conditions (16)–(21).The x-axis is Cφ and the y-axis is a. In the shaded area, there are twominima; in theother area, there is only one minimum.

has at least one and at most two minima in the interval φ ∈ [zφ −

π, zφ + π). Moreover, there are two minima if and only if

a ≥ 1 (16)−a sin Cφ − π ≤ 0 (17)a2 − 1 + ϕ1 ≥ 0 (18)

−

a2 − 1 + ϕ2 ≤ 0 (19)

−a sin Cφ + π ≥ 0 (20)

ϕ1 − ϕ2 ≤ 0 (21)

hold simultaneously, where

a = a0pφ, Cφ = zφ − φ0

and

ϕ1 = wraparccos

−

1a

− Cφ

,

ϕ2 = wrap

− arccos

−1a

− Cφ

.

(22)

Proof. See Appendix. �

Remark 6. Fig. 3 illustrates conditions (16)–(21). The possible pairof a, Cφ when there are two minima is shown in the shaded area.For example, it can be seen that if a < 1, there is only one min-

imum. If |Cφ | < arcsin

π√π2+1

= 1.2626, there is also only

one minimum. Note that Cφ is the difference between the orien-tation measurement from odometry (zφ) and the orientation thatbest fits the observation data (φ0). So in any practical scenario, |Cφ |

is smaller than 1.2626 (72.3 degrees) and there is only one mini-mum. There are twominima only when the odometry data and theobservation data are extremely inconsistent.

6. A bisection based algorithm to solve the spherical one-stepSLAM problem

The following is a new algorithm to obtain the optimal solutionto the one-step SLAM problem based on Theorem 2 and a globalbisection approach.

Algorithm 1. • Step1: computeparametersA, C, z0, z1 in (7)–(8),and c0, a0, φ0 in (12)–(14), using the given odometry and obser-vation data.

H. Wang et al. / Automatica 49 (2013) 3112–3119 3117

Fig. 4. Joining of two local maps.

• Step 2: obtain the one or two minima of (15) in the interval[zφ − π, zφ + π) by Theorem 2, Table 2 and the bisectionalgorithm, i.e., first find the monotone increasing intervals off ′

1(φ) in [zφ − π, zφ + π ]; then use bisection to obtain the oneor two minima by satisfying conditions f ′

1(φ) = 0, f ′′

1 (φ) ≥ 0,e.g., if there is only one minimum, use bisection to obtain theminimum in the interval [zφ − π, zφ + π ], else if there aretwo minima, use bisection to obtain the minima in intervals[zφ − π, zφ + ϕ1] and [zφ + ϕ2, zφ + π ] respectively, whereϕ1, ϕ2 are defined in (22).

• Step 3: if there are twominima, compare the values f1(φ) of thetwo minima to get the optimal solution φ∗; if there is only oneminimum, then it is the optimal φ∗.

• Step 4: get all the other variablesXL as a function ofφ∗, i.e., usingformula (11) to get the optimal solution to the spherical one-step SLAM problem.

Remark 7. Instead of solving f ′

1(φ) = 0 using the bisectionmethod as in Algorithm 1, it is also possible to apply the goldensection method (e.g. Loxton and Lin (2011)) to find the global min-imumof f1(φ) in (15). Algorithm1presents a newway to obtain thesolution of the spherical one-step SLAM problem by solving a one-dimensional problem through bisection and a closed-form formula(11). It should be pointed out that the joining of two local maps asshown in Fig. 4 can be seen as a one-step n-feature SLAM problemif each local map is treated as an integrated observation (Huang,Wang, &Dissanayake, 2008) and the corresponding covariancema-trices of the two localmaps are spherical. In Fig. 4, n1 is the numberof features that appear only in local map 1, n is the number of fea-tures that appear in both local map 1 and local map 2, and n2 is thenumber of features that appear only in local map 2. r0 is the startpose of local map 1 which is the origin of the global map. r1 is theend pose in local map 1 as well as the start pose of local map 2, r2 isthe end pose in local map 2. In fact, only common features are im-portant in the map joining problem, and it is evident that the mapjoining problem in Fig. 4 with r2 and n1 > 0, n2 > 0 is equivalentto the case without r2 and n1 = 0, n2 = 0 which is shown in Fig. 2.

Remark 8. For an m-step SLAM problem, it can be shown thatthe problem is equivalent to an optimization problem of morientation variables φr1 , . . . , φrm . However, the structure of them-dimensional function becomes much more complicated thanfχ (φ) in (10) due to the involvement of product terms such assin(φr1) cos(φr2). On the other hand, a multi-step SLAM problemcan be solved by a sequence of map joining problems, either usingthe sequential map joining strategy (Huang, Wang, & Dissanayake,2008) or divide-and-concur strategy (Cadena & Neira, 2010).Algorithm 1 can be used in both of the two strategies to obtainapproximate solutions to the SLAM problem by approximating thelocal map covariance matrices with spherical ones.

Fig. 5. Comparison of map joining results using the new approach and I-SLSJFapproach of the Victoria Park 200 local map data. The circles denote the result ofthe new approach, and the dots denote the result of the I-SLSJF approach.

Remark 9. As discussed in Remark 4, it is crucial to have a spher-ical covariance matrix for our approach. Spherical covariancesmean that the uncertainty is equal in all the directions which isrealistic for some situations, e.g. when sensors used are laser rangefinders. Other sensors, in particular stereo vision, havemuch largeruncertainty in the depth direction. However, under these situa-tions, we can still approximate the covariance as spherical onesfirst and quickly compute an approximate solution which can beused as a good initial value for the original SLAM problem.

7. Experiment results

We use the publicly available Victoria Park dataset to demon-strate the results in this paper.

7.1. Testing the number of minima

We first demonstrate the conclusion that the spherical one-step SLAM problem has only one minimum in most realisticscenarios. We use the equivalent map joining problem withspherical covariance matrices as an example. The Victoria Parkdataset was divided into two parts to build two local maps. Thecovariance matrices of the two local maps are set to be identitymatrices. Using the local map data to compute the values of a, Cφ

in Theorem 2, we obtain a = 203 570, Cφ = −0.0028. Obviously,these do not satisfy conditions (16)–(21) (also refer to Fig. 3),meaning that the map joining problem has only one minimum.To check this conclusion, the Gauss–Newton algorithm was usedto join the maps. In an experiment with more than 100 trials, thealgorithm always converges to the optimal solution from arbitraryinitial guesses to the robot poses and feature locations (Huanget al., 2012).

7.2. Approximate solution to SLAM

The Victoria Park dataset was divided into 200 parts to build200 local maps using least squares optimization. With G2O(Kuemmerle, Grisetti, Strasdat, Konolige, & Burgard, 2011), thebuilding of the 200 local maps took 0.8 s in total.

We apply Algorithm 1 to obtain an approximate solution to thesequential map joining problem as explained in Remarks 7 and 8,which is also an approximate solution to the original SLAM prob-lem. Fig. 5 shows the map joining results of the 200 local mapsusing the new approach proposed in this paper and the I-SLSJF ap-proach proposed in Huang, Wang, Dissanayake, and Frese (2008),respectively. The average difference in x position is 0.2494 m, theaverage difference in y position is 0.5281 m (sum of the absolute

3118 H. Wang et al. / Automatica 49 (2013) 3112–3119

Table 1Different techniques compared in Section 7.2.

Techniques Time (s) CPU (GHz) Solution

G2O + I-SLSJF 29.9 2.00 ExactG2O + I-SLSJFIdentity 19.3 2.00 ApproximateG2O + Algorithm 1 3.7 2.00 ApproximateTSAM2 5 2.80 Exact

‘‘G2O + I-SLSJF’’ means using G2O to build the 200 local maps and using I-SLSJFto join the 200 local maps. ‘‘G2O + I-SLSJFIdentity ’’ means the same but assumingidentity covariance matrices and not computing the information matrix in I-SLSJF.‘‘G2O + Algorithm 1’’ has a similar meaning. The timing of these three algorithmsinclude 0.8 s for local map building by G2O.

difference divided by the number of landmarks and poses), and theaverage difference in orientation for each pose is 0.0080 rad (sumof the absolute difference divided by the number of poses).

The map joining process took 29.1 s for I-SLSJF and 2.9 swith the new approach proposed in this paper. The main reasonswhy the new method is faster than I-SLSJF are: (i) we solvedone-dimensional problems 200 times by Algorithm 1 instead ofsolving 200 problems with increasing dimension as in I-SLSJF;(ii) the information matrix computing step is not needed in ourmethod since we simply assume that all the covariance matricesare spherical. If we also make the spherical covariance matrices’assumption in I-SLSJF and do not compute the information matrix,then the computation time of I-SLSJF is 18.5 s. The timing results ofmap joining are obtained by running MATLAB code on a computerIntel(R) Pentium(R) with 2.00 GHz CPU. As a comparison, TSAM 2took 5 s running C++ code on a Macbook Pro with 2.80 GHz CPUaccording to Ni and Dellaert (2010). It should be noted that TSAM2 gives the exact solution instead of an approximate solution usingour approach. Table 1 gives a summary of the different techniquescompared in the experiment.

8. Conclusions and future work

The main contribution of this paper is a mathematical proofof the special properties of the one-step SLAM problem, i.e., thehigh dimensional SLAM problem is equivalent to a simple one-dimensional optimization problem under the condition that theassociated covariance matrices are spherical. The necessary andsufficient condition for the existence of one or two minima makesit possible to guarantee that the globally optimal solution has beenreached, which leads to the possibility of obtaining robust solu-tions to the SLAM problem even when the initial guess obtainedthrough odometry or relative pose estimates is unreliable.

More research is needed to analyze the multi-step SLAM prob-lem, the impact of using non-spherical covariancematrices and ro-bust kernels in the objective function (Kuemmerle et al., 2011), andthe extension of the work to 3D. It is not difficult to prove thatthe one-step 3D SLAM is equivalent to an optimization problemin 3 variables (the three parameters describing the robot orienta-tion). However, it is non-trivial to derive the explicit conditions forthe existence of one or more minima mainly because the 3D ori-entation link is a lot more difficult to analyze. Work in all thesedirections has the potential to enhance the understanding of thisimportant robotics problem and lead to more reliable and efficientsolutions to robot navigation.

Appendix

A.1. Proof of Theorem 2

Let ϕ = φ − zφ, a = a0pφ, Cφ = zφ −φ0, thenminimizing f1(φ)in (15) is equivalent to minimizing the function

f̄1(ϕ) =12pφ f1(φ) =

12pφc0 − a cos(ϕ + Cφ) +

12ϕ2.

Table 2Analysis of the number of solutions to (23) for ϕ ∈ [−π, π).

h(−π) − − − − − − − + + +

h(ϕ1) − − − + + + + − − +

h(ϕ2) − + + − − + + − + −

h(π) + − + − + − + + + +

No. 1 1 1 1 2 1 1 1 1 1

‘−’ denotes ‘<0’, ‘+’ denotes ‘>0’, ‘1, 2’ denote the number of solutions to (23) in[−π, π).

Let h(ϕ) = f̄ ′

1(ϕ), and g(ϕ) = f̄ ′′

1 (ϕ), we have

h(ϕ) = a sin(ϕ + Cφ) + ϕ

g(ϕ) = a cos(ϕ + Cφ) + 1,

a local minimum ϕ of f̄1(ϕ) satisfies

h(ϕ) = 0, g(ϕ) ≥ 0 (23)

and analyzing the local minima ϕ ∈ [−π, π) of f̄1(ϕ) is equivalentto analyzing the local minima φ ∈ [zφ −π, zφ +π) of f1(φ) in (15).

First consider the case when a < 1. Since cos(ϕ + Cφ) ≥

−1, g(ϕ) > 0 for any ϕ, which means h(ϕ) is monotonicallyincreasing. Since we have

h(−π) = a sin(−π + Cφ) − π < a − π < 0h(π) = a sin(π + Cφ) + π > −a + π > 0

there is one and only one ϕ ∈ [−π, π) satisfying (23).Now consider the case when a ≥ 1. g(ϕ) = 0 has two solutions

in [−π, π) which are ϕ1 = wraparccos

−

1a

− Cφ

, and ϕ2 =

wrap− arccos

−

1a

− Cφ

.

First consider the casewhen ϕ1 ≤ ϕ2, since g(ϕ1) = 0, g(ϕ2) =

0, ϕ1, ϕ2 divide the interval [−π, π) into three intervals whereh(ϕ) is monotone in each of them, i.e., [−π, ϕ1), [ϕ1, ϕ2), and[ϕ2, π), there are at most two ϕ satisfying (23), since betweentwo monotonously increasing zero-crossings of h(ϕ), there mustbe a region where h(ϕ) decreases. So there are at most two ϕsatisfying (23) and this occurs only if h(ϕ) is increasing in [−π, ϕ1),decreasing in [ϕ1, ϕ2), and increasing again in [ϕ2, π].

Since we only consider ϕ ∈ [−π, π), the number of solutionsto satisfy (23) can be analyzed by observing the four valuesh(−π), h(ϕ1), h(ϕ2), h(π).

Note that

h(−π) = −a sin Cφ − π, h(π) = −a sin Cφ + π

h(ϕ1) =

a2 − 1 + ϕ1, h(ϕ2) = −

a2 − 1 + ϕ2;

it is impossible to have h(−π) > 0 and h(π) < 0 simultaneously.It can also be proved that the cases

h(−π) < 0, h(ϕ1) < 0, h(ϕ2) < 0, h(π) < 0

and

h(−π) > 0, h(ϕ1) > 0, h(ϕ2) > 0, h(π) > 0

could never happen. Thus there are only 10 cases which are statedin Table 2.

Similarly, it can be proved that there is at most one solution to(23) for the case when ϕ1 > ϕ2.

Thus, there are two solutions to (23) if and only if the conditionsin (17)–(20) hold simultaneously togetherwithϕ1 ≤ ϕ2 and a > 1.This completes the proof.

References

Agrawal, M., & Konolige, K. (2008). Frameslam: from bundle adjustment to real-time visual mapping. IEEE Transactions on Robotics, 24(5), 1066–1077.

Arun, K. S., Huang, T. S., & Blostein, S. D. (1987). Least squares fitting of 3-d point sets.IEEE Transactions on Pattern Analysis andMachine Intelligence, PAMI-9, 698–700.

Bailey, T., & Durrant-Whyte, H. (2006). Simultaneous localization and mapping(SLAM): Part II. IEEE Robotics & Automation Magazine, 13(2), 99–110.

H. Wang et al. / Automatica 49 (2013) 3112–3119 3119

Cadena, C., & Neira, J. (2010). SLAM in o(log n) with the combined Kalman-information filter. Robotics and Autonomous Systems, 58(11), 1207–1219.

Carlone, L., Aragues, R., Castellanos, J., & Bona, B. (2011). A linear approximation forgraph-based simultaneous localization and mapping. In 2011 Robotics: scienceand systems conference (RSS). Los Angeles, CA, USA.

Dellaert, F., & Kaess, M. (2006). Square root sam: simultaneous localizationand mapping via square root information smoothing. International Journal ofRobotics Research, 25(12), 1181–1203.

Dissanayake, G., Newman, P., Clark, S., Durrant-Whyte, H., & Csorba, M. (2001). Asolution to the simultaneous localization and map building (SLAM) problem.IEEE Transactions on Robotics and Automation, 17(3), 229–241.

Durrant-Whyte, H., & Bailey, T. (2006). Simultaneous localization and mapping(SLAM): Part I. IEEE Robotics & Automation Magazine, 13(3), 108–117.

Frese, U. (2006). A discussion of simultaneous localization and mapping.Autonomous Robots, 20(1), 25–42.

Grisetti, G., Rizzini, D.L., Stachniss, C., Olson, E., & Burgard, W. (2008). Onlineconstraint network optimization for efficient maximum likelihood mapping. InIEEE international conference on robotics and automation (ICRA) (pp. 1880–1885).Pasadena, California.

Grisetti, G., Stachniss, C., & Burgard, W. (2009). Non-linear constraint networkoptimization for efficient map learning. IEEE Transactions on IntelligentTransportation Systems, 10(3), 428–439.

Guivant, J. E., & Nebot, E. M. (2001). Optimization of the simultaneous localizationand map building (SLAM) algorithm for real time implementation. IEEETransactions on Robotics and Automation, 17(3), 242–257.

Huang, S., & Dissanayake, G. (2007). Convergence and consistency analysis forextended Kalman filter based SLAM. IEEE Transactions on Robotics, 23(5),1036–1049.

Huang, S., Lai, Y., Frese, U., & Dissanayake, G. (2010). How far is SLAM from a linearleast squares problem? In IEEE/RSJ international conference on intelligent robotsand systems (IROS) (pp. 3011–3016). Taipei, Taiwan.

Huang, S., Wang, Z., & Dissanayake, G. (2008). Sparse local submap joining filter forbuilding large-scale maps. IEEE Transactions on Robotics, 24(5), 1121–1130.

Huang, S., Wang, Z., Dissanayake, G., & Frese, U. (2008). Iterated SLSJF: a sparselocal submap joining algorithmwith improved consistency. In 2008 Australiasanconference on robotics and automation. Canberra, Australia.

Huang, S., Wang, H., Frese, U., & Dissanayake, G. (2012). On the number oflocal minima to the point feature based SLAM problem. In IEEE internationalconference on robotics and automation (ICRA) (pp. 2074–2079). St. Paul,Minnesota, USA.

Kuemmerle, R., Grisetti, G., Strasdat, H., Konolige, K., & Burgard, W. (2011). G2O: ageneral framework for graph optimization. In IEEE International conference onrobotics and automation (ICRA) (pp. 3607–3613). Shanghai, China.

Kummerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G., Stachniss, C., et al.(2009). On measuring the accuracy of SLAM algorithms. Autonomous Robots,27(4), 387–407.

Loxton, R., & Lin, Q. (2011). Optimal fleet composition via dynamic programmingand golden section search. Journal of Industrial and Management Optimization,7(4), 875–890.

Martinez, J.L., Morales, J., Mandow, A., & Garcia-Cerezo, A. (2010). Incrementalclosed-form solution to globally consistent 2d range scan mapping with two-steppose estimation. In The 11th IEEE internationalworkshop on advancedmotioncontrol (pp. 252–257). Nagaoka, Japan.

Ni, K., & Dellaert, F. (2010). Multi-level submap based SLAMusing nested dissection.In The 2010 IEEE/RSJ international conference on intelligent robots and systems(IROS) (pp. 2558–2565). Taipei, Taiwan.

Olson, E., Leonard, J.J., & Teller, S. (2006). Fast iterative optimization of pose graphswith poor initial estimates. In IEEE international conference on robotics andautomation (ICRA) (pp. 2262–2269). Orlando, Florida.

Rizzini, D.L. (2009). Towards a closed-form solution of constraint networks formaximum likelihood mapping. In The 14th international conference on advancedrobotics (ICAR) (pp. 1–5). Munich, Germany.

Tardos, J. D., Neira, J., Newman, P. M., & Leonard, J. J. (2002). Robust mapping andlocalization in indoor environments using sonar data. International Journal ofRobotics Research, 21(4), 311–330.

Wang, H., Hu, G., Huang, S., & Dissanayake, G. (2012). On the structure ofnonlinearities in pose graph SLAM. In 2012 Robotics: science and systemsconference (RSS). Sydney, Australia.

Heng Wang received the Bachelor’s degree and thePh.D. degree from the College of Information Scienceand Engineering at Northeastern University, P. R. China,in 2003 and 2008, respectively. He was a Post-DocResearch Fellow at the Centre for Autonomous Systems,University of Technology, Sydney, from July 2010 to July2011. He is currently a Lecturer at College of ElectronicInformation and Control Engineering, Beijing Universityof Technology, China. His current research interestsinclude fault detection, robust control, and mobile robotssimultaneous localization and mapping (SLAM) etc.

Shoudong Huang received the Bachelor and Master de-grees in Mathematics, Ph.D. in Automatic Control fromNortheastern University, P. R. China, in 1987, 1990, and1998, respectively. He is currently an Associate Professorat Centre for Autonomous Systems, Faculty of Engineer-ing and Information Technology, University of Technology,Sydney, Australia. His research interests include nonlinearcontrol systems and mobile robots simultaneous localiza-tion and mapping (SLAM), exploration and navigation.

Udo Frese is an assistant professor at theUniversity of Bre-men and leads the group for real time computer vision. Heis furthermore affiliated with the German Center for Arti-ficial Intelligence (DFKI) in Bremen. He received his Ph.D.degree from the Friedrich–Alexander-Universitat Erlan-gen–Nürnberg where he studied different aspects of thesimultaneous localization and mapping (SLAM) problems.He developed two successful mapping systems, namelyTreeMap and Multi-Level Relaxation (MLR). Additionally,he is working in the field of safety algorithms for robotsincluding areas such as collision avoidance.

Gamini Dissanayake is the James N Kirby Professor ofMechanical and Mechatronic Engineering at University ofTechnology, Sydney (UTS). His current research interestsare in the areas of localization and map building formobile robots, navigation systems, dynamics and controlof mechanical systems, cargo handling, optimization andpath planning. He leads the Centre for AutonomousSystems at UTS. He graduated in Mechanical/ProductionEngineering from the University of Peradeniya, Sri Lanka.He received his M.Sc. in Machine Tool Technology andPh.D. in Mechanical Engineering (Robotics) from the

University of Birmingham, England in 1981 and 1985, respectively.