D Gonzalez-Aguirre

E Bayro-Corrochano

R Dillmann

T Asfour

CiteSeerX

Model-Based Visual Self-Localization using Geometry and Graphs
D. Gonzalez-Aguirre, T. Asfour, *E. Bayro-Corrochano, R. Dillmann
Institute of Computer Science and Engineering, University of Karlsruhe-Germany
*CINVESTAV, Guadalajara-Mexico
{gonzalez,asfour,dillmann}@ira.uka.de, *edu@gdl.cinvestav.mx
Abstract
In this paper, a geometric approach for global self-
localization based on a world-model and active stereo
vision is introduced. The method uses class specific ob-
ject recognition algorithms to obtain the location of en-
tities within the surroundings. The perceived entities in
recognition trials are simultaneously filtered and fused
to provide a robust set of class features. These classi-
fied perceptions which simultaneously satisfy geometric
and topological constraints are employed for pruning
purposes upon the world-model generating the location
hypotheses set. Finally, the hypotheses are validated
and disambiguated by applying visual recognition al-
gorithms to selected entities of the world-model. The
proposed approach has been successfully used with a
humanoid robot.
1. Introduction
The self-localization capability is essential for au-
tonomous systems, like humanoid robots, operating in
built-for-humans environments, where the use of vision
is the only natural approach. In those structured envi-
ronments the geometrical and topological interrelations
of the elements provide substantial advantages for fea-
ture extraction, object recognition and self-localization.
Self-localization can be categorically divided into
global and fine localization [8]. The first one considered
in this paper determines the position and orientation of
the robot (pose) within a world coordinate system U ,
see Fig.6. The second one deals with the continuous
state (dynamic-pose) of the robot.
So far, vision-based self-localization approaches
([4],[8]) have been commonly conceptualized as the ex-
traction and processing of image features, which by
means of recursive state estimations provide the contin-
uous location of the camera(s). However, partially sig-
nificant visual landmarks (stored scale invariant featu-
res) and assessed poses (only linked to the unknown ini-
tial pose) provide insufficient useful information for real
applications. In these cases, the robot requires plenary
environmental information (vision-to-model coupling)
to actively interact with its environment, i.e. solving as-
sertions concerning the status of its world, visual plan-
ning, grasping, etc. These limitations can be overcome
with a proper mechanism which provides fast and reli-
able global localization by systematically exploiting the
intrinsic natural constraints accessible through an effec-
tive and consistent world-model representation.
2. Outline of Visual Self-Localization
This approach1 consists of a collection of active
visual perception-recognition components, a world-
model and a hypotheses generation-validation appara-
tus, see Fig.1.
2.1. Object Recognition
The basic inputs are perceived-recognized objects,
i.e. Percepts, see Fig.1-a. For instance, but not limited
to handles, doors or windows in a building, see Fig.2.
The advantage of using class-based object recognition
schemas has been previously exploited [10]. In this way
not only fast and robust methods are applied but also
the data association between features and model enti-
ties is partially2 solved. In contrast, general feature ap-
proaches [8] lack of model association while offering
poor reliability compared to those specific ones.
In this approach, doors and handles are robustly re-
cognized by means of gaussian classification over char-
acteristics feature spaces extracted from class specific
descriptors of the eigenvectors3 of corresponding color-
segmented regions from stereo images, see Fig.2-b.
1Additional material at http://i61www.ira.uka.de/users/gonzalez/
2Up to the class-instance association level.
3From the covariance matrix of the color-clustered regions.
978-1-4244-2175-6/08/$25.00 ©2008 IEEE
Figure 1. The model-based visual self-localization approach. a) Visual perception-recognition components. b) Recogni-
tion fusion. c) Percept subgraphs. d) World-model at graph pruning. e) Hypotheses generation-validation. f) Pose estimation.
Subsequently, the left-right cross match using size, po-
sition, orientation, perpendicular distance to the epipo-
lar line and standard disparity constraints [7] allows
powerful rejection of the remaining outliers.
Figure 2. Class specific object recognition.
The used growing region generation criteria admit
pixels considering the size of the region (Si < Smax)
and the volume (Vi < Vmax) of the ellipsoid expanded
by points Kmean, Kmin and Kmax, see Fig.2-a. After-
wards, 5D feature vectors
Fi := [Xc,K
T
mean]
T , (1)
are used to compute the mean shift algorithm [3] for
clustering regions, which present chromatic and spa-
tial proximity, i.e. Blobs. In this manner, the shifting
method benefits from processing less (but still coherent)
features. Experimental results show a reduction from
2000-3000ms to 100-200ms.
Subsequently, the blobs representing doors were re-
cognized by means of the descriptor
Vd := [
dρ1max
dρ1min
,
dρ2max
dρ2min
,
Si
Li
, θ1, θ2]
T , (2)
which components involve the ratios of the maximal-
minimal lengths (the projection from pixels along each
blob axis), the elasticity (ratio area-perimeter Si
Li
) and
the angles θ1 and θ2 (see Fig.2-c), which restrict the con-
vexity and skewness of the blob. Afterwards, the de-
scriptor of the handle is given by
Vh := [
ρ1
ρ2
, arccos(Q̂ · K̂mean), ||Q−Kmean||]T , (3)
where ρ1 and ρ2 denote the eigenvalues of the blob.
Here, the components represent the axis-compactness
and the angle-length discrepancy between the mean
color of the blob and the ideal color of the handle. The
remaining outliers are discarded using the Shi-Tomasi
response function [9] to verify the existence of two par-
allel edges meeting at clear corners on both ends.
In addition, many specific recognition components
may be added to improve the performance of the system
at graph filtering by increasing the amount of partitions
of the graph, i.e. reinforcing constraints and increasing
pruning.
2.1.1 Ego Perception and Recognition Fusion. De-
spite the robustness of the class specific algorithms and
due to certain phenomena (varying illumination, singu-
larities of the field of view, etc.) false positives might
sporadically occur. In order to anticipate these situa-
tions, all recognized objects are related to the ego center
Xego of the robot by considering its kinematic configu-
ration while executing the scanning-strategy, i.e. the
planned trajectory for capturing stereo images using the
head of the robot. The resulting registration data struc-
tures include frame identifier, type of the percept and its
3D location.
In the next phase, fusion begins by calculating dis-
tances between percepts of same type in frame t and
those found within the frame range [ς0(t),ς1(t)], herein
the functions ς0(t) and ς1(t) provide the first and last
frame that share visual space with the frame t. As a re-
sult, percepts which closest distance to other percepts
exceeds the threshold4  are ignored.
Within this phase, there is a tacit underlying 3D
multimodal spatial-density function ∂̂α(X) : 3 →
 of the percepts type α (see Fig.1-b), which im-
plies that the stationary points X{αi,ς0(t),ς1(t)} (the lo-
4The size of the 3D field of view.
2
cations of the α-modes) describe the fused locations
of the α-elements of the set. Elements converging to
X{αi,ς0(t),ς1(t)} constitute the fusion set (cluster delin-
eation in [3]) with a cardinality C{αi,ς0(t),ς1(t)}. Finally,
the pointX{αi,ς0(t),ς1(t)} allows to determine the amount
of frames Ts in which the corresponding percept has to
be found. Consequently, subsets carrying through the
minimal confidence criterion
C{αi,ς0(t),ς1(t)}
Ts(X{αi,ς0(t),ς1(t),ι})
> Emin (4)
are merged into a fused percept Opfi , where the para-
meter ι relaxes the calculation contemplating errors in-
troduced by noisy percepts and ego-mapping. Conclu-
sively, the collection of all Opfi forms the set Hf , see
Fig.1-b,c.
2.2. World-Model
The world-model has two levels of abstraction. On
the first level, the 3D vertices and their composition des-
cribing geometric primitives are stored. On the second
level, these structures compose instances of object mod-
els Omi with attributes, e.g. identifier, type, size and
pose. The collection of object models instances consti-
tutes the node set
Ξ := {Omi }i=1,...,n, (5)
whereas the link set
Λ ⊂ {Omi ×Omj : i > j, |Xi −Xj | < } (6)
depicts the connections λi,j formed by all object model
instances which relative distances fall below the thresh-
old  .
The considered world-model5 kitchen consists of
611 rectangular prisms, 124 cylinders, 18 general poly-
hedra with 846 faces all arranged by 1,524 general
transformations (rotation, translation and scaling) with
a total of 13,853 vertices and 25,628 normal vectors
composed in the scene-graph6 from the construction
CADmodel and verified against real furniturewith laser
devices, see Fig.6.
2.3. Graph Pruning
The previous world representation has been enriched
with schemas, which not only integrate and filter the
ideal graph model with the noisy percepts, but also cre-
ate constraints Ωξ
〈
Omi , O
p
j
〉
yielding to the hypotheses
set Δ, see Fig.1-c,e.
2.3.1 Proximity Filtering. When our algorithm filters
links in the world graph, noise is taken into account as
deviation
εi ∼= 1
ζ
(||Xfi − CL||)2, (7)
5Human-centered environment [1].
6Extending http://www.coin3d.org/
this is a function describing the distance between the
perceived-recognized objectsOpfi with locationsXi and
the center of the left camera CL [6].
The result of the filter is a set of links ψ{α,β,φ,τ} ⊂
Λ connecting nodes of type α to nodes type β which are
separated by a distance φ with an error-tolerance τ =
maxi∈Θ(εi), where Θ denotes the subset of objects of
both types
ψ{α,β,φ,τ} ⊂ {Om(i,α) × Om(j,β) : (φ− ||Xi −Xj||) < τ}. (8)
The active link set ψact consists of nodes from the in-
tersection ofm proximity filtering results
ψact :=
m⋂
i
ψ{αi,βi,φi,τi}. (9)
Each filtering stage O(n) produces a remarkable reduc-
tion of the cardinality of the set ψact, i.e. the remain-
ing nodes should have neighbours with restricted types
at constrained distance ranges. A high performance
was accomplished by means of dynamic programming
techniques (distances-lookup tableO(n2) ) filtering only
previously selected nodes.
2.3.2 Orientation Filtering. A more powerful tech-
nique to reduce the nodes cardinality in the set
ψact consists of accepting only those elements which
incidence-neighbour nodes have a relative pose, i.e.
a displacement vector V pf
i, 
jk
from the neighbour node
O
pf
j to a third linked node O
pf
k
in terms of the created
reference frame Si,j,k
Percept
, which is linked to the ego-
perception frame, see Fig.3.
Σ
Figure 3. The world-model graph being pruned
by means of orientation filtering, with a complexity
O(m2), where m is the degree of the node being fil-
tered.
In this sense, the definition of the frame has to be
consistent while considering the noisy nature of the per-
cepts, as follows: first, three non-collinear elements
O
pf
i , O
pf
j and O
pf
k are selected from Hf specifying
the frame Si,j,k
Percept
:= [Ri,j,k
Percept
, Xfi ] relative to the ego-
perception frame7
δ1 = X
f
j −Xfi , δ2 = δ1 × (Xfk −Xfi ),
7Which orthonormal basis vectors are {ê1, ê2, ê3}.
3
δ3 = δ1 × δ2, Ri,j,kPercept =
[
δ̂n · ên
]
n=1,...,3
.
Next, the relative displacement is computed
V
pf
i, 
jk
= Si,j,k
Percept
(X
pf
j −X
pf
k
). (10)
Therefore, it is possible to reject nodes which do not
have a “similar” displacement vector among two of
their neighbours with corresponding type and proxim-
ity. This noisy-similarity is made quantifiable as the dis-
crepancy length μ and the angle κ between the percepts
V p
i, 
jk
and those from the model V m
u, 
uw
, expressed on the
world-model frame Su,w,v
model
. Fig.3 shows the subspace
Σ bounded by ||V p
i, 
jk
− V m
u, 
vw
|| < μmax and arccos(V̂ p
i, 
jk
·
V̂ m
u, 
vw
) < κmax. When filtering nodes, the combinational
burden is reduced by computing only subgraphs which
link lengths fall into the range ||V p
i, 
jk
|| ± μmax.
2.4. Hypotheses Generation and Validation
The sequence of previous stages extracts model
subgraphs, which simultaneously match the typed-
incidences and the relative-poses of acquired percept
subgraphs. Latter associations establish the coupling
between the visual space, world-model and physical
world, see Fig.1-c,d. In fact they impose restraints
which are the geometric-compelling keys to deduct the
6D pose of the robot.
Each association
〈
O
pf
i , O
m
j
〉
constrains the position
of the robot Xego to a subspace of all points which are∣∣∣∣Xpfi ∣∣∣∣ units away from Xmj . This subspace is in fact
a sphere Ω
〈
O
pf
i , O
m
j
〉
centered at Xmj (the position of
the matched world-model node) with a radius
∣∣∣∣Xpfi ∣∣∣∣,
i.e. the distance from the fused percept to the ego-
center, see Fig.4-a. Now considering Ω1
〈
O
pf
i , O
m
j
〉
and
Ω2
〈
O
pf
k , O
m
l
〉
, two restriction spheres (see Fig.4-b),
they implicate that the position of the robot belongs to
both subspaces. Hence, the restricted subspace is a cir-
Figure 4. The Ωi subspaces-intersections con-
straining the position of the robot. a) Sphere. b)
Circle. c) Pair of points. d) Point.
cle, i.e. the intersection of spheres
Z(1∧2) = Ω1
〈
O
pf
i , O
m
j
〉
∧ Ω2
〈
O
pf
k
, Oml
〉
. (11)
Following the same pattern, a third sphere
Ω3
〈
O
pf
r , O
m
s
〉
enforces the restriction to a pair of
points, i.e. circle-sphere intersection, see Fig.4-c
J(1∧2∧3) = Z(1∧2) ∧ Ω3
〈
O
pf
r , O
m
s
〉
. (12)
Finally, a fourth sphere Ω4
〈
O
pf
t , O
m
h
〉
uniquely deter-
mines the position of the robot, i.e. the intersection of
the latter pair of points with Ω4
〈
O
pf
t , O
m
h
〉
, see Fig.4-d
P(1∧2∧3∧4) = J(1∧2∧3) ∧ Ω4
〈
O
pf
t , O
m
h
〉
. (13)
When more than one matched percept subgraph was
extracted, it implicates different plausible positions of
the robot. In order to generate and disambiguate those
location hypotheses the conformal geometric algebra
[6] is used by expressing spheres as computational
primitives as well as computing general intersections
among them. Fig.5 shows the latter concepts in an effi-
cient location hypotheses generation mechanism.
Figure 5. Location hypotheses generation.
Due to uncertainty in the fused percepts, the inter-
section between restriction spheres is likely to fall in
degenerated states (e.g. spheres may not meet, uncer-
tainty from distant percept could diminish the system
precision, etc.), which could compromise the quality
and existence of the pose-solution. In order to con-
template these facts with their side effects, a statistical
method (for a complete description including detailed
experimental results see our approach [5]) has been in-
troduced, which in a closed-form simultaneously en-
sures the solution existence (i.e. maximal density po-
sition) and improves the precision of the localization by
considering the uncertainties from Eq.7 and the map-
ping process in section 2.1.1.
2.4.1 Generation. Percept subgraphs are used to pro-
duce the zero-level set, composed of spheres
Φ0 := {Ωζ
〈
Omi , O
p
j
〉
}ζ=1,...,n. (14)
These spheres are intersected by means of the wedge
operator∧ in an upper triangular fashion producing the
first-level set Φ1 comprising circles. The second-level
set Φ2 is computed by intersecting those circles with
spheres from Φ0. The latter resulting pair of points are
intersected in the same way creating the third-level set
Φ3. Here, points from the intersection of four spheres
4
are contained. Elements of Φ2 without descendants in
Φ3 and all elements in Φ3 represent location hypotheses
Δ :=
∧
ξ
Ωξ
〈
Omi , O
p
j
〉
. (15)
The total computational complexity O(n4) is feasible
because in the practice n ≤ 4.
2.4.2 Validation. Hypotheses are checked by select-
ing associations
〈
O
pf
i , O
m
j
〉
which were not considered
when the current validating hypothesis was generated.
In case there is more than one prevailing hypothesis,
an active validation needs to take place by selecting
objects from the model and localizing them in the vi-
sual space. The criterion to select the discriminator per-
cept is the maximal pose difference between pairs of
hypotheses.
2.4.4 Pose Estimation. Once the location hypothesis
has revealed the position of the robotXego, the 6D pose
is expressed as
Sego = S
u,w,v
model
[Si,j,k
Percept
]−1. (16)
This is the transformation of the kinematic chain cou-
pling the world-model frame Smodel with the perception
frame SPercept, see Fig.6.
3. Experimental Results and Conclusions
The global self-localization of the humanoid robot
ARMAR-III [2] within the modeled environment was
successfully performed using this approach, see Fig.6.
The scanning strategy takes 15-20 seconds processing
20 real stereo images, graph model pruning takes 100-
150 ms. Finally, the hypotheses generation-validation
takes 200-500 ms.
The proposed approach solves the global localiza-
tion by using the conformal geometric framework and
an efficient graph representation of interrelated geomet-
ric object features. The resulting pose and those vision-
to-model subgraphs associations provide very substan-
tial information which is fundamental for autonomous
systems, where the visual coupling is needed for higher
planning, strategic or semantic abstraction levels.
4 Acknowledgment
The work described in this paper was conducted
within the GermanHumanoid Research project SFB588
funded by the German Research Foundation (DFG:
Deutsche Forschungsgemeinschaft).
Figure 6. ARMAR-III self-localization.
References
[1] T. Asfour, P. Azad, N. Vahrenkamp, K. Regenstein,
A. Bierbaum, K. Welke, J. Schröder, and R. Dillmann.
Toward humanoid manipulation in human-centred envi-
ronments. Robot. Auton. Syst., 56(1):54–65, 2008.
[2] T. Asfour, K. Regenstein, P. Azad, J. Schroder, A. Bier-
baum, N. Vahrenkamp, and R. Dillmann. Armar-III: An
integrated humanoid platform for sensory-motor con-
trol. Humanoid Robots, 2006 6th IEEE-RAS Interna-
tional Conference on, pages 169–175, Dec. 2006.
[3] D. Comaniciu and P. Meer. Mean shift: a robust ap-
proach toward feature space analysis. Pattern Anal-
ysis and Machine Intelligence, IEEE Transactions on,
24(5):603–619, May 2002.
[4] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse.
Monoslam: Real-time single camera slam. Pattern
Analysis and Machine Intelligence, IEEE Transactions
on, 29(6):1052–1067, June 2007.
[5] D. Gonzalez-Aguirre, T. Asfour, E. Bayro-Corrochano,
and R. Dillmann. Improving model-based visual self-
localization using gaussian spheres. To Appear, Inter-
national Conference AGACSE 2008.
[6] D. Gonzalez-Aguirre and E. Bayro-Corrochano. A ge-
ometric approach for an intuitive perception system of
humanoids. In IAS, pages 399–407, 2006.
[7] M. Pollefeys, L. V. Gool, M. Vergauwen, F. Verbiest,
K. Cornelis, J. Tops, and R. Koch. Visual modeling with
a hand-held camera. Int. J. Comput. Vision, 59(3):207–
232, 2004.
[8] S. Se, D. Lowe, and J. Little. Vision-based global loca-
lization and mapping for mobile robots. Robotics, IEEE
Transactions on, 21(3):364–375, June 2005.
[9] J. Shi and C. Tomasi. Good features to track. 1994
IEEE Conference on Computer Vision and Pattern Re-
cognition (CVPR’94), pages 593 – 600, 1994.
[10] S. Ullman. High-Level Vision. MIT press, Mas-
sachusetts, MA, 1996.
5


Model-based visual self-localization using geometry and graphs

http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=62CA69C66D2FAEDDB29A8365B7193C85?doi=10.1.1.1056.5492&rep=rep1&type=pdf

Model-based visual self-localization using geometry and graphs

Abstract

Similar works

Full text

Available Versions

CiteSeerX