EKF-based SLAM fusing heterogeneous landmarks

HAL Id: hal-01354861https://hal.archives-ouvertes.fr/hal-01354861

Submitted on 19 Aug 2016

HAL is a multi-disciplinary open accessarchive for the deposit and dissemination of sci-entific research documents, whether they are pub-lished or not. The documents may come fromteaching and research institutions in France orabroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, estdestinée au dépôt et à la diffusion de documentsscientifiques de niveau recherche, publiés ou non,émanant des établissements d’enseignement et derecherche français ou étrangers, des laboratoirespublics ou privés.

EKF-based SLAM fusing heterogeneous landmarksJorge Othón Esparza-Jiménez, Michel Devy, José-Luis Gordillo

To cite this version:Jorge Othón Esparza-Jiménez, Michel Devy, José-Luis Gordillo. EKF-based SLAM fusing hetero-geneous landmarks. 17th International Conference on Information Fusion ( FUSION ), Jul 2014,Salamanca, Spain. �hal-01354861�

https://hal.archives-ouvertes.fr/hal-01354861

https://hal.archives-ouvertes.fr

EKF-based SLAM fusing heterogeneouslandmarks

Jorge Othon Esparza-Jimenez∗†, Michel Devy∗, J. L. Gordillo †∗CNRS, LAAS, Univ de Toulouse, 7 avenue du colonel Roche, F-31400 Toulouse, France

Email: jorge.othon.esparza.jimenez, [email protected]†Centro de Sistemas Inteligentes, Tecnologico de Monterrey, Mexico

Email: [email protected]

Abstract—Visual SLAM (Simultaneous Localizationand Mapping from Vision) concerns both the spatial andtemporal fusion of sensory data in a map when movinga camera in an unknown environment. This paper con-cerns the construction of landmarks-based stochastic map,using Extended Kalman Filtering in order to fuse newobservations in the map, when considering heterogeneouslandmarks. It is evaluated how this combination allows toimprove the accuracy both on the map and on the cameralocalization, depending on the parametrization selected forpoints and straight lines. It is analyzed using a simulatedenvironment, so knowing perfectly the ground truth, whatare the better landmark representations. Experiments onimage sequences acquired from a camera mounted ona mobile robot, were already presented: it is detailedhere a new front end where segment matching has beenimproved.

I. INTRODUCTION

SLAM was introduced twenty-five years ago as afunction required for a robot, to build a map fromobservations acquired by embedded sensors, while ex-ploring an unknown environment, . It has been pro-posed many formal approaches to deal with the fusionof observations in the map (estimation, optimization,interval analysis. . . ), and many representations for theenvironment (landmarks, grids, raw data. . . ) [1]. Whileit is built, the map must allow the robot to estimate itspose; so many SLAM approaches are only devoted tobuild a landmarks-based map, i.e. a sparse model madewith distinctive and characteristic entities located in the3D space, which correspond without ambiguity, to someobserved features.

Visual SLAM has been studied since 12 years [2],using at first an estimation framework. It could findapplications not only in robotics, but also for the intro-duction of new services using smart devices equippedwith a camera (smart phones, Kinect. . . ). Landmarksare typically 3D points or 3D lines, that are observed bypoints or segments features in images; one observationdoes not allow to initialize such a landmark, while a 3Dpoint is observed by an optical ray (i.e. a 3D straight

line), and a 3D line is observed by an interpretationplane.

So in order to initialize such landmarks with theirminimal euclidean parameters, it is necessary to wait forother observations. It is the reason why the first strategyproposed in [2] [3] applied a delayed initialization; alandmark was added in the map only when it was knownin the euclidean space. This approach is unable to uselandmarks that are very far from the robot. So severalparametrizations have been proposed for points [4][5] orlines [6] landmarks, for their undelayed initialization,i.e. they are added in the map as soon as they areobserved. Sola and al [7] have analyzed the pros andcons of several representations for 3D points and 3Dlines, before they can be triangulated with a sufficientaccuracy.

Here this analysis is made more complete using anheterogeneous map, where points and lines are bothinitialized as soon as they are detected from featuresextracted in images acquired by a camera moved inthe environment. It is now known [8] that optimization-based methods like PTAM [9] or methods based on theg2o library [10] allow to avoid the possible divergenceof methords based on estimation, due to linearizationof the observation model. Nevertheless here the fusionis performed from an EKF-based SLAM method, as avery light approach that can be integrated on dedicatedarchitecture using co-design methodology, to be usedon small aerial vehicles.

In the next section it is recalled the differentparametrization proposed for points and lines land-marks. Then the sections III and IV summarize the waylandmarks are initialized as soon as they are observed,and then updated from next observations. Finally, It isanalyzed in section V, what are the best landmarks rep-resentations when a heterogeneous map is built. Also,in the section VI, it is shown some results obtained onimage sequences acquired from a mobile robot movingon a road close to buildings.

II. LANDMARK PARAMETERIZATION

In [7], Sola et al. introduce an undelayed landmarkinitialization (ULI) for different points and lines param-eterizations. It consists on substituting the unmeasureddegrees of freedon by a Gaussian prior that handlesinfinite uncertainty but that is still manageable by EKF.

For point and line landmark , uncertainty has distinctimplications. For points, there is uncertainty in depthand it covers all the visual ray until infinity. Infinitestraight lines handles uncertainty in two degrees offreedom, which correspond to a depth that should becovered up to infinity, and all possible orientations.

A. 3D point parameterizations

This section explains some point parameterizations.The aspects included in each description refer to theparameterization itself, camera projection, coordinatetransformation, and back-projection.

1) Euclidean point: The parameters of an Euclideanpoint consist on its Cartesian coordinates.

LEP = p = [x y z]T ∈ R3

The projection to camera frame is given by thefollowing equation:

u = KRT (p− T) ∈ P2 (1)

where,

K =

[αu 0 u0

0 αv v0

0 0 1

]

R and T are the rotation matrix and translationvector that define the camera C. Underlined vectors likeu represent homogeneous coordinates.

2) Homogeneous point: Homogeneous points areconformed by a 4-vector, which is composed by the3D vector m and scalar ρ.

LHP = p =

[mρ

]= [mx my mz ρ]

T ∈ P3 ⊂ R4

In order to convert from homogeneous to Euclideancoordinates, the following equation is applied:

p =m

ρ. (2)

In the camera frame, m is the director vector of theoptical ray, and ρ has a linear dependence to the inverseof the distance d defined from the optical center to thepoint.

ρ = ‖m‖d

This allows to express the unbounded distanceof a point along the optical ray from 0 to infinity,into this bounded interval in parameter space ρ ∈(0, ‖m‖/dmin].

The frame transformation of an homogeneous pointis performed according to the next equation:

p = HpC =

[R T0 1

]pC, (3)

where super-index C indicates the frame to whichthe point is refered, and matrix H sepecifies the frameto which the point is transformed.

The projection of a point into the image frame isperformed with the following expression:

u = KRT (m− Tρ) ∈ P2. (4)

Expressing an homogeneous point in the cameraframe, the projected image point is u = KmC, andρC is not measurable. Back-projection is then:

mC = K−1u.

The complete homogeneous point parameterizationis given in the following equations:

LHP = p =

[mρ

]= H

[K−1uρC

]=

[RK−1u + TρC

ρC

] (5)

,

where ρC must be given as prior and representsinverse-distance from the origin of coordinates.

Homogeneous point parameterization is shown infigure 1.

Z

X

Y

EKF-SLAM with heterogeneous landmarks

Michel Devy Jorge Othon Esparza-Jimenez

Abstract—

I. INTRODUCTION


In [?], Sola et al. introduce an undelayed landmarkinitialization (ULI) for different points and lines param-eterizations. It consists on substituting the unmeasureddegree of freedon by a Gaussian prior that handlesinfinite uncertainty but that is still manageable my theEKF.

For different landmark types such as points andlines, uncertainty has distict implications. For points,there is uncertainty in distance and it covers all thevisual ray until infinity. Infinite straight lines handlesuncertainty in two degrees of freedom, which corre-spond to a distance that should be covered up to infinity,and all possible orientations. In the next sections, pointand line parameterizations used for ULI are covered, forthen deepen in the initialization process itself.



Figure 1: Point parameterizations. Image extracted from[?]


LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ. (2)


ρ = �m�d

This allows to express the unbounded distanceof a point along the optical ray from 0 to infinity,into this bounded interval in parameter space ρ ∈(0, �m�/dmin].


p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)




Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ. (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)




Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ, d = �m�/ρ. (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)




Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ.O (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)


Figure 1: Homogeneous point parameterization.

3) Anchored homogeneous point: In order to im-prove linearity, an anchor is added as a reference tothe optical center at initialization time of the landmark.

2

Thus, the landmark is a 6-vector that includes the anchor3D coordinates, the Cartesian coordinates of the pointwith respect to the anchor, and an inverse-depth scalar.

LAHP =

[p0

mρ

]= [x0 y0 z0 mx my mz ρ]

T ∈ R7.

The convertion from anchred homogeneous point toEuclidean coordinates can be achieved by the followingequation:

p = p0 +m

ρ. (6)

The projection and frame transformation process isgiven in the next expression:

u = KRT (m− (T− p0) ρ) ∈ P2. (7)

The complete anchores homogeneous point param-eterization is the following:

LAHP =

[p0

mρ

]=

TRK−1uρC

, (8)

where ρC must be given as prior.

Anchored homogeneous point parameterization isshown in figure 2.

Z

X

Y



Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ. (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)




Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ. (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)




Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ, d = �m�/ρ. (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)




Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ.O (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)


Figure 2: Anchored homogeneous point parameteriza-tion.

B. 3D line parameterizations

In this section, some line parameterizations are cov-ered. The description of projection to image frame, bi-linear transformation and back-projection are included.

1) Plucker line: A line in P3 defined by two pointsa = [a a]

T and b = [b b]T can be represented as

homogeneous 6-vector, known as Plucker coordinates:

LPL =

[nv

]= [nx ny nz vx vy vz]

T ∈ P5 ⊂ R6 ,

where n = a × b, n = ab − ba, n,v ∈ R3, andhaving the following Plucker constraint: nTv = 0.

Geometrically speaking, n is the vector normal tothe plane π containing the line and the origin, andv is the director vector from a to b. The Euclideanorthogonal distance from the line to the origin is givenby ‖n‖/‖v‖. Thus, ‖v‖ is the inverse-depth, analogousto ρ of homogeneous points. Plucker line geometricalrepresentation is shown in figure 3.



Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ.O (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)


mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,


3) Anchored homogeneous point: In order to im-prove linearity, an anchor is added as a reference tothe optical center at initialization time of the landmark.Thus, the landmark is a 6-vector that includes the anchor3D coordinates, the Cartesian coordinates of the pointwith respect to the anchor, and an inverse-depth scalar.

LAHP =

�p0

mρ

�= [x0 y0 z0 mx my mz ρ]

T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv

�= [nx ny nz vx vy vz]

T ∈ P5 ⊂ R6 ,


Geometrically speaking, n is the vector normal tothe plane π containing the line and the origin, andv is the director vector from a to b. The Euclideanorthogonal distance from the line to the origin is givenby �n�/�v�. Thus, �v� is the inverse-depth, analogousto ρ of homogeneous points. Plucker line geometricalrepresentation is shown in figure 2.

Figure 2: Plucker line geometrical representation. Imageextracted from [?]

Plucker coordinates transformation from cameraframe is performed as shown next:

LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.

The whole transformation and projection process forPlucker coordinates in terms of R , T, n, and v is:

l = K · RT · (n− T× v) (9)

where K is the instrinsic projection Plucker matrixdefined as:

K =

�αv 0 00 αu 0

−αvu0 αuv0 αuαv

�.

When Plucker coordinates are expressed in cameraframe, projection is only obtained by

l = K · nC (10)

Line’s range and orientation expressed in vC are notmeasurable.

For Plucker line back projection, vectors nC and vC

are computed according to these expressions:

2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v) (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v) d =�n��v� (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)π (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)π (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)π (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)π (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

Figure 3: Plucker line geometrical representation.


LPL = H · LCPL =

[R [T]× R0 R

]·[

nC

vC

].


l = K · RT · (n− T× v) C (9)


K =

[αv 0 00 αu 0


].


l = K · nC (10)




nC = K−1 · lvC = β1 · e1 + β2 · e2

where β1, β2 ∈ R and{e1, e2,n

C}

are mutuallyorthogonal.

3

Defining β = (β1, β2) ∈ R2, vector vC can be alsoexpressed as:

vC = E · β,

where vC ∈ πC for any value of β.

Plucker line back projection is shown in figure 4.

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)

where β1,β2 ∈ R and�e1, e2,n

C�


Defining β = (β1,β2) ∈ R2, vector vC can be alsoexpressed as:

vC = E · β, (13)


In order for �β� to be exactly inverse-distance ande1 be parallel to the image plane, e1 and e2 are obtainedwith the next expressions:

e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)


Figure 3: Plucker line back-projection. Image extractedfrom [?]

The complete Plucker line parameterization is thefollowing:

LPL = H�

nC

vC

�= H

�K−1lEβ

�=

�RK−1l + T× REβ

REβ

�,

(16)

where β must be provided as a prior.

2) Anchored Homogeneous-points line: Anotherway of representing a line is by the endpoints thatdefine it. Departing from the anchored homogeneouspoint parameterization, an homogeneous-point line is an11-vector defined as follows:

LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11

For each point, the transformation and projection ofa pinhole camera is , as previously stated,

ui = KRT (mi − (T− p0) ρi) (17)

An homogeneous 2D line is obtained by the crossproduct of two points lying on it, l = u1×u2 and thus,

l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)

Comparing this result to what was obtained forPlucker coordinates, it can be seen that the productm1 × m2 is a vector orthogonal to the plane π,analogous to the Plucker sub-vector n. Also, the term(ρ1m2 − ρ2m1) is a vector joining the two supportpoints of the line, therefore related to Plucker sub-vectorv.

Figure 4 shows this parameterization.

Figure 4: Anchored homogeneous-points line parame-terization. Image extracted from [?]

III. LANDMARK INITIALIZATION

Points are stacked as a 2-vector containing Cartesiancoordinates in pixel space, and are modeled as a Gausianvariable.

u =

�uv

�∼ N {u,U}

In homogeneous coordinates,

3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)π (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)πC (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v) I (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v) C (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

Figure 4: Plucker line back-projection.


LPL = H[

nC

vC

]= H

[K−1lEβ

]=

[RK−1l + T× REβ

REβ

],

(11)



LAHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) (12)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(13)





Abstract—

I. INTRODUCTION








LEP = p = [x y z]T ∈ R3


u = KRT (p− T) ∈ P2 (1)

where,

K =

�αu 0 u0

0 αv v0

0 0 1

�



LHP = p =

�mρ

�= [mx my mz ρ]

T ∈ P3 ⊂ R4


p =m

ρ.O (2)


ρ = �m�d



p = HpC =

�R T0 1

�pC, (3)



u = KRT (m− Tρ) ∈ P2. (4)


mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v) (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

mC = K−1u.


LHP = p =

�mρ

�= H

�K−1uρC

�=

�RK−1u + TρC

ρC

� (5)

,



LAHP =

�p0

mρ


T ∈ R7.


p = p0 +m

ρ. (6)


u = KRT (m− (T− p0) ρ) ∈ P2. (7)


LAHP =

�p0

mρ

�=

TRK−1u

ρC

, (8)







LPL =

�nv


T ∈ P5 ⊂ R6 ,





LPL = H · LCPL =

�R [T]× R0 R

�·�

nC

vC

�.


l = K · RT · (n− T× v)π (9)


K =

�αv 0 00 αu 0


�.


l = K · nC (10)




2

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) �m1�/ρ1 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi) �m2�/ρ2 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi)m2 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi)m1 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi)p1 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi)p2 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

nC = K−1 · l (11)vC = β1 · e1 + β2 · e2 (12)


C�



vC = E · β, (13)



e1 =

�nC

2 −nC1 0

�T��

nC1

�2+�nC

2

�2 · �nC� (14)

e2 =nC × e1

�nC� (15)




LPL = H�

nC

vC

�= H

�K−1lEβ

�=


REβ

�,

(16)



LAHPL =

p0

m1

ρ1

m2

ρ2

∈ R11


ui = KRT (mi − (T− p0) ρi)p2 (17)


l = KRT ((m1 ×m2)− (T− p0)× (ρ1m2 − ρ2m1)) .(18)






u =

�uv

�∼ N {u,U}


3

Figure 5: Anchored homogeneous-points line parame-terization.



u =

[uv

]∼ N {u,U}


u =

[u1

]∼ N {u,U} = N

{[u1

],

[U 00 0

]}

In the case of lines, they can be expressed asbounded segments by means of their endpoints in a 4-vector, also with a Gaussian probability distribution.

s =

[u1

u2

]∼ N {s,S} = N

{[u1

u2

],

[U 00 U

]}

The probability distribution function for infinite lineslike Plucker, pdfN

{l,L}

, uses the homogeneous linerepresentation previously shown, and the Gaussian dis-tribution defined by :

l = u1 × u2, and L = [u1]×U [u1]T× + [u2]×U [u2]

T×

The uncertainty in 3D points and lines coming fromprojection is kept and modeled in inverse-distance priorsρC and βC through Gaussian variables. The origin ofeach of these priors must be inside the 2σ of the theirprobability density functions.

For points and point supported lines, the minimumdistance must match the upper 2σ bound, thus:

4

ρ− nσρ = 0, 0 ≤ n < 2

ρ+ 2σρ = 1/dmin

Being n = 1, leads to,

ρ = 1/3dmin, and σρ = 1/3dmin

The probability distribution function of a point sup-ported line is defined as tC ∼ N

{¯t;T}

, with:

t =

[ρρ

], T =

[σ2ρ 0

0 σ2ρ

]

Plucker lines prior βC ∼ N{β;B

}take the follow-

ing values:

β =

[1/3dmin

0

], and

B =

[(1/3dmin)

20

0 (1/2dmin)2

]

This penalizes lines at the back of the camera.

A. Undelayed landmark initialization algorithm

The ULI algorithm, as presented in [7] is composedby the followings steps:

1) Identify mapped magnitudes x ∼ N {x,P}.2) Identify measurements z ∼ N {z,R}, where z

is either point or line (i.e. u or s respectively).3) Define Gaussian prior π ∼ N {π; Π} for

unmeasured degree of freedom. π can eitherbe ρC, tC or βC.

4) Back-project the Gaussian measurement andget landmark mean and Jacobians.

L = g(C, z, π

)

GC = dgdC

∣∣∣C,z,π

, Gz = dgdz

∣∣∣C,z,π

,

Gπ = dgdπ

∣∣∣C,z,π

5) Compute landmarks co- and cross-variances.PLL = GCPCCG

TC + GzRGz

T + GπΠGTπ

PLx = GCPCx = GC [PCC PCM]6) Augment SLAM map

x←[

xL]

P←[

P PTLx

PLx PLL

]

IV. LANDMARK UPDATE

The landmark update process starts by projecting alllandmarks to the image plane, and selecting those withhigher uncertainty to be corrected. In the case of points,the observation function h() applies an homogeneous toEuclidean transformation h2e() to the transformation +projection proceses previously presented.

z = h2e(u) =

[u1/u3

u2/u3

]∈ R2

Innovation y is then computed as

Innovation mean: y = z− h (x)

Innovation covariance: Y = R + H ·P ·HT

where R = U the measurement noise covarianceand Jacobian H = ∂h

∂x

∣∣x

.

For lines, observation function computes the orthog-onal distances from the detected endpoints ui to a linel.

z =

[lT · u1/

√l21 + l22

lT · u2/√l21 + l22

]∈ R2

Being the EKF innovation, the difference betweenthe actual measurement and the expectation y = z −h (x), z is the orthogonal distance from the endpointsto the line defined by them. Thus

y = 0− h (x)

A landmark is found consistent if the squared Ma-halanobis distance MD2 of innovation is smaller thana threshold MD2th

MD2 = yT ·Y−1 · y < MD2th

Being that the case, landmark is updated

Kalman gain: K = P ·H ·Y−1

State update: x← x + K · yCovariance update: P← P−K ·H ·P

V. EXPERIMENTAL RESULTS

This section presents the results of the simula-tion experiments performed for comparing the differentpoint and lines parameterizations with heterogeneousapproches that combine points and lines in the samemap.

The experiments were performed in MATLABr,departing from the EKF-SLAM toolbox [11] and addingthe heterogeneous functionality.

The following parameterizations were evaluated:

• Anchored Homogenenous Point (AHP)

• Plucker Line (PL)

• Anchored Homogenenous Point Line (AHPL)

• AHP + PL

• AHP + AHPL

The environment consisted on a house conformed by23 lines and an array of 16 points distributed equally

5

among the walls of the house. The environment for thesimulation is shown in figure 6.

The robot performs a circular trajectory of 5 m ofdiameter, with a pose step of 8 cm and 0.09◦. the linearnoise is 0.5 cm and the angular noise 0.05◦

Figure 6: Environment world of the simulation experi-ments.

In addition to the heterogeneous landmark handlingof the toolbox, two more considerations were integratedto evaluate the performance of the parameterizations ina more realistic way. The first one was the transparencyof the objects in the scene. Normally, the objects inthe simulaton enviroment of the toolbox are transparent,allowing to have an almost complete view of the land-marks during all the time steps. An aspect graph wasimplemented in order to see only the visible surfaces ofthe house from each camera position. Also, by defaultthe algorithm is aware of loops, so it automaticallyperforms feature matching, which allows to have a moreaccurate position estimation. It was modified in orderto have the capability of initializing the landmarks asnew ones on each turn and evaluate the performance inthat case too. Thus, four different setup conditions wereconsidered:

• Transparent objects with loop acquaintance.

• Transparent objects without loop acquaintance.

• Opaque objects with loop acquaintance.

• Opaque objects without loop acquaintance.

Figure 7 shows the sensor view considering transpar-ent objects, while figure 8 shows the case with opaqueobjects. Figure 9 gives an example of the environmentdisplayed after a complete turn with Plucker line +inverse depth point parameterization. It displays in greenthe line landmarks estimated, and in blue the point land-marks. Real, predicted, and estimated robot trajectoriesare displayed in blue, red, and green respectively.

A trajectory of 5 turns was performed for eachparameterizaton and each condition mentioned earlier.

Figure 7: Sensor view considering transparent objects.

Figure 8: Sensor view considering opaque objects.

Figure 9: Image of the environment after performing acomplete turn with Plucker line + inverse depth pointparameterization.

The position error of the robot for each case is shownin figures 10, 11, 12, and 13.

Among the parameterizations, the highest error cor-respond to Plucker line. The best performance cor-respond to the anchored parameterizations, both forpoints and lines. It can be seen the improvement effectin Plucker line parameterization by the addition ofpoints. Even for the anchored parameterizations, alreadyhaving a relatively well performance while workingindependently, the heterogeneity brings benefits, in sucha way that the combination of both AHP and AHPL is

6

0 200 400 600 800 1000 1200 1400 1600 1800 20000

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Posi

tion

estim

ated

erro

r(m

)

AHPPLAHPLAHP + PLAHP + AHPL

Figure 10: Transparent objects with loop acquaintanceposition estimation errors.

0 200 400 600 800 1000 1200 1400 1600 1800 20000

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

Posi

tion

estim

ated

erro

r(m

)


Figure 11: Transparent objects without loop acquain-tance position estimation errors.

0 200 400 600 800 1000 1200 1400 1600 1800 20000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2

Posi

tion

estim

ated

erro

r(m

)


Figure 12: Opaque objects with loop acquaintance po-sition estimation errors.

the one with the least error along the major part of thesimulated trajectories.

0 200 400 600 800 1000 1200 1400 1600 1800 20000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

Posi

tion

estim

ated

erro

r(m

)


Figure 13: Opaque objects without loop acquaintanceposition estimation errors.

VI. REAL IMAGES

The evaluation on real images is on the way; someresults have been already presented, especially in [12].Here the contribution concerns the integration of theLSD segment detector presented in [13] and a movingedge tracker based on [14] [15].

Figure 14 shows a set of frames of a sequence thathave been processed for the tracking of automaticallydetected linear segments, and the detecton of points.

VII. CONCLUSION

This paper intends to prove the benefits of consid-ering heterogeneous landmarks when building a mapfrom an EKF-based visual SLAM method. Using onlymonocular vision, only partial observations of land-marks are provided by features extracted from images;here it is used undelayed initialization of landmarkslikeit was proposed initially by Solaand al [5] [6] for pointsand segments. It has been shown using simulated data,how the choice of the landmarks representation hasan impact on the map accuracy. Finally the best onesconsidering the construction of map with heterogeneouslandmarks, are Anchored Homogeneous Points and An-chrored Homogeneous-PointsLines.

Experiments with real images are on the way, andsome preliminary results have been presented on linetracking using different sequences acquired in urbanenvironment. In the future works, constraints will be ex-ploited in the map, typically when points and segmentsare extracted from the same facades.

ACKNOWLEDGMENT

This work was performed during a long-term stay ofJ.O.Esparza Jimenez, PHd student in ITESM Monterrey,Mexico, in the robotics department of LAAS-CNRS in

7

France. The authors would like to thank CONACYTfor the funding of this stay; this work contributes to thefrench FUI project AIR-COBOT.

REFERENCES

[1] H. Durrant-Whyte and T. Bailey, “Simultaneous Localizationand Mapping (SLAM): Part I & II,” IEEE Robotics & Automa-tion Magazine, 2006.

[2] A. J. Davison, “Real-time simultaneous localisation andmapping with a single camera,” in Proceedings of theNinth IEEE International Conference on Computer Vision -Volume 2, ser. ICCV ’03. Washington, DC, USA: IEEEComputer Society, 2003, pp. 1403–. [Online]. Available:http://portal.acm.org/citation.cfm?id=946247.946734

[3] T. Lemaire, S. Lacroix, and J. Sola, “A practical 3d bearing-only slam algorithm,” in Intelligent Robots and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International Conference on,2005, pp. 2449 – 2454.

[4] J. M. M. Montiel, “Unified inverse depth parametrization formonocular slam,” in Proc. Robotics: Science and Systems(RSS), 2006.

[5] J. Sola, A. Monin, M. Devy, and T. Lemaire, “Undelayedinitialization in bearing only slam,” in Proc. IEEE/RSJ Int.Conf. on Intelligent Robots and Systems (IROS), 2005.

[6] J. Sola, V. C. T., and M. Devy, “Undelayed initialization of linesegments in monocular slam,” in Proc. IEEE/RSJ Int. Conf. onIntelligent Robots and Systems (IROS), 2009.

[7] J. Sola, T. Vidal-Calleja, J. Civera, and J. M. M. Montiel,“Impact of landmark parametrization on monocular ekf-slamwith points and lines,” International journal of computer vision,vol. 97, no. 3, pp. 339–368, 2012.

[8] H. Strasdat, J. Montiel, and A. Davison, “Real-time monocularslam: Why filter?” in Proc. IEEE Int. Conf. Robotics andAutomation (ICRA), May 2010.

[9] G. Klein and D. Murray, “Parallel tracking and mapping forsmall AR workspaces,” in Proc. 6th IEEE and ACM Int. Symp.on Mixed and Augmented Reality. IEEE Computer Society,2007.

[10] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, andW. Burgard, “g2o: A general framework for graph optimiza-tion,” in Proc. IEEE Int. Conf. on Robotics and Automation(ICRA), 2011.

[11] J. Sola, D. Marquez, J. Codol, and T. Vidal-Calleja, “An EKF-SLAM toolbox for MATLAB,”http://homepages.laas.fr/jsola/JoanSola/eng/toolbox.html,2009.

[12] T. Vidal-Calleja, C. Berger, J. Sola, and S. Lacroix, “Large scalemultiple robot visual mapping with heterogeneous landmarksin semi-structured terrain,” Robotics and Autonomous Systems,vol. 59, no. 9, pp. 654—-674, 2011.

[13] R. Grompone von Gioi, J. Jakubowicz, J.-M. Morel, andG. Randall, “LSD: a Line Segment Detector,” Image ProcessingOn Line, vol. 2, pp. 35–55, 2012.

[14] P. Bouthemy, “A maximum likelihood framework for determin-ing moving edges,” Pattern Analysis and Machine Intelligence,IEEE Transactions on, vol. 11, no. 5, pp. 499–511, 1989.

[15] A. I. Comport, E. Marchand, M. Pressigout, and F. Chaumette,“Real-time markerless tracking for augmented reality: the vir-tual visual servoing framework,” Visualization and ComputerGraphics, IEEE Transactions on, vol. 12, no. 4, pp. 615–628,2006.

Figure 14: Some frames of the sequence are includedand the detected/tracked segments are shown in green,blue and yellow, while detected points are displayed inred.

8

Documents

EKF-based SLAM fusing heterogeneous landmarks